Szilard Liptak / Mbed 2 deprecated R2D2_robot

A robot rover with distance sensing and audiovisual effects

Dependencies:   4DGL-uLCD-SE Motordriver PID mbed

Fork of PIDRover by Aaron Berk

main.cpp@1:c70bc01ebfdd, 2016-03-16 (annotated)

Committer:
Szilard
Date:
Wed Mar 16 00:52:47 2016 +0000
Revision:
1:c70bc01ebfdd
Parent:
0:be99ed42340d
Child:
2:f4d6c9ba05d0
not tuned PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:be99ed42340d 1 /**
aberk 0:be99ed42340d 2 * Drive a robot forwards or backwards by using a PID controller to vary
aberk 0:be99ed42340d 3 * the PWM signal to H-bridges connected to the motors to attempt to maintain
aberk 0:be99ed42340d 4 * a constant velocity.
aberk 0:be99ed42340d 5 */
Szilard 1:c70bc01ebfdd 6 /*Sources: mbed Rover cookbook page: https://developer.mbed.org/cookbook/mbed-Rover
Szilard 1:c70bc01ebfdd 7 InterruptIn handbook page: https://developer.mbed.org/handbook/InterruptIn
Szilard 1:c70bc01ebfdd 8 */
Szilard 1:c70bc01ebfdd 9
Szilard 1:c70bc01ebfdd 10 #include "mbed.h"
Szilard 1:c70bc01ebfdd 11 #include "motordriver.h"
Szilard 1:c70bc01ebfdd 12 #include "PID.h"
Szilard 1:c70bc01ebfdd 13 //one full revolution=193 counts
aberk 0:be99ed42340d 14
aberk 0:be99ed42340d 15
Szilard 1:c70bc01ebfdd 16 class Counter {
Szilard 1:c70bc01ebfdd 17 public:
Szilard 1:c70bc01ebfdd 18 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
Szilard 1:c70bc01ebfdd 19 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
Szilard 1:c70bc01ebfdd 20 }
Szilard 1:c70bc01ebfdd 21
Szilard 1:c70bc01ebfdd 22 void increment() {
Szilard 1:c70bc01ebfdd 23 _count++;
Szilard 1:c70bc01ebfdd 24 }
Szilard 1:c70bc01ebfdd 25
Szilard 1:c70bc01ebfdd 26 int read() {
Szilard 1:c70bc01ebfdd 27 return _count;
Szilard 1:c70bc01ebfdd 28 }
Szilard 1:c70bc01ebfdd 29
Szilard 1:c70bc01ebfdd 30 private:
Szilard 1:c70bc01ebfdd 31 InterruptIn _interrupt;
Szilard 1:c70bc01ebfdd 32 volatile int _count;
Szilard 1:c70bc01ebfdd 33 };
Szilard 1:c70bc01ebfdd 34
Szilard 1:c70bc01ebfdd 35 Motor leftMotor(p22, p5, p6, 1); // pwm, fwd, rev, can brake
Szilard 1:c70bc01ebfdd 36 Motor rightMotor(p21, p8, p7, 1); // pwm, fwd, rev, can brake
Szilard 1:c70bc01ebfdd 37 Counter leftPulses(p9), rightPulses (p10);
aberk 0:be99ed42340d 38 //Tuning parameters calculated from step tests;
aberk 0:be99ed42340d 39 //see http://mbed.org/cookbook/PID for examples.
Szilard 1:c70bc01ebfdd 40 //PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td old
Szilard 1:c70bc01ebfdd 41 PID leftPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
aberk 0:be99ed42340d 42 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
Szilard 1:c70bc01ebfdd 43 DigitalOut led(LED1);
aberk 0:be99ed42340d 44
aberk 0:be99ed42340d 45 int main() {
aberk 0:be99ed42340d 46 //Input and output limits have been determined
aberk 0:be99ed42340d 47 //empirically with the specific motors being used.
aberk 0:be99ed42340d 48 //Change appropriately for different motors.
aberk 0:be99ed42340d 49 //Input units: counts per second.
aberk 0:be99ed42340d 50 //Output units: PwmOut duty cycle as %.
aberk 0:be99ed42340d 51 //Default limits are for moving forward.
aberk 0:be99ed42340d 52 leftPid.setInputLimits(0, 3000);
Szilard 1:c70bc01ebfdd 53 leftPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 54 leftPid.setMode(AUTO_MODE);
Szilard 1:c70bc01ebfdd 55 rightPid.setInputLimits(0, 3000);
Szilard 1:c70bc01ebfdd 56 rightPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 57 rightPid.setMode(AUTO_MODE);
aberk 0:be99ed42340d 58
Szilard 1:c70bc01ebfdd 59
Szilard 1:c70bc01ebfdd 60 int leftPrevPulses = 0, leftActPulses=0; //The previous reading of how far the left wheel
aberk 0:be99ed42340d 61 //has travelled.
aberk 0:be99ed42340d 62 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per
aberk 0:be99ed42340d 63 //second.
Szilard 1:c70bc01ebfdd 64 int rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel
aberk 0:be99ed42340d 65 //has travelled.
aberk 0:be99ed42340d 66 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
aberk 0:be99ed42340d 67 //second.
Szilard 1:c70bc01ebfdd 68
Szilard 1:c70bc01ebfdd 69 int distance = 1000; //Number of pulses to travel.
Szilard 1:c70bc01ebfdd 70 led=0;
aberk 0:be99ed42340d 71
Szilard 1:c70bc01ebfdd 72 wait(0.5); //Wait a few seconds before we start moving.
aberk 0:be99ed42340d 73
aberk 0:be99ed42340d 74 //Velocity to mantain in pulses per second.
aberk 0:be99ed42340d 75 leftPid.setSetPoint(1000);
Szilard 1:c70bc01ebfdd 76 rightPid.setSetPoint(1000);
aberk 0:be99ed42340d 77
Szilard 1:c70bc01ebfdd 78 while ((leftActPulses < distance) || (rightActPulses < distance)) {
aberk 0:be99ed42340d 79
aberk 0:be99ed42340d 80 //Get the current pulse count and subtract the previous one to
aberk 0:be99ed42340d 81 //calculate the current velocity in counts per second.
Szilard 1:c70bc01ebfdd 82 leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
Szilard 1:c70bc01ebfdd 83 leftPrevPulses = leftActPulses;
aberk 0:be99ed42340d 84 //Use the absolute value of velocity as the PID controller works
aberk 0:be99ed42340d 85 //in the % (unsigned) domain and will get confused with -ve values.
aberk 0:be99ed42340d 86 leftPid.setProcessValue(fabs(leftVelocity));
aberk 0:be99ed42340d 87 leftMotor.speed(leftPid.compute());
aberk 0:be99ed42340d 88
Szilard 1:c70bc01ebfdd 89 rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
Szilard 1:c70bc01ebfdd 90 rightPrevPulses = rightActPulses;
aberk 0:be99ed42340d 91 rightPid.setProcessValue(fabs(rightVelocity));
aberk 0:be99ed42340d 92 rightMotor.speed(rightPid.compute());
Szilard 1:c70bc01ebfdd 93
aberk 0:be99ed42340d 94 wait(0.01);
Szilard 1:c70bc01ebfdd 95 led=!led;
Szilard 1:c70bc01ebfdd 96
Szilard 1:c70bc01ebfdd 97 leftActPulses=leftPulses.read();
Szilard 1:c70bc01ebfdd 98 rightActPulses=rightPulses.read();
aberk 0:be99ed42340d 99 }
aberk 0:be99ed42340d 100
Szilard 1:c70bc01ebfdd 101 leftMotor.stop(0.5);
Szilard 1:c70bc01ebfdd 102 rightMotor.stop(0.5);
aberk 0:be99ed42340d 103
aberk 0:be99ed42340d 104 }