public void DrawProjectilePath()
{
var p = new Projectile
{
Position = new Point(0, 1, 0),
Velocity = new Vector(1, 1.8, 0).Normalize() * 11.25
};
var gravity = new Vector(0, -0.1, 0);
var wind = new Vector(-0.01, 0, 0);
var w = new World {
Gravity = gravity, Wind = wind
};
var c = new Canvas(900, 550);
var tick = MotionHelper.Tick(w, p);
while (tick.Position.X < c.Width && tick.Position.Y < c.Height && tick.Position.Y > 0)
{
c.WritePixel((int)tick.Position.X, c.Height - (int)tick.Position.Y, new Color(1, 0, 0));
tick = MotionHelper.Tick(w, tick);
}
var ppm = c.ToPpm();
File.WriteAllText(@"d:\zetes\projectile.ppm", ppm);
}
public void Transform(PathTransform p)
{
for (int i = 0; i < points.Count; i++)
{
points[i] *= p.Scale;
points[i] += p.Translation;
points[i] = MotionHelper.Rotate(points[i], p.Rotation, p.RotationOrigin);
}
}
/// <summary>
/// Do work event handler.
/// Turns until the robot sees a mark.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void _findMarkWorker_DoWork(object sender, DoWorkEventArgs e)
{
// reset the actual rotation
var actualRotation = 0f;
//init robot
InitRobotPosition();
// create a LandMarkDetectionProxy to start looking for marks
using (var landMarkDetectionProxy = _connection.GetProxy <LandMarkDetectionProxy>())
{
landMarkDetectionProxy.subscribe(SUBSCRIBER_NAME);
// create a MemoryProxy to read landmark information
using (var memProxy = _connection.GetProxy <MemoryProxy>())
{
// use one motion proxy
using (var motionProxy = _connection.GetProxy <MotionProxy>())
{
do
{// turn the robot 360 degrees
// reset the yaw to 0
MotionHelper.MoveHead(motionProxy, null, 0f, true);
// initialize the look direction
var startLookDirection = this.StartLookDirection;
var currentLookDirection = this.StartLookDirection;
do
{// make the robot look left and right
// reset head pitch position to 0
MotionHelper.MoveHead(motionProxy, 0f, null, true);
do
{ // make the robot look down
var landMarks = this.TryGetLandMark(memProxy);
if (landMarks != null && landMarks.ContainsKey(MarkID))
{
// align the robot to the landmark
AlignWithMark(motionProxy, landMarks[MarkID]);
// reload the mark information from memory because the robot moved
landMarks = this.TryGetLandMark(memProxy);
// should not be empty but just make sure
if (landMarks != null && landMarks.ContainsKey(MarkID))
{
// save the result
e.Result = new KeyValuePair <int, ArrayList>(MarkID, landMarks[MarkID]);
}
}
}while (e.Result == null && !_findMarkWorker.CancellationPending && LookDown(motionProxy));
}while (e.Result == null && !_findMarkWorker.CancellationPending &&
LookAround(motionProxy, startLookDirection, ref currentLookDirection));
}while (e.Result == null && !_findMarkWorker.CancellationPending && Rotate(motionProxy, ref actualRotation));
}
}
landMarkDetectionProxy.unsubscribe(SUBSCRIBER_NAME);
}
}
private int LookForMarks(MotionProxy motionProxy)
{
List <string> names = new List <string>();
List <float[]> times = new List <float[]>();
List <float[]> keys = new List <float[]>();
var time = 2f;
MotionHelper.MoveHead(motionProxy, 0f, 0f, true);
/*
* names.Add("HeadYaw");
* times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f });
* keys.Add(new[] { 0.00149f, -0.27925f, -0.34826f, -0.47124f, -0.52360f, -0.28690f, -0.00940f, -0.00940f });
*/
/* // absolute
* names.Add("HeadPitch");
* times.Add(new[] { 2f, 4f, 6f, 9f, 15f, 18f, 21f, 22f, 28f, 29f, 35f, 38f});
* keys.Add(new[] { 0f, 0.5f, 0f, 0f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f});
*
* names.Add("HeadYaw");
* times.Add(new[] { 2f, 4f, 6f, 9f, 15f, 18f, 21f, 22f, 28f, 29f, 35f, 38f});
* keys.Add(new[] { 0f, 0f, 0f, -1f, 1f, 0f, -1f, -1f, 1f, 1f, -1f, 0f });
*
*
* var methodID = motionProxy.post.angleInterpolation(names, keys, times, true);
*/
// relative
names.Add("HeadPitch");
times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f });
keys.Add(new[] { 0.5f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f });
names.Add("HeadYaw");
times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f });
keys.Add(new[] { 0f, 0f, -1f, 1f, 1f, -1f, -1f, 1f, 0f });
var methodID = motionProxy.post.angleInterpolation(names, keys, times, false);
return(methodID);
}
/// <summary>
/// Aligns the robot with the landmark
/// </summary>
/// <param name="motionProxy"></param>
/// <param name="markInfo"></param>
private void AlignWithMark(MotionProxy motionProxy, ArrayList markInfo)
{
// get mark shape infor
var shapeInfo = LandMarkHelper.Instance.GetShapeInfo(markInfo);
// get current yaw position
var currentYawPosition = motionProxy.getAngles("HeadYaw", true);
if (!currentYawPosition.IsNullOrEmpty() && shapeInfo[1] != null)
{
// move pitch to landmark
MotionHelper.MoveHead(motionProxy, shapeInfo[2] as float?, null, false);
// calculate robot rotation
var rotation = currentYawPosition[0] + (float)shapeInfo[1];
motionProxy.moveTo(0f, 0f, rotation);
// reset yaw
MotionHelper.MoveHead(motionProxy, null, 0f, true);
}
}
public void ProjectileInWorld()
{
var p = new Projectile {
Position = new Point(0, 1, 0), Velocity = new Vector(1, 1, 0).Normalize()
};
var w = new World {
Gravity = new Vector(0, -0.1, 0), Wind = new Vector(-0.01, 0, 0)
};
var expected = new Projectile
{
Position = new Point(0.70710678118654746, 1.7071067811865475, 0),
Velocity = new Vector(0.69710678118654745, 0.60710678118654748, 0)
};
var actual = MotionHelper.Tick(w, p);
Assert.Equal(expected.Position, actual.Position);
Assert.Equal(expected.Velocity, actual.Velocity);
}
private void Lift()
{
List <string> names = new List <string>();
List <float[]> times = new List <float[]>();
List <float[]> keys = new List <float[]>();
var time = 1.5f;
var shoulderPitch = MotionHelper.DegreeToRadian(40f);
#region Positions
names.Add("LShoulderPitch");
times.Add(new[] { time });
keys.Add(new[] { shoulderPitch });
names.Add("RShoulderPitch");
times.Add(new[] { time });
keys.Add(new[] { shoulderPitch });
#endregion
AngleInterpolation(names, keys, times, true);
}
private void InitHands()
{
List <string> names = new List <string>();
List <float[]> times = new List <float[]>();
List <float[]> keys = new List <float[]>();
var time = 1.5f;
var elbowRoll = MotionHelper.DegreeToRadian(20f);
#region 1
names.Add("LElbowRoll");
times.Add(new[] { time });
keys.Add(new[] { elbowRoll });
names.Add("RElbowRoll");
times.Add(new[] { time });
keys.Add(new[] { -elbowRoll });
#endregion
AngleInterpolation(names, keys, times, true);
}
private bool LookDown(MotionProxy motionProxy)
{
// get initial head pitch position
var initialHeadPitchPosition = motionProxy.getAngles("HeadPitch", true);
MotionHelper.MoveHead(motionProxy, this.HeadPitchStepAmount, null, false);
// get current pitch position
var currentHeadPitchPosition = motionProxy.getAngles("HeadPitch", true);
// return true if the current position changed
if (!initialHeadPitchPosition.IsNullOrEmpty() && !currentHeadPitchPosition.IsNullOrEmpty() &&
initialHeadPitchPosition[0] != currentHeadPitchPosition[0])
{
return(true);
}
else
{
// reset pitch position
MotionHelper.MoveHead(motionProxy, 0f, null, true);
return(false);
}
}
/// <summary>
/// Makes the robot look left and right
/// </summary>
/// <param name="motionProxy"></param>
/// <returns></returns>
private bool LookAround(MotionProxy motionProxy, LookDirection startLookDirection, ref LookDirection currentLookDirection)
{
// get initial head yaw position
var initialHeadYawPositionArray = motionProxy.getAngles("HeadYaw", true);
if (initialHeadYawPositionArray.IsNullOrEmpty())
{
// should not happen
return(false);
}
var initialHeadYawPosition = initialHeadYawPositionArray[0];
float maxHeadYaw, step;
float marginOfError = 0.01f;
var maxReached = false;
var directionChanged = currentLookDirection != startLookDirection;
if (currentLookDirection == LookDirection.Left)
{
maxHeadYaw = this.MaxHeadYaw;
step = this.HeadYawStepAmount;
// check if it's the last movement and ajust the step if it is
if (step + initialHeadYawPosition >= maxHeadYaw)
{
step = maxHeadYaw - initialHeadYawPosition;
}
// if it reached the maximum value we need to change the direction and update the step
if (step - marginOfError <= 0)
{
maxReached = true;
step = -this.HeadYawStepAmount;
}
}
else
{
// if it's the right direction we need to have negative values for max and step angles
maxHeadYaw = -this.MaxHeadYaw;
step = -this.HeadYawStepAmount;
// check if it's the last movement and ajust the step if it is
if (step + initialHeadYawPosition <= maxHeadYaw)
{
step = maxHeadYaw - initialHeadYawPosition;
}
// if it reached the maximum value we need to change the direction and update the step
if (step + marginOfError >= 0)
{
maxReached = true;
step = this.HeadYawStepAmount;
}
}
if (maxReached && directionChanged)
{
// no more movement allowed
// reset head yaw position to 0
MotionHelper.MoveHead(motionProxy, null, 0f, true);
return(false);
}
else
{
MotionHelper.MoveHead(motionProxy, null, step, maxReached);
if (maxReached)
{
currentLookDirection = currentLookDirection == LookDirection.Left ? LookDirection.Right : LookDirection.Left;
}
return(true);
}
}
public void auto()
{
NowModel = ModelsHelper.NowModels;
string[,] array = new string[100, 20];
string file = Application.StartupPath + "\\data\\" + NowModel;
if (!File.Exists(file))
{
PushMesss("文件不存在File not Find");
return;
}
int Rows = 0;
// DataGridView DGV = new DataGridView();
ProfileHelper.ReadExcel1(file, ref array, ref Rows);
// array =ProfileHelper. GetTable(DGV);
// int a = DGV.Rows.Count;
bool first = true;
//自动时
if (ucSwitch7.Checked)
{
while (!ucSwitch2.Checked || first) //根据条件进行循环
{
for (int i = 0; i < Rows - 1; i++)
{
if (array[i, 1] == "Arc_Target")
{
continue;
}
var mess = Getmess(array, i);
PushMesss(mess);
Thread.Sleep(100);
Global.Cdefault data1 = new Global.Cdefault();
data1.setup = i;
data1.table = array;
data1.tryRun = !ucSwitch1.Checked;
MotionHelper.MotionFun(data1);
first = false;
// ActionThread.Abort();
}
button1.Image = global::LuoGuoFeng.Properties.Resources.暂停;
}
}
else //单步
{
//
Setup = Convert.ToInt32(numericUpDown1.Value);
PushMesss((Setup) + " line " + array[Setup, 1] + " " + array[Setup, 2] + " " + array[Setup, 3] + " " + array[Setup, 4]);
Global.Cdefault data1 = new Global.Cdefault();
data1.setup = Setup;
data1.table = array;
data1.tryRun = !ucSwitch1.Checked;
MotionHelper.MotionFun(data1);
if (Setup < Rows - 2)
{
Setup++;
}
else
{
Setup = 0;
}
ucTextBoxexMess(Setup);
button1.Image = global::LuoGuoFeng.Properties.Resources.暂停;
}
}