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); }