public override void Initialize()
{
_encoders.CollectAndResetDistanceFromEncoders();
_encoders.ResetTotalDistanceTraveled();
_wheels.SetSpeed(70, 70);
}
private void StartControlLogicTask()
{
_cancellationTokenSource = new CancellationTokenSource();
Task.Run(() =>
{
if (Error.Unacknowledged)
{
RunStudentLogic = false;
return;
}
try
{
Initialize();
while (RunStudentLogic && !_cancellationTokenSource.IsCancellationRequested)
{
Run(_cancellationTokenSource.Token);
}
_wheel.SetSpeed(0, 0);
}
catch (Exception e)
{
RunStudentLogic = false;
Error.Unacknowledged = true;
Error.Message = e.Message;
Error.DetailedMessage = e.ToString();
_wheel.SetSpeed(0, 0);
}
}, _cancellationTokenSource.Token);
}
public override void Initialize()
{
_encoders.CollectAndResetDistanceFromEncoders();
_encoders.ResetTotalDistanceTraveled();
_encoders.CollectContinously = true;
_encoders.CollectionInterval = 200;
distanceTraveled = 0;
forwad = true;
_wheel.SetSpeed(80, 80);
}
public override void Run(CancellationToken cancellationToken)
{
ThrowExceptionIfLidarCollectorIsStoppedOrSensorError();
float angleToLargestDistance = _lidar.LargestDistanceInRange(260, 100).Angle;
if (float.IsNaN(angleToLargestDistance))
{
_wheels.Stop();
Debug.WriteLine("STOPPED due to no LIDAR distance found in range!", "ControlLogic");
Thread.Sleep(200);
}
else
{
_speedLeft = BaseSpeed;
_speedRight = BaseSpeed;
CompensateSpeedTowardsAngle(angleToLargestDistance);
CompensateSpeedFromSideClearance();
if (_ultrasonic.Fwd < clearanceLimitFwd)
{
Debug.WriteLine("Emergency steer Fwd started.", "ControlLogic");
EmergencySteerFromObstacleInFront(1.3f, 50, cancellationToken);
Debug.WriteLine("Emergency steer Fwd completed.", "ControlLogic");
}
else
{
_wheels.SetSpeed(_speedLeft, _speedRight);
}
}
Thread.Sleep(50);
}
public void KinematicsLoop(double headingDesired, double headingActual, double speedDesired, double speedActual)
{
// Set the heading & speed PID controllers
headingPid.SetPoint = headingDesired;
headingPid.ProcessVariable = headingActual;
speedPid.SetPoint = speedDesired;
speedPid.ProcessVariable = speedActual;
// Delta time
var now = DateTime.Now;
var dt = now - last;
last = now;
// Run the controller
var heading = headingPid.ControlVariable(dt);
var speed = headingPid.ControlVariable(dt);
// calculate the change in angle from last
var dAlpha = kAlpha * (headingLast - heading);
headingLast = heading;
// Electronically "Mix" the speed and angle angle change to set the left/right motor speeds
left.SetSpeed((int)(speed + dAlpha));
right.SetSpeed((int)(speed - dAlpha));
}
private void SteerTowardsAngle(float angleDeviation, int baseSpeed)
{
int leftSpeedReduction = angleDeviation > 180 ? 360 - (int)angleDeviation : 0;
int rightSpeedReduction = angleDeviation < 180 ? (int)angleDeviation : 0;
_wheels.SetSpeed(baseSpeed - leftSpeedReduction, baseSpeed - rightSpeedReduction);
}
public void Forward()
{
bool hit = false;
left.SetSpeed(200);
right.SetSpeed(200);
position.LoopReadPosition(pos =>
{
Xunit.Assert.True(pos.position.X > 0.0d);
Xunit.Assert.Equal(0.0d, pos.position.Y);
hit = true;
});
System.Threading.Thread.Sleep(1500);
Xunit.Assert.True(hit);
}
public void Wheel_SetSpeed_OverspeedReverse()
{
left = new Wheel(PinPwm, PinFwd, PinRev);
left.SetSpeed(-150);
Assert.False(left.Forward);
Assert.True(left.Reverse);
Assert.Equal(-100, left.Speed);
}
private string HandleWheelCommand(Dictionary <string, string> request)
{
if (!request.ContainsKey(Key.Left) || !request.ContainsKey(Key.Right))
{
throw new Exception("Commands to the Wheel components requires the following two keys:\n" +
$"{Key.Left}, {Key.Right}");
}
if (_exampleLogic.ActiveExampleLogic.RunExampleLogic || _studentLogicService.ActiveStudentLogic.RunStudentLogic)
{
return("Can't give command while control logic is running. Stop control logic first.");
}
int requestedLeftSpeed = Int32.Parse(request[Key.Left]);
int requestedRightSpeed = Int32.Parse(request[Key.Right]);
_wheel.SetSpeed(requestedLeftSpeed, requestedRightSpeed);
return($"Left: {_wheel.CurrentSpeedLeft}, Right: {_wheel.CurrentSpeedRight}.");
}
public void Wheel_SetSpeed_Stop()
{
yomo.Utility.Settings.Testing = true;
left = new Wheel(PinPwm, PinFwd, PinRev);
left.SetSpeed(0);
Assert.False(left.Forward);
Assert.False(left.Reverse);
}
public void Wheel_SetSpeed_Overspeed()
{
yomo.Utility.Settings.Testing = true;
left = new Wheel(PinPwm, PinFwd, PinRev);
left.SetSpeed(150);
Assert.True(left.Forward);
Assert.False(left.Reverse);
Assert.Equal(100, left.Speed);
}
public void Wheel_SetSpeed_CalculatedDutyCycle100()
{
yomo.Utility.Settings.Testing = true;
left = new Wheel(PinPwm, PinFwd, PinRev);
left.SetSpeed(100);
Assert.True(left.Forward);
Assert.False(left.Reverse);
Assert.Equal(100, left.Speed);
Assert.Equal((1024), left.DutyCycle);
}
// reduction rate should be a decimal number (1 is full speed)
private void KeepCenterCorridor(float argumentSpeedScaler)
{
float distanceLeft = _ultrasonic.Left;
float distanceRight = _ultrasonic.Right;
float relativeSpeedScaler = 100 / Math.Max(distanceLeft, distanceRight);
int leftSpeed = (int)(distanceRight * relativeSpeedScaler * argumentSpeedScaler);
int rightSpeed = (int)(distanceLeft * relativeSpeedScaler * argumentSpeedScaler);
_wheels.SetSpeed(leftSpeed, rightSpeed);
Thread.Sleep(10);
}
public override void Run(CancellationToken cancellationToken)
{
for (int i = 0; i < 100 && !cancellationToken.IsCancellationRequested; i++)
{
_wheels.SetSpeed(speedLeft++, speedRight);
Thread.Sleep(10);
}
for (int i = 0; i < 100 && !cancellationToken.IsCancellationRequested; i++)
{
_wheels.SetSpeed(speedLeft--, speedRight);
Thread.Sleep(10);
}
for (int i = 0; i < 100 && !cancellationToken.IsCancellationRequested; i++)
{
_wheels.SetSpeed(speedLeft, speedRight--);
Thread.Sleep(10);
}
for (int i = 0; i < 100 && !cancellationToken.IsCancellationRequested; i++)
{
_wheels.SetSpeed(speedLeft, speedRight++);
Thread.Sleep(10);
}
}
// SOME OF THE COMMANDS THAT CAN BE SENT TO SOCKET SERVER:
// Set wheel speed: { "REQUEST_TYPE": "Command", "COMPONENT": "Wheel", "LEFT": "0", "RIGHT": "0" }
// Request Ultrasound distance: { "REQUEST_TYPE": "Data", "COMPONENT": "Ultrasound" }
// Request Wheel and Ultrasound: { "REQUEST_TYPE": "Data", "COMPONENT": "Wheel Ultrasound" }
private async void StreamSocketListener_ConnectionReceived(StreamSocketListener sender, StreamSocketListenerConnectionReceivedEventArgs args)
{
// TODO: Prevent multiple people connecting to the vehicle (for control) at the same time
try
{
Debug.WriteLine($"server received a connection from {args.Socket.Information.RemoteServiceName}", "TcpSocketServer");
NumberOfClientsConnected += 1;
using (var streamReader = new StreamReader(args.Socket.InputStream.AsStreamForRead()))
using (Stream outputStream = args.Socket.OutputStream.AsStreamForWrite())
using (var streamWriter = new StreamWriter(outputStream))
{
string request;
while ((request = await streamReader.ReadLineAsync()) != null)
{
Debug.WriteLine($"server received the request: \"{request}\"", "TcpSocketServer");
Dictionary <string, string> responseKeyValuePair;
try
{
Dictionary <string, string> requestKeyValuePair = JsonConvert.DeserializeObject <Dictionary <string, string> >(request);
responseKeyValuePair = _requestHandler.HandleRequest(requestKeyValuePair);
}
catch (JsonReaderException jre)
{
Error.Message = GetJsonReaderErrorMessage(request);
Error.DetailedMessage = jre.ToString();
Error.Unacknowledged = true;
responseKeyValuePair = new Dictionary <string, string> {
{ Key.Error, Error.Message }
};
}
string response = JsonConvert.SerializeObject(responseKeyValuePair);
await streamWriter.WriteLineAsync(response);
await streamWriter.FlushAsync();
Debug.WriteLine($"server sent back the response: \"{response}\"", "TcpSocketServer");
}
Debug.WriteLine("client exited...", "TcpSocketServer");
}
}
catch (IOException ioe)
{
Error.Message = $"{ioe.Message}\n\n" +
$"Note: Client connection closed, but Socket Server is still open for new connections.\n" +
$"Reconnect and try again";
Error.DetailedMessage = ioe.ToString();
Error.Unacknowledged = true;
}
catch (COMException comE)
{
Error.Message = $"{comE.Message}\n\n" +
$"Note: Client connection closed, but Socket Server is still open for new connections.\n" +
$"Reconnect and try again";
Error.DetailedMessage = comE.ToString();
Error.Unacknowledged = true;
}
catch (Exception e)
{
ListenForConnections = false;
Error.Message = $"{e.Message}\n\n" +
$"Note: Async Server has been shut down";
Error.DetailedMessage = e.ToString();
Error.Unacknowledged = true;
}
finally
{
NumberOfClientsConnected -= 1;
_wheel.SetSpeed(0, 0, false);
}
}
private static void Go(IWheel lf, IWheel rf, int l, int r, int duration)
{
lf.SetSpeed(l);
rf.SetSpeed(r);
System.Threading.Thread.Sleep(duration);
}
