public void MoveLegs(LegPositions position)
{
MoveLeftFrontLeg(position.LeftFront);
MoveRightFrontLeg(position.RightFront);
MoveLeftRearLeg(position.LeftRear);
MoveRightRearLeg(position.RightRear);
}
public SmartQuadrupedController(QuadrupedIkDriver driver)
{
_driver = driver ?? throw new ArgumentNullException(nameof(driver));
_driver.Setup();
_lastWrittenPosition = _driver.ReadCurrentLegPositions();
Task.Run(MainControllerLoop).ConfigureAwait(false);
}
public bool MoveFinished(LegPositions other)
{
return(LeftFront == other.LeftFront &&
RightFront == other.RightFront &&
LeftRear == other.LeftRear &&
RightRear == other.RightRear);
}
private async Task ExecuteMove(LegPositions targetPosition)
{
while (!_lastWrittenPosition.MoveFinished(targetPosition))
{
_lastWrittenPosition = _lastWrittenPosition.MoveTowards(targetPosition, Speed * 0.001f * _updateDelay);
_driver.MoveLegsSynced(_lastWrittenPosition);
await Task.Delay(_updateDelay);
}
}
public LegPositions MoveTowards(LegPositions target, float distance)
{
var newLeftFront = LeftFront.MoveTowards(target.LeftFront, distance);
var newRightFront = RightFront.MoveTowards(target.RightFront, distance);
var newLeftRear = LeftRear.MoveTowards(target.LeftRear, distance);
var newRightRear = RightRear.MoveTowards(target.RightRear, distance);
return(new LegPositions(newLeftFront, newRightFront, newLeftRear, newRightRear));
}
public InterpolationGaitEngine(QuadrupedIkDriver driver) : base(driver)
{
Driver.Setup();
_lastWrittenPosition = Driver.ReadCurrentLegPositions();
if (_moves.TryDequeue(out var deqeueuedLegPosition))
{
_nextMove = deqeueuedLegPosition;
}
StartEngine();
}
public void MoveLegsSynced(LegPositions position)
{
if (position == null)
{
throw new ArgumentNullException(nameof(position));
}
var rightFrontLegGoalPositions = CalculateIkForLeg(position.RightFront, RightFront);
var rightRearLegGoalPositions = CalculateIkForLeg(position.RightRear, RightRear);
var leftFrontLegGoalPositions = CalculateIkForLeg(position.LeftFront, LeftFront);
var leftRearLegGoalPositions = CalculateIkForLeg(position.LeftRear, LeftRear);
byte[] servoIds = new byte[12];
float[] servoGoals = new float[12];
// Pair motor ids with their respective goal positions
servoIds[0] = RightFront.CoxaId;
servoGoals[0] = rightFrontLegGoalPositions.Coxa;
servoIds[1] = RightFront.FemurId;
servoGoals[1] = rightFrontLegGoalPositions.Femur;
servoIds[2] = RightFront.TibiaId;
servoGoals[2] = rightFrontLegGoalPositions.Tibia;
servoIds[3] = RightRear.CoxaId;
servoGoals[3] = rightRearLegGoalPositions.Coxa;
servoIds[4] = RightRear.FemurId;
servoGoals[4] = rightRearLegGoalPositions.Femur;
servoIds[5] = RightRear.TibiaId;
servoGoals[5] = rightRearLegGoalPositions.Tibia;
servoIds[6] = LeftFront.CoxaId;
servoGoals[6] = leftFrontLegGoalPositions.Coxa;
servoIds[7] = LeftFront.FemurId;
servoGoals[7] = leftFrontLegGoalPositions.Femur;
servoIds[8] = LeftFront.TibiaId;
servoGoals[8] = leftFrontLegGoalPositions.Tibia;
servoIds[9] = LeftRear.CoxaId;
servoGoals[9] = leftRearLegGoalPositions.Coxa;
servoIds[10] = LeftRear.FemurId;
servoGoals[10] = leftRearLegGoalPositions.Femur;
servoIds[11] = LeftRear.TibiaId;
servoGoals[11] = leftRearLegGoalPositions.Tibia;
lock (_driver.SyncLock)
{
_driver.GroupSyncSetGoalPositionInDegrees(servoIds, servoGoals);
}
}
public FunctionalGaitEngine(QuadrupedIkDriver driver) : base(driver)
{
Driver.Setup();
var currentPosition = driver.ReadCurrentLegPositions();
float average = (currentPosition.LeftFront.Z +
currentPosition.RightFront.Z +
currentPosition.LeftRear.Z +
currentPosition.RightRear.Z) / 4;
if (average > -9)
{
Driver.MoveLegsSynced(RelaxedStance.Transform(new Vector3(0, 0, -LegHeight)));
Thread.Sleep(1000);
}
_lastWrittenPosition = RelaxedStance;
Driver.MoveLegsSynced(_lastWrittenPosition);
StartEngine();
}
protected override void EngineSpin()
{
// if last telemetrics was too long time ago fire
var currentTickCount = Environment.TickCount;
if (currentTickCount - _lastTelemetricsUpdate > TelemetricsUpdateInterval)
{
_lastTelemetricsUpdate = currentTickCount;
NewTelemetricsUpdate?.Invoke(this, Driver.ReadTelemetrics());
}
if (_lastWrittenPosition.MoveFinished(_nextMove ?? _lastWrittenPosition))
{
if (_moves.TryDequeue(out var deqeueuedLegPosition))
{
_moveQueueSingal.Reset();
_nextMove = deqeueuedLegPosition;
}
else
{
if (_moves.IsEmpty)
{
_moveQueueSingal.Set();
}
return;
}
}
try
{
_lastWrittenPosition = _lastWrittenPosition.MoveTowards(_nextMove, NextStepLength);
Driver.MoveLegsSynced(_lastWrittenPosition);
}
catch (IOException e)
{
GaitEngineError?.Invoke(this, e);
}
}
private LegFlags DetermineBestLeg(LegPositions currentPositions)
{
Vector2 rightFrontProfile = currentPositions.RightFront.ToDirectionVector2().Normal();
Vector2 rightRearProfile = currentPositions.RightRear.ToDirectionVector2().Normal();
Vector2 leftFrontProfile = currentPositions.LeftFront.ToDirectionVector2().Normal();
Vector2 leftRearProfile = currentPositions.LeftRear.ToDirectionVector2().Normal();
var relaxed = RelaxedStance;
double rightFrontValue = AreaOfTriangle(rightRearProfile, leftFrontProfile, relaxed.LeftRear.ToDirectionVector2().Normal());
double rightRearValue = AreaOfTriangle(rightFrontProfile, leftRearProfile, relaxed.LeftFront.ToDirectionVector2().Normal());
double leftFrontValue = AreaOfTriangle(rightFrontProfile, leftRearProfile, relaxed.RightRear.ToDirectionVector2().Normal());
double leftRearValue = AreaOfTriangle(rightRearProfile, leftFrontProfile, relaxed.RightFront.ToDirectionVector2().Normal());
double highest = new double[] { rightFrontValue, rightRearValue, leftFrontValue, leftRearValue }.Max();
if (highest == rightFrontValue)
{
return(LegFlags.RightFront);
}
else if (highest == rightRearValue)
{
return(LegFlags.RightRear);
}
else if (highest == leftFrontValue)
{
return(LegFlags.LeftFront);
}
else if (highest == leftRearValue)
{
return(LegFlags.LeftRear);
}
else
{
throw new InvalidOperationException();
}
}
public void AddStep(LegPositions nextStep)
{
_moves.Enqueue(nextStep);
_moveQueueSingal.Reset();
}
protected override void EngineSpin()
{
double angle = TimeSinceStart / 1000.0 * 280.0; // this determins the speed of switching legs
double legLiftSin = Math.Sin((angle * 2).DegreeToRad());
double swaySin = Math.Sin(angle.DegreeToRad());
var currentLeg = _legs[(int)(angle % 360 / 90)];
// shift leg offsets
// TODO refactor
if (currentLeg == LegFlags.RightFront)
{
_rightFrontOffset += 3 * NextStepLength;
}
else
{
_rightFrontOffset -= NextStepLength;
}
if (currentLeg == LegFlags.RightRear)
{
_rightRearOffset += 3 * NextStepLength;
}
else
{
_rightRearOffset -= NextStepLength;
}
if (currentLeg == LegFlags.LeftFront)
{
_leftFrontOffset += 3 * NextStepLength;
}
else
{
_leftFrontOffset -= NextStepLength;
}
if (currentLeg == LegFlags.LeftRear)
{
_leftRearOffset += 3 * NextStepLength;
}
else
{
_leftRearOffset -= NextStepLength;
}
_lastWrittenPosition = RelaxedStance
.Transform(new Vector3((float)(swaySin * 3) * _direction.Y, -(float)(swaySin * 3) * _direction.X, 0))
.Transform(new Vector3(_rightFrontOffset * _direction.X, _rightFrontOffset * _direction.Y, 0), LegFlags.RightFront)
.Transform(new Vector3(_rightRearOffset * _direction.X, _rightRearOffset * _direction.Y, 0), LegFlags.RightRear)
.Transform(new Vector3(_leftFrontOffset * _direction.X, _leftFrontOffset * _direction.Y, 0), LegFlags.LeftFront)
.Transform(new Vector3(_leftRearOffset * _direction.X, _leftRearOffset * _direction.Y, 0), LegFlags.LeftRear)
.Transform(new Vector3(0, 0, (float)Math.Abs(legLiftSin) * 3), currentLeg);
try
{
Driver.MoveLegsSynced(_lastWrittenPosition);
}
catch (IOException e)
{
Console.ForegroundColor = ConsoleColor.Red;
Console.WriteLine(e);
Console.ResetColor();
throw;
}
}
