Skip to content
This repository was archived by the owner on Feb 22, 2023. It is now read-only.

Commit 5ffdf3a

Browse files
dmweisdmweis
committed
Update smart controller for deployment
1 parent 328cb80 commit 5ffdf3a

File tree

5 files changed

+90
-31
lines changed

5 files changed

+90
-31
lines changed

‎Quadruped.TestConsole/Program.cs

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
using System;
2+
using System.Numerics;
3+
using System.Threading;
24
using Quadruped.Driver;
35

46
namespace Quadruped.TestConsole
@@ -10,11 +12,20 @@ static void Main(string[] args)
1012
Console.WriteLine("Starting");
1113
using (var driver = new DynamixelDriver("COM4"))
1214
using (var ikDriver = new QuadrupedIkDriver(driver))
13-
using (var controller = new SmartQuadrupedController(gaiteEngine))
15+
using (var controller = new SmartQuadrupedController(ikDriver))
1416
{
15-
controller.
17+
//controller.Speed = 30;
18+
var moveDistance = 200;
19+
//controller.Direction = Vector2.UnitX;
20+
controller.Rotation = 10;
21+
Thread.Sleep(2000);
22+
controller.Direction = Vector2.Zero;
23+
controller.Rotation = 0;
24+
25+
Console.WriteLine(controller.Odometry);
1626
Console.WriteLine("Done!");
1727
Console.ReadLine();
28+
controller.Stop();
1829
}
1930
Console.WriteLine("Press enter to exit...");
2031
Console.ReadLine();

‎Quadruped.WebInterface/RTC/RobotMessage.cs

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -32,13 +32,14 @@ public float GetScaledX(float deadzone, float min, float max)
3232
return x;
3333
}
3434

35-
public Vector2 CalculateHeadingVector(float deadzone = 0.0f)
35+
public Vector2 CalculateHeadingVector(float deadzone = 0.0f, float maxSize = 6f)
3636
{
3737
if (MessageType != MessageType.Movement)
3838
{
3939
return Vector2.Zero;
4040
}
41-
if (Math.Abs(Distance / 50) < deadzone)
41+
var size = Math.Abs(Distance / 50);
42+
if (size < deadzone)
4243
{
4344
return Vector2.Zero;
4445
}
@@ -49,7 +50,8 @@ public Vector2 CalculateHeadingVector(float deadzone = 0.0f)
4950
X = x,
5051
Y = y
5152
};
52-
return vector;
53+
var resultVector = vector.Normal() * (size * maxSize);
54+
return resultVector;
5355
}
5456

5557
public Vector2 CalculatePinPosition(float deadzone = 0.0f)

‎Quadruped.WebInterface/RobotController/SmartRobot.cs

Lines changed: 21 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,12 +2,15 @@
22
using System.Numerics;
33
using System.Threading.Tasks;
44
using Microsoft.AspNetCore.Hosting;
5+
using Microsoft.Extensions.Logging;
56

