private static void LoadLimits(DynamixelDriver driver)
{
Console.WriteLine("Loading motor limits");
List <MinMax> minMaxPairs = JsonConvert.DeserializeObject <List <MinMax> >(File.ReadAllText("MinMaxPairs.json"));
foreach (var minMaxPair in minMaxPairs)
{
ushort maxAngle = DynamixelDriver.DegreesToUnits(minMaxPair.Max);
ushort minAngle = DynamixelDriver.DegreesToUnits(minMaxPair.Min);
driver.SetCcwMaxAngleLimit((byte)minMaxPair.Index, maxAngle);
driver.SetCwMinAngleLimit((byte)minMaxPair.Index, minAngle);
}
Console.ForegroundColor = ConsoleColor.Blue;
Console.WriteLine(JsonConvert.SerializeObject(minMaxPairs));
Console.ResetColor();
Console.WriteLine("Limits loaded");
}
static void Main(string[] args)
{
Console.WriteLine("Starting");
using (var driver = new DynamixelDriver("COM4"))
using (var ikDriver = new QuadrupedIkDriver(driver))
using (var controller = new SmartQuadrupedController(ikDriver))
{
//controller.Speed = 30;
var moveDistance = 200;
//controller.Direction = Vector2.UnitX;
controller.Rotation = 10;
Thread.Sleep(2000);
controller.Direction = Vector2.Zero;
controller.Rotation = 0;
Console.WriteLine(controller.Odometry);
Console.WriteLine("Done!");
Console.ReadLine();
controller.Stop();
}
Console.WriteLine("Press enter to exit...");
Console.ReadLine();
}
public ushort ToDynamixelUnits()
{
return(DynamixelDriver.DegreesToUnits(new Angle(_value + 150f)));
}
public CameraController(DynamixelDriver driver)
{
_driver = driver;
CenterView();
}
public QuadrupedIkDriver(DynamixelDriver driver)
{
_driver = driver ?? throw new ArgumentNullException(nameof(driver));
}
static void Main(string[] args)
{
Directory.SetCurrentDirectory(Path.GetDirectoryName(Assembly.GetEntryAssembly().Location));
Console.WriteLine("Starting");
using (var driver = new DynamixelDriver(args.Length > 0 ? args[0] : "COM4"))
using (var quadruped = new QuadrupedIkDriver(driver))
{
LoadLimits(driver);
using (var gaiteEngine = new InterpolationGaitEngine(quadruped))
using (var controller = new InterpolationController(gaiteEngine))
{
bool keepGoing = true;
LegFlags nextLegCombo = LegFlags.RfLrCross;
while (keepGoing)
{
nextLegCombo = nextLegCombo == LegFlags.RfLrCross ? LegFlags.LfRrCross : LegFlags.RfLrCross;
Vector2 direction = Vector2.Zero;
ConsoleKeyInfo keyInfo = GetCurrentConsoleKey();
switch (keyInfo.Key)
{
case ConsoleKey.LeftArrow:
case ConsoleKey.A:
direction = new Vector2(-1, 0);
break;
case ConsoleKey.RightArrow:
case ConsoleKey.D:
direction = new Vector2(1, 0);
break;
case ConsoleKey.UpArrow:
case ConsoleKey.W:
direction = new Vector2(0, 1);
break;
case ConsoleKey.DownArrow:
case ConsoleKey.S:
direction = new Vector2(0, -1);
break;
case ConsoleKey.D3:
controller.EnqueueOneRotation(keyInfo.Modifiers == ConsoleModifiers.Shift ? 25 : 12.5f, nextLegCombo);
break;
case ConsoleKey.D2:
controller.EnqueueOneRotation(keyInfo.Modifiers == ConsoleModifiers.Shift ? -25 : -12.5f, nextLegCombo);
break;
case ConsoleKey.Q:
direction = keyInfo.Modifiers == ConsoleModifiers.Shift ? new Vector2(-1, -1) : new Vector2(-1, 1);
break;
case ConsoleKey.E:
direction = keyInfo.Modifiers == ConsoleModifiers.Shift ? new Vector2(1, -1) : new Vector2(1, 1);
break;
case ConsoleKey.Z:
direction = new Vector2(-1, -1);
break;
case ConsoleKey.C:
direction = new Vector2(1, -1);
break;
case ConsoleKey.Spacebar:
controller.EnqueueOneRotation(0);
break;
case ConsoleKey.Escape:
keepGoing = false;
break;
}
controller.EnqueueOneStep(direction, nextLegCombo);
gaiteEngine.WaitUntilCommandQueueIsEmpty();
}
gaiteEngine.Stop();
quadruped.DisableMotors();
}
}
Console.WriteLine("Done");
}
