Download the source code here.
You can compile the code I propose in this page using Xamarin Studio 5.10.1 (Build 6), with Mono 4.2.1 as active runtime and the Monobrick Firmware Library 1.2.0.39486 to let it work on board the Lego Mindstorms Ev3 with the Monobrick firmware.
The source code above is happily shared under the Code Project Open Licence (CPOL) 1.02.
← Part 2 – The control strategy
The (simple) code architecture
The code running on board the Sup3r Car in the track following program, is based on a single class indeed called the SuperCar where i just dropped the “3” in Sup3er. The SuperCar class starts the following concurrent threads:
- the Sensors Update Thread whose responsibility is to update the readings from the sensors;
- the LCD Thread which updates the Ev3 brick LCD with debug information;
- the Drive Thread that implements the
external Drive loop controller described in Part 2;
- the Steering Thread whose thread is responsible of steering the front wheels and represents the
internal Steer loop controller described in Part 2;
The Sensors Update Thread
The responsibility of the Sensors Update Thread is to read the values from the sensors and update the corresponding private fields. This thread asynchronously also feeds the steerPID.InputSignal with the current value read from the Ev3 Medium Motor tachometer.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
/// <summary> /// Sensors update thread /// </summary> private void SensorUpdateThread() { Thread.CurrentThread.IsBackground = true; while (!stopSensorUpdate.WaitOne (sensorUpdateSamplingTime)) { // Ev3 Color sensor in reflected light mode reflectedLight = colorSensor.Read (); // Current steering angle and asynchronous feed to the Steering Thread currentSteeringAngle = steerWheel.GetTachoCount (); steerPID.InputSignal = (sbyte) currentSteeringAngle; } } |
The LCD Thread
The LCD Thread updates the Ev3 Brick LCD panel with useful real-time data, namely the current reflected light value and the current steering angle. You can use these data to understand the correct set-point to assign to the Drive external control loop according to the colors of the line and the floor.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
/// <summary> /// LCD Thread /// </summary> private void LCDThread() { Thread.CurrentThread.IsBackground = true; Font f = Font.MediumFont; Point offset = new Point(0,25); Point p = new Point(10, Lcd.Height-75); Point boxSize = new Point(100, 24); Rectangle box = new Rectangle(p, p+boxSize); while (!stopLCDThread.WaitOne(lCDThreadSampleTime)) { Lcd.Clear (); Lcd.WriteTextBox (f, box , "Light = " + reflectedLight, true); Lcd.WriteTextBox (f, box + offset , "Steer " + currentSteeringAngle, false); Lcd.Update (); } } |
The Drive Thread
The Drive Thread is maybe the most interesting of this project.
The external Drive loop is a proportional controller with a dead band on the regulation error; this means that if the absolute value of the regulation error is smaller than the defined threshold, no correction is applied to the vehicle trajectory.
Once the correction, if any, has been computed, an adjustment to the vehicle speed is performed. It means that vehicle slows down when a curve is detected, just as a human driver would do.
The correction to the trajectory is than evaluated against the valid range and eventually saturated to the violated boundary. After that, a fixed additional correction is made to account for the gears backlash; than it is fed steerPID to be executed.
Last, but very important, thing is to adjust the relative speed of the rear wheels according to direction and intensity of the steering. The Sup3r Car model has no differential gearbox on the rear wheels; so the differential gearbox must be emulated reducing the speed of the internal wheel during a curve.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 |
/// <summary> /// Drive thread /// </summary> private void DriveThread() { Thread.CurrentThread.IsBackground = true; // Parameters sbyte setPoint = 15; sbyte maxDriveSpeed = 30; sbyte driveSpeed = 0; sbyte steeringOffset = 0; // PID Variables sbyte deadband = 5; sbyte maxOutput = 40; sbyte error = 0; sbyte u = 0; float kp = 0.8f; // Electronic differential variables; sbyte leftSpeed = driveSpeed; sbyte rightSpeed = driveSpeed; while (!stopDrive.WaitOne (driveSamplingTime)) { // Compute tracking error error = (sbyte) (setPoint - reflectedLight); // Determine if error is within deadband and if not, compute the correction if (Math.Abs (error) > deadband) u = (sbyte)(-kp * error); else u = 0; // Determine speed if (Math.Abs (error) <= deadband) driveSpeed = maxDriveSpeed; else if ((Math.Abs (error) > deadband) || (Math.Abs (error) < 3 * deadband)) driveSpeed = (sbyte) (maxDriveSpeed / 2); else if (Math.Abs (error) >= 3*deadband) driveSpeed = (sbyte) (maxDriveSpeed / 3); // See if the correction is outside admitted bounds if (u > maxOutput) u = maxOutput; if (u < (sbyte) -maxOutput) u = (sbyte) -maxOutput; steeringOffset = 0; // Adjust rear wheels speed when steering if (u < 0) { // Steering to right leftSpeed = driveSpeed; rightSpeed = (sbyte) (driveSpeed * Math.Abs (maxOutput + u) / maxOutput); steeringOffset = -20; } else if (u > 0) { // Steering to left leftSpeed = (sbyte) (driveSpeed * Math.Abs (maxOutput - u) / maxOutput); rightSpeed = driveSpeed; } steerPID.SetPoint = (sbyte) (-gearRatio * (u + steeringOffset)); rightEngine.SetSpeed ((sbyte) -rightSpeed); leftEngine.SetSpeed ((sbyte) -leftSpeed); Buttons.LedPattern (1); } Buttons.LedPattern (0); } |
The Steering Thread
The Steering thread is not explicitly defined; instead a MediumMotorPositionPID object, steerPID , is instantiated and used to control the front steering wheels.
← Part 2 – The control strategy