Harry Eakins / Mbed 2 deprecated SimplePIDBot

A simple robot with 2 motors and 2 encoders. Motor speed is set to 40 and PID controllers are use to attain that speed

Dependencies:   PinDetect mbed mRotaryEncoder PID

main.cpp@0:e019949c4c69, 2011-09-20 (annotated)

Committer:
harryeakins
Date:
Tue Sep 20 12:55:47 2011 +0000
Revision:
0:e019949c4c69
Initial version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
harryeakins 0:e019949c4c69 1 #include "mbed.h"
harryeakins 0:e019949c4c69 2
harryeakins 0:e019949c4c69 3 #include "mRotaryEncoder.h"
harryeakins 0:e019949c4c69 4 #include "HBridgeMotor.h"
harryeakins 0:e019949c4c69 5 #include "PID.h"
harryeakins 0:e019949c4c69 6
harryeakins 0:e019949c4c69 7 // PID constants
harryeakins 0:e019949c4c69 8 #define RATE 0.2
harryeakins 0:e019949c4c69 9 #define Kc 1.5 // Proportional
harryeakins 0:e019949c4c69 10 #define Ti 1.0 // Integral
harryeakins 0:e019949c4c69 11 #define Td 0.0 // Derivative
harryeakins 0:e019949c4c69 12
harryeakins 0:e019949c4c69 13 DigitalOut led1(LED1);
harryeakins 0:e019949c4c69 14
harryeakins 0:e019949c4c69 15 // Two PID Controllers
harryeakins 0:e019949c4c69 16 PID leftPID(Kc, Ti, Td, RATE);
harryeakins 0:e019949c4c69 17 PID rightPID(Kc, Ti, Td, RATE);
harryeakins 0:e019949c4c69 18
harryeakins 0:e019949c4c69 19 // Two de-bounced encoders
harryeakins 0:e019949c4c69 20 mRotaryEncoder left_enc(p19, p20, p29);
harryeakins 0:e019949c4c69 21 mRotaryEncoder right_enc(p26, p27, p28);
harryeakins 0:e019949c4c69 22
harryeakins 0:e019949c4c69 23 // Two motor/h-bridge objects
harryeakins 0:e019949c4c69 24 HBridgeMotor left_motor(p25, p24);
harryeakins 0:e019949c4c69 25 HBridgeMotor right_motor(p21, p22);
harryeakins 0:e019949c4c69 26
harryeakins 0:e019949c4c69 27 void initializePidControllers(void) {
harryeakins 0:e019949c4c69 28 leftPID.setInputLimits(-300, 300);
harryeakins 0:e019949c4c69 29 leftPID.setOutputLimits(-1.0, 1.0);
harryeakins 0:e019949c4c69 30 leftPID.setMode(AUTO_MODE);
harryeakins 0:e019949c4c69 31
harryeakins 0:e019949c4c69 32 rightPID.setInputLimits(-300, 300);
harryeakins 0:e019949c4c69 33 rightPID.setOutputLimits(-1.0, 1.0);
harryeakins 0:e019949c4c69 34 rightPID.setMode(AUTO_MODE);
harryeakins 0:e019949c4c69 35 }
harryeakins 0:e019949c4c69 36
harryeakins 0:e019949c4c69 37 int main() {
harryeakins 0:e019949c4c69 38 int left_pulses = 0, right_pulses = 0;
harryeakins 0:e019949c4c69 39
harryeakins 0:e019949c4c69 40 initializePidControllers();
harryeakins 0:e019949c4c69 41
harryeakins 0:e019949c4c69 42 // 40 encoder ticks per second target speed
harryeakins 0:e019949c4c69 43 leftPID.setSetPoint(40.0);
harryeakins 0:e019949c4c69 44 rightPID.setSetPoint(40.0);
harryeakins 0:e019949c4c69 45
harryeakins 0:e019949c4c69 46 while (true) {
harryeakins 0:e019949c4c69 47 // Get velocities
harryeakins 0:e019949c4c69 48 float left_velocity = float(left_enc.Get() - left_pulses)/RATE;
harryeakins 0:e019949c4c69 49 float right_velocity = float(right_enc.Get() - right_pulses)/RATE;
harryeakins 0:e019949c4c69 50
harryeakins 0:e019949c4c69 51 // Pass the PID Controller the latest velocity value
harryeakins 0:e019949c4c69 52 leftPID.setProcessValue(left_velocity);
harryeakins 0:e019949c4c69 53 rightPID.setProcessValue(right_velocity);
harryeakins 0:e019949c4c69 54
harryeakins 0:e019949c4c69 55 // Query the PID Controllers for the desired power outputs
harryeakins 0:e019949c4c69 56 // and set the motors to those powers
harryeakins 0:e019949c4c69 57 left_motor.set(leftPID.compute());
harryeakins 0:e019949c4c69 58 right_motor.set(rightPID.compute());
harryeakins 0:e019949c4c69 59
harryeakins 0:e019949c4c69 60 // Save the number of encoder ticks for velocity calculation in the next loop
harryeakins 0:e019949c4c69 61 left_pulses = left_enc.Get();
harryeakins 0:e019949c4c69 62 right_pulses = right_enc.Get();
harryeakins 0:e019949c4c69 63
harryeakins 0:e019949c4c69 64 // Toggle the LED and wait
harryeakins 0:e019949c4c69 65 led1 = 1 - led1;
harryeakins 0:e019949c4c69 66 wait(RATE);
harryeakins 0:e019949c4c69 67 }
harryeakins 0:e019949c4c69 68 }