private static void DoPWM()
{
var driverHandler = new ServoDriver();
driverHandler.Configure();
driverHandler.Reset();
Console.WriteLine($"Obtained hanlder: {driverHandler}");
Console.ReadKey();
string readedValue;
while (true)
{
Console.WriteLine("Give me value:");
readedValue = Console.ReadLine();
if (string.IsNullOrEmpty(readedValue))
{
break;
}
driverHandler.SetPWM(1, int.Parse(readedValue));
}
}
private static void DoPWM(IWrapI2C i2cWrapper, ILogger logger, ITimer timer, int address)
{
var driverHandler = new ServoDriver(i2cWrapper, address, logger, timer);
driverHandler.Configure();
driverHandler.Reset();
Console.WriteLine($"Obtained hanlder: {driverHandler}");
Console.ReadKey();
string readedValue;
while (true)
{
Console.WriteLine("Give me value:");
readedValue = Console.ReadLine();
if (string.IsNullOrEmpty(readedValue))
{
break;
}
driverHandler.SetPWM(1, int.Parse(readedValue));
}
}
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();
}
