public void AddKeyFrame(ArmCursor cursor)
{
if (sel < 0)
{
return;
}
animations.ElementAt(sel).Add(new KeyFrame(cursor, 0));
Console.WriteLine(sel);
}
public Angle[] Reset(ArmCursor orig)
{
Angle[] a = poseCalculator.PoseToAngles(orig.Pos, orig.Dir);
for (int i = 0; i < a.Length; i++)
{
servos[i].Set(a[i]);
}
return(a);
}
public Angle[] SetTarget(ArmCursor t)
{
//ArmCursor ac = poseCalculator.AnglesToPose(Array.ConvertAll(servos, (Servo s)=> { return s.GetAngle(); }));
//Console.WriteLine("current cursor: " + ac);
Angle[] angles = poseCalculator.PoseToAngles(t.Pos, t.Dir);
//int i = 0;
//foreach (Angle a in angles)
//{
// Console.Write(a + " ");
// if (i == 2) Console.Write("|| ");
// i++;
//}
//Console.WriteLine();
setTarget(angles);
return(angles);
//ArmCursor ac = poseCalculator.AnglesToPose(angles);
//Console.WriteLine("current cursor: " + ac);
}
public override bool getNextTarget(
ref ArmCursor offset, ref Collection <Key> keysDown, ref Collection <Key> keysToggle, float stepAmount, out ComData msg)
{
stepAmount *= CRot;
if (keysDown.Contains(Key.LeftShift) || keysDown.Contains(Key.RightShift))
{
stepAmount *= -1;
}
if (keysDown.Contains(Key.LeftCtrl) || keysDown.Contains(Key.RightCtrl))
{
stepAmount *= 2;
}
//Angle[] dAngles = new Angle[toCheck.Length];
bool changed = false;
for (int i = 0; i < toCheck.Length; i++)
{
Key k = toCheck[i];
if (keysDown.Contains(k))
{
changed = true;
offsets[i + 1] = (float)new Angle(offsets[i + 1] + stepAmount);
}
}
offsets[0] = Time.Now();
msg = null;
if (changed)
{
msg = new ComData(new JSONableArray <float>(offsets, float.Parse));
if (VERBOSE)
{
Console.WriteLine("raw: {1}[\n{0}\n]", string.Join("\n", offsets), changed);
}
}
return(changed);
}
public override bool getNextTarget(
ref ArmCursor offset, ref Collection <Key> keysDown, ref Collection <Key> keysToggle, float stepAmount, out ComData msg)
{
Vector3 dPos = Vector3.Zero;
Vector3 dDir = Vector3.Zero; // just for storing dRotation angles
if (keysDown.Contains(Key.A))
{
dPos.X += -1;
}
if (keysDown.Contains(Key.D))
{
dPos.X += 1;
}
if (keysDown.Contains(Key.S))
{
dPos.Y += -1;
}
if (keysDown.Contains(Key.W))
{
dPos.Y += 1;
}
if (keysDown.Contains(Key.C))
{
dPos.Z += -1;
}
if (keysDown.Contains(Key.Z))
{
dPos.Z += 1;
}
// acting as temp rotation vector
if (keysDown.Contains(Key.J))
{
dDir.Y += -1;
}
if (keysDown.Contains(Key.L))
{
dDir.Y += 1;
}
if (keysDown.Contains(Key.I))
{
dDir.X += -1;
}
if (keysDown.Contains(Key.K))
{
dDir.X += 1;
}
if (keysDown.Contains(Key.M))
{
dDir.Z += -1;
}
if (keysDown.Contains(Key.OemPeriod))
{
dDir.Z += 1;
}
if (dPos == Vector3.Zero && dDir == Vector3.Zero)
{
msg = null;
return(false);
}
dPos = dPos.LengthSquared() != 0 ? Vector3.Normalize(dPos) * stepAmount * CTrans : dPos;
dDir = dDir.LengthSquared() != 0 ? Vector3.Normalize(dDir) * stepAmount * CRot : dDir;
Quaternion q = Quaternion.CreateFromYawPitchRoll(dDir.Y, dDir.X, dDir.Z);
q *= offset.Dir;
offset.Set(offset.Pos + dPos, q);
msg = new ComData(new KeyFrame(offset, Time.Now()));
if (VERBOSE)
{
Console.WriteLine("Cursor: \n{0}\n", offset.ToString());
}
return(true);
}
public abstract bool getNextTarget(
ref ArmCursor offset, ref Collection <Key> keysDown, ref Collection <Key> keysToggle, float stepAmount, out ComData msg);
public static void Run()
{
//Quaternion qa = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Math.PI / 1.3f);
//Quaternion qb = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Math.PI / -2.4f);
//Quaternion q1 = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Math.PI / 2);
//Quaternion q2 = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Math.PI / 2);
//Quaternion qdiff = Quaternion.Inverse(q1) * q2;
//Console.WriteLine("q1: " + Vector3.Transform(Vector3.UnitZ, q1));
//Console.WriteLine("-q1: " + Vector3.Transform(Vector3.UnitZ, Quaternion.Inverse(q1)));
//Console.WriteLine("q2: " + Vector3.Transform(Vector3.UnitZ, q2));
//Console.WriteLine(" qd: "+Vector3.Transform(-Vector3.UnitY, qdiff));
//Angle a1 = (float)(2f * Math.Acos(qdiff.W));
//Console.WriteLine(a1);
//Angle a = 0;
//Angle b = 1.5;
//Angle i = 0.7;
//Angle a2 = 3;
//Angle b2 = -3;
//Angle j = 3.1;
//Console.WriteLine(a.Intercepts(b, i));
//Console.WriteLine(a2.Intercepts(b2, j));
ServoDriver sd = new ServoDriver();
ArmCursor cursor = new ArmCursor();
cursor.Dir = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Math.PI / 2f);
cursor.Pos.X = 6f + 23;
cursor.Pos.Z = -5.5f + 30.5f;
ArmCursor orig = cursor;
ArmCursor rest = new ArmCursor(new Vector3(16, 0, 8), cursor.Dir);
Angle[] angles = sd.GetAngles();
sd.Run();
while (true)
{
//Console.Write("$");
string s = Console.ReadLine();
if (s.Length == 0)
{
continue;
}
if (s.Contains("x"))
{
break;
}
if (s.Contains("rr"))
{
angles = sd.Reset(orig);
cursor = orig;
Console.WriteLine("synced angles");
continue;
}
if (s.Contains("cal"))
{
cursor = orig;
angles = sd.SetTarget(orig);
continue;
}
if (s.Contains("rest"))
{
cursor = rest;
angles = sd.SetTarget(rest);
continue;
}
if (s.Contains("p"))
{
sd.SetTarget(angles);
continue;
}
if (s.Contains(" "))
{
string[] ss = s.Split(' ');
int ax = int.Parse(ss[0].Trim());
float deg = float.Parse(ss[1].Trim());
angles[ax - 1] = deg * (float)Math.PI / 180f;
Console.WriteLine("axis " + ax + " to " + angles[ax - 1]);
cursor = sd.SetTarget(angles);
}
else
{
s = s.ToUpper();
foreach (char ch in s)
{
t(ch, ref cursor);
}
Console.WriteLine("moving cursor to " + cursor.ToString());
angles = sd.SetTarget(cursor);
}
}
sd.Close();
}
private static bool t(char key, ref ArmCursor target)
{
const float CTrans = 1;
const float CRot = (float)Math.PI / 12;
Vector3 dPos = Vector3.Zero;
Vector3 dDir = Vector3.Zero; // just for storing dRotation angles
if (key == 'A')
{
dPos.X += -1;
}
if (key == 'D')
{
dPos.X += 1;
}
if (key == 'S')
{
dPos.Y += -1;
}
if (key == 'W')
{
dPos.Y += 1;
}
if (key == 'C')
{
dPos.Z += -1;
}
if (key == 'Z')
{
dPos.Z += 1;
}
// acting as temp rotation vector
if (key == 'J')
{
dDir.Y += -1;
}
if (key == 'L')
{
dDir.Y += 1;
}
if (key == 'I')
{
dDir.X += -1;
}
if (key == 'K')
{
dDir.X += 1;
}
if (key == 'M')
{
dDir.Z += -1;
}
if (key == 'N')
{
dDir.Z += 1;
}
if (dPos == Vector3.Zero && dDir == Vector3.Zero)
{
return(false);
}
dPos = dPos.LengthSquared() != 0 ? Vector3.Normalize(dPos) * CTrans : dPos;
dDir = dDir.LengthSquared() != 0 ? Vector3.Normalize(dDir) * CRot : dDir;
Quaternion q = Quaternion.CreateFromYawPitchRoll(dDir.Y, dDir.X, dDir.Z);
q *= target.Dir;
target.Set(target.Pos + dPos, q);
return(true);
}
