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@2:f4d6c9ba05d0, 2016-03-16 (annotated)

Committer:
Szilard
Date:
Wed Mar 16 04:46:09 2016 +0000
Revision:
2:f4d6c9ba05d0
Parent:
1:c70bc01ebfdd
Child:
3:905643e72bcd
moving in straight line

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 2:f4d6c9ba05d0 13 #include "uLCD_4DGL.h"
Szilard 1:c70bc01ebfdd 14 //one full revolution=193 counts
aberk 0:be99ed42340d 15
aberk 0:be99ed42340d 16
Szilard 1:c70bc01ebfdd 17 class Counter {
Szilard 1:c70bc01ebfdd 18 public:
Szilard 1:c70bc01ebfdd 19 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
Szilard 1:c70bc01ebfdd 20 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
Szilard 1:c70bc01ebfdd 21 }
Szilard 1:c70bc01ebfdd 22
Szilard 1:c70bc01ebfdd 23 void increment() {
Szilard 1:c70bc01ebfdd 24 _count++;
Szilard 1:c70bc01ebfdd 25 }
Szilard 1:c70bc01ebfdd 26
Szilard 1:c70bc01ebfdd 27 int read() {
Szilard 1:c70bc01ebfdd 28 return _count;
Szilard 1:c70bc01ebfdd 29 }
Szilard 1:c70bc01ebfdd 30
Szilard 1:c70bc01ebfdd 31 private:
Szilard 1:c70bc01ebfdd 32 InterruptIn _interrupt;
Szilard 1:c70bc01ebfdd 33 volatile int _count;
Szilard 1:c70bc01ebfdd 34 };
Szilard 1:c70bc01ebfdd 35
Szilard 2:f4d6c9ba05d0 36 int distTransform(float input) {
Szilard 2:f4d6c9ba05d0 37 if (input>3) return 6;
Szilard 2:f4d6c9ba05d0 38 else if (input>2.5) return 8;
Szilard 2:f4d6c9ba05d0 39 else if (input>2) return 10;
Szilard 2:f4d6c9ba05d0 40 else if (input>1.5) return 14;
Szilard 2:f4d6c9ba05d0 41 else if (input>1.1) return 22;
Szilard 2:f4d6c9ba05d0 42 else if (input>0.9) return 27;
Szilard 2:f4d6c9ba05d0 43 else if (input>0.75) return 35;
Szilard 2:f4d6c9ba05d0 44 else if (input>0.6) return 45;
Szilard 2:f4d6c9ba05d0 45 else if (input>0.5) return 60;
Szilard 2:f4d6c9ba05d0 46 else if (input>0.4) return 75;
Szilard 2:f4d6c9ba05d0 47 else return 99;
Szilard 2:f4d6c9ba05d0 48 }
Szilard 2:f4d6c9ba05d0 49
Szilard 1:c70bc01ebfdd 50 Motor leftMotor(p22, p5, p6, 1); // pwm, fwd, rev, can brake
Szilard 1:c70bc01ebfdd 51 Motor rightMotor(p21, p8, p7, 1); // pwm, fwd, rev, can brake
Szilard 1:c70bc01ebfdd 52 Counter leftPulses(p9), rightPulses (p10);
aberk 0:be99ed42340d 53 //Tuning parameters calculated from step tests;
aberk 0:be99ed42340d 54 //see http://mbed.org/cookbook/PID for examples.
Szilard 1:c70bc01ebfdd 55 //PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td old
Szilard 2:f4d6c9ba05d0 56 PID leftPid(0.4310, 0.1, 0.0, 0.01); //Kc, Ti, Td
aberk 0:be99ed42340d 57 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
Szilard 1:c70bc01ebfdd 58 DigitalOut led(LED1);
Szilard 2:f4d6c9ba05d0 59 AnalogIn ain(p15);
Szilard 2:f4d6c9ba05d0 60 uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;
aberk 0:be99ed42340d 61
aberk 0:be99ed42340d 62 int main() {
Szilard 2:f4d6c9ba05d0 63 uLCD.printf("\nHello World\n"); //Default Green on black text
Szilard 2:f4d6c9ba05d0 64
aberk 0:be99ed42340d 65 //Input and output limits have been determined
aberk 0:be99ed42340d 66 //empirically with the specific motors being used.
aberk 0:be99ed42340d 67 //Change appropriately for different motors.
aberk 0:be99ed42340d 68 //Input units: counts per second.
aberk 0:be99ed42340d 69 //Output units: PwmOut duty cycle as %.
aberk 0:be99ed42340d 70 //Default limits are for moving forward.
aberk 0:be99ed42340d 71 leftPid.setInputLimits(0, 3000);
Szilard 1:c70bc01ebfdd 72 leftPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 73 leftPid.setMode(AUTO_MODE);
Szilard 2:f4d6c9ba05d0 74 rightPid.setInputLimits(0, 3200);
Szilard 1:c70bc01ebfdd 75 rightPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 76 rightPid.setMode(AUTO_MODE);
Szilard 2:f4d6c9ba05d0 77 Serial pc(USBTX, USBRX);
Szilard 1:c70bc01ebfdd 78
Szilard 2:f4d6c9ba05d0 79 int leftPrevPulses = 0; //The previous reading of how far the left wheel
aberk 0:be99ed42340d 80 //has travelled.
aberk 0:be99ed42340d 81 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per
aberk 0:be99ed42340d 82 //second.
Szilard 2:f4d6c9ba05d0 83 int rightPrevPulses = 0; //The previous reading of how far the right wheel
aberk 0:be99ed42340d 84 //has travelled.
aberk 0:be99ed42340d 85 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
aberk 0:be99ed42340d 86 //second.
Szilard 1:c70bc01ebfdd 87
Szilard 1:c70bc01ebfdd 88 int distance = 1000; //Number of pulses to travel.
Szilard 1:c70bc01ebfdd 89 led=0;
aberk 0:be99ed42340d 90
Szilard 1:c70bc01ebfdd 91 wait(0.5); //Wait a few seconds before we start moving.
Szilard 2:f4d6c9ba05d0 92 uLCD.cls();
aberk 0:be99ed42340d 93
aberk 0:be99ed42340d 94 //Velocity to mantain in pulses per second.
aberk 0:be99ed42340d 95 leftPid.setSetPoint(1000);
Szilard 2:f4d6c9ba05d0 96 rightPid.setSetPoint(900);
aberk 0:be99ed42340d 97
Szilard 2:f4d6c9ba05d0 98 while ((leftPulses.read() < distance) || (rightPulses.read() < distance)) {
Szilard 2:f4d6c9ba05d0 99
aberk 0:be99ed42340d 100
Szilard 2:f4d6c9ba05d0 101 leftVelocity = (leftPulses.read() - leftPrevPulses) / 0.01;
Szilard 2:f4d6c9ba05d0 102 leftPrevPulses = leftPulses.read();
Szilard 2:f4d6c9ba05d0 103 rightVelocity = (rightPulses.read() - rightPrevPulses) / 0.01;
Szilard 2:f4d6c9ba05d0 104 rightPrevPulses = rightPulses.read();
Szilard 2:f4d6c9ba05d0 105
aberk 0:be99ed42340d 106 leftPid.setProcessValue(fabs(leftVelocity));
aberk 0:be99ed42340d 107 leftMotor.speed(leftPid.compute());
aberk 0:be99ed42340d 108 rightPid.setProcessValue(fabs(rightVelocity));
aberk 0:be99ed42340d 109 rightMotor.speed(rightPid.compute());
Szilard 1:c70bc01ebfdd 110
Szilard 2:f4d6c9ba05d0 111 pc.printf("\n%i", distTransform(ain));
Szilard 2:f4d6c9ba05d0 112
Szilard 2:f4d6c9ba05d0 113 uLCD.locate(1,2);
Szilard 2:f4d6c9ba05d0 114 uLCD.printf("%i", distTransform(ain));
Szilard 2:f4d6c9ba05d0 115 wait(0.1);
Szilard 1:c70bc01ebfdd 116 led=!led;
Szilard 1:c70bc01ebfdd 117
aberk 0:be99ed42340d 118 }
aberk 0:be99ed42340d 119
Szilard 1:c70bc01ebfdd 120 leftMotor.stop(0.5);
Szilard 1:c70bc01ebfdd 121 rightMotor.stop(0.5);
aberk 0:be99ed42340d 122
aberk 0:be99ed42340d 123 }