1. Lecture 3: The Controller PID Control and Speed Profile

2. Objective Create a system that allows you to easily control the angular and translational velocity of your mouse Utilize feedback from multiple sensors (IR sensors, encoders, gyro) Mouse should accelerate and decelerate at a constant rate, for both angular and translational movement Mouse should not collide with walls Control your mouse using practical measurement units, like millimeters

3. Overview Most of the code will be in the systick handler This will make sure that your mouse is always maintaining the correct velocity Also makes sure we can obtain accurate velocities, since we know the time between systick calls (1 ms). From outside your controller, you will be able to set the mouse’s two velocities (angular and translational). The controller will automatically accelerate or decelerate to that velocity, then maintain it.

5. Six Velocity Variables targetVelX / targetVelW Long term velocity goal Simplifies control by abstracting away the acceleration Written to in main(), but read from in systick() Controls all motion from the perspective of main() idealVelX / idealVelW Short term velocity goal Accounts for acceleration actualVelX / actualVelW We want this to match the IdealVelocity Obtained from sensors (encoders/gyro/IR)

6. The Steps Within 1ms we need to finish all things as follow in sequence 1.Collect data from all Sensors(IR, gyro, encoders) 2.Find the actualVelX/W 3.Calclulate the new values for idealVelX/W 4.velErrorX/W is the difference between actualVelX/W and idealVelX/W 5.posErrorX/W is the integral of velErrorX/W 6.Combine errors in a clever way (i.e. PID controller) to get overall errors: errorX/W 7.Set the PWM of the motors based on the combined errors

7. Find the actual velocities To find actualVelX: Take the derivative of the encoder counts to get rightVel and leftVel actualVelX is the sum of rightVel and leftVel (can divide it by 2 if you want) To find actualVelW: Can use gyro output or difference between leftVel and rightVel Better to use gyro during curved turns, since wheels might slip Especially if you ever make a 4-wheeled mouse

8. Find the ideal velocities If your previous idealVel is less/more than your targetVel, then add/subtract the acceleration rate to your previous idealVel to get the current idealVel Need to also check that you didn’t overshoot the targetVel or you might keep ping-ponging above and below the targetVel If your previous idealVel is the same as your targetVel, then the idealVel is unchanged This creates the trapezoidal shape for your idealVel

9. Get IR sensor error if(DLSensor > DLMiddleValue && DRSensor < DRMiddleValue) IRSensorError = DLMiddleValue - DLSensor; else if(DRSensor > DRMiddleValue && DLSensor < DLMiddleValue) IRSensorError = DRSensor - DRMiddleValue; else IRSensorError = 0; Move away from a wall if you are too close, if not then just use encoders to go straight. What about if there is a wall on one side, but you are too far from it?

10. PID Purpose of PID I term tries to remember past errors D term tries to predict future errors Problems with PID I tends to accumulate small constant errors into big errors D tends to be noisy Proposed solution Take advantage of the different sensor systems in a way that exploits their strengths and suppress their weaknesses Gyro/Encoders accurately tell us velocity IR sensors accurately tell us position

11. Some Proposed PID Solutions (to find errorW) Positional PID P term is positional error I term is the integral of positional error D term is the velocity error We probably don’t want to have the I term for encoder/gyro since we would be taking the double integral from the native value (velocity) Maybe we don’t want the D term from IR sensors since it might amplify the noise For errorX, we only have encoders to use, so just use PD Encoder / GyroIR Sensors PXX IX DX Encoder / GyroIR Sensors PXX IX DXX

12. PID Code Example IRSensorError = sensorError() * IRScalingFactor; IRSensorErrorInteg += IRSensorError; P = posErrorW + IRSensorError; I = IRSensorErrorInteg; D = velErrorW; errorW = Kp * P + Ki * I + Kd * D;

13. Set the PWM of the motors Something like this should be at the end of your systick function: setLeftPwm(errorX – errorW); setRightPwm(errorX + errorW); If your errorW is inverted from this example, then switch the + and -