67
namespace Quadruped.WebInterface.RobotController
78
{
89
public class SmartRobot : IRobot
910
{
1011
private readonly SmartQuadrupedController _controller;
12+
private readonly ILogger<Robot> _logger;
13+
1114
public event EventHandler<QuadrupedTelemetrics> NewTelemetricsUpdate;
1215

1316
public Vector2 Direction
@@ -16,25 +19,38 @@ public Vector2 Direction
1619
set => _controller.Direction = value;
1720
}
1821

19-
public float Rotation { get; set; }
22+
public float Rotation
23+
{
24+
get => _controller.Rotation;
25+
set => _controller.Rotation = value;
26+
}
27+
28+
public Vector3 RelaxedStance { get; private set; } = Vector3.Zero;
29+
public Rotation RelaxedStanceRotation { get; private set; } = new Rotation();
30+
2031
public RobotConfig GaitConfiguration { get; set; }
32+
33+
2134
public void StartRobot()
2235
{
2336
throw new NotImplementedException();
2437
}
2538

26-
public Vector3 RelaxedStance { get; }
27-
public Rotation RelaxedStanceRotation { get; }
28-
2939
public SmartRobot(SmartQuadrupedController controller, IApplicationLifetime applicationLifetime)
3040
{
3141
_controller = controller;
42+
_controller.NewTelemetricsUpdate += (sender, telemetrics) => NewTelemetricsUpdate?.Invoke(sender, telemetrics);
3243
applicationLifetime.ApplicationStopping.Register(() => _controller.Stop());
3344
}
3445

3546
public void UpdateAboluteRelaxedStance(Vector3 transform, Rotation rotation)
3647
{
37-
throw new NotImplementedException();
48+
RelaxedStance = transform;
49+
RelaxedStanceRotation = rotation;
50+
_controller.RelaxedStance = SmartQuadrupedController
51+
.OriginalRelaxedStance
52+
.Transform(transform)
53+
.Rotate(rotation);
3854
}
3955

4056
public Task DisableMotors()

‎Quadruped.WebInterface/Startup.cs

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,10 @@ public void ConfigureServices(IServiceCollection services)
4545
services.AddSingleton(new DynamixelDriver(portName));
4646
services.AddSingleton<QuadrupedIkDriver>();
4747
services.AddSingleton<InterpolationGaitEngine>();
48-
services.AddSingleton<InterpolationController>();
49-
services.AddSingleton<IRobot, Robot>();
48+
//services.AddSingleton<InterpolationController>();
49+
//services.AddSingleton<AdvanceInterpolationController>();
50+
services.AddSingleton<SmartQuadrupedController>();
51+
services.AddSingleton<IRobot, SmartRobot>();
5052
if (Configuration.GetSection("streamerConfig").GetValue<bool>("ControllerOn"))
5153
{
5254
services.AddSingleton<ICameraController, CameraController>();

‎Quadruped/SmartQuadrupedController.cs

Lines changed: 47 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -17,40 +17,51 @@ public class SmartQuadrupedController : IDisposable
1717
private const float LegHeight = -9f;
1818
private const int LegDistanceLongitudinal = 15;
1919
private const int LegDistanceLateral = 15;
20+
private const int TelemetricsUpdateInterval = 2000;
2021

21-
public LegPositions OriginalRelaxedStance => new LegPositions
22+
public event EventHandler<QuadrupedTelemetrics> NewTelemetricsUpdate;
23+
24+
public static LegPositions OriginalRelaxedStance => new LegPositions
2225
(
2326
new Vector3(LegDistanceLateral, LegDistanceLongitudinal, LegHeight),
2427
new Vector3(LegDistanceLateral, -LegDistanceLongitudinal, LegHeight),
2528
new Vector3(-LegDistanceLateral, LegDistanceLongitudinal, LegHeight),
2629
new Vector3(-LegDistanceLateral, -LegDistanceLongitudinal, LegHeight)
2730
);
2831

29-
public LegPositions RelaxedStance { get; set; }
32+
public LegPositions RelaxedStance { get; set; } = OriginalRelaxedStance;
33+
3034
public int Speed { get; set; } = 60;
3135
public Vector2 Direction { get; set; }
36+
public float Rotation { get; set; }
3237

33-
private LegPositions _lastWrittenPosition;
3438
private Vector2 _odometry = Vector2.Zero;
35-
private LegFlags _lastUsedCombo = LegFlags.RfLrCross;
39+
public Vector2 Odometry => _odometry;
40+
private float _rotationalOdometry = 0f;
41+
public float RotationalOdometry => _rotationalOdometry;
42+
public Angle Heading => new Angle(_rotationalOdometry);
43+
44+
private LegPositions _lastWrittenPosition;
45+
private LegFlags _lastUsedCombo = LegFlags.LfRrCross;
3646
private bool _keepRunning = true;
3747
private bool _moving;
3848

3949
public SmartQuadrupedController(QuadrupedIkDriver driver)
4050
{
4151
_driver = driver ?? throw new ArgumentNullException(nameof(driver));
42-
RelaxedStance = OriginalRelaxedStance;
4352
_driver.Setup();
4453
_lastWrittenPosition = _driver.ReadCurrentLegPositions();
45-
Task.Run(MainControllerLoop);
54+
Task.Run(MainControllerLoop).ConfigureAwait(false);
4655
}
4756

4857
private async Task MainControllerLoop()
4958
{
5059
await ExecuteMove(RelaxedStance);
60+
var watch = new Stopwatch();
61+
watch.Start();
5162
while (_keepRunning)
5263
{
53-
if (Direction.IsZero())
64+
if (Direction.IsZero() && Rotation == 0f)
5465
{
5566
if (_moving)
5667
{
@@ -59,19 +70,35 @@ private async Task MainControllerLoop()
5970
}
6071
else
6172
{
62-
await Task.Delay(_updateDelay);
73+
if (!_lastWrittenPosition.MoveFinished(RelaxedStance))
74+
{
75+
await ShiftToRelaxed();
76+
}
77+
else
78+
{
79+
await Task.Delay(_updateDelay);
80+
}
6381
}
6482
}
6583
else
6684
{
67-
await ExecuteStep(Direction, GetNextLegCombo());
85+
await ExecuteStep(Direction, Rotation, GetNextLegCombo());
6886
_moving = true;
6987
}
70-
Console.WriteLine(_odometry);
88+
if (watch.ElapsedMilliseconds > TelemetricsUpdateInterval)
89+
{
90+
watch.Restart();
91+
NewTelemetricsUpdate?.Invoke(this, _driver.ReadTelemetrics());
92+
}
7193
}
7294
_driver.DisableMotors();
7395
}
7496

97+
private async Task ShiftToRelaxed()
98+
{
99+
await ExecuteMove(RelaxedStance);
100+
}
101+
75102
private async Task StepToRelaxed(LegFlags legsToAlign, float liftHeight = 2)
76103
{
77104
if (legsToAlign != LegFlags.LfRrCross && legsToAlign != LegFlags.RfLrCross)
@@ -83,13 +110,10 @@ await ExecuteMove(RelaxedStance
83110
await ExecuteMove(RelaxedStance);
84111
}
85112

86-
private async Task ExecuteStep(Vector2 direction, LegFlags forwardMovingLegs, float distance = 6f, float liftHeight = 2)
113+
private async Task ExecuteStep(Vector2 direction, float angle, LegFlags forwardMovingLegs, float distance = 1f, float liftHeight = 2)
87114
{
88-
// identity comparasion to prevent error on float.NaN
89-
if (!direction.IsZero())
90-
{
91-
direction = direction.Normal();
92-
}
115+
_rotationalOdometry += angle;
116+
angle = angle / 4;
93117
if (forwardMovingLegs != LegFlags.LfRrCross && forwardMovingLegs != LegFlags.RfLrCross)
94118
{
95119
throw new ArgumentException($"{nameof(forwardMovingLegs)} has to be {nameof(LegFlags.RfLrCross)} or {nameof(LegFlags.LfRrCross)}");
@@ -100,20 +124,24 @@ private async Task ExecuteStep(Vector2 direction, LegFlags forwardMovingLegs, fl
100124
// Lift
101125
var nextStep = RelaxedStance
102126
.Transform(new Vector3(legShift * direction.X, legShift * direction.Y, liftHeight), forwardMovingLegs)
103-
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs);
127+
.Rotate(new Angle(-angle), forwardMovingLegs)
128+
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs)
129+
.Rotate(new Angle(angle), backwardsMovingLegs);
104130
await ExecuteMove(nextStep);
105131

106132
// Lower
107133
nextStep = nextStep
108134
.Transform(new Vector3(legShift * direction.X, legShift * direction.Y, -liftHeight), forwardMovingLegs)
109-
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs);
135+
.Rotate(new Angle(-angle), forwardMovingLegs)
136+
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs)
137+
.Rotate(new Angle(angle), backwardsMovingLegs);
110138
await ExecuteMove(nextStep);
111139

112140
// Move all
113141
nextStep = nextStep.Transform(new Vector3(-legShift * direction.X * 2, -legShift * direction.Y * 2, 0));
114142
await ExecuteMove(nextStep);
115143

116-
_odometry += direction * distance;
144+
_odometry += Vector2.Transform(direction * distance, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, _rotationalOdometry.DegreeToRad()));
117145
}
118146

119147
private async Task ExecuteMove(LegPositions targetPosition)

0 commit comments

Comments
 (0)