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); }