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

Committer:
Szilard
Date:
Wed Mar 16 07:24:04 2016 +0000
Revision:
5:a8f6ac485b5d
Parent:
4:48f440b9a787
Child:
6:9dc165a89453
untuned, but works

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 4:48f440b9a787 14 #include "SongPlayer.h"
Szilard 1:c70bc01ebfdd 15 //one full revolution=193 counts
aberk 0:be99ed42340d 16
aberk 0:be99ed42340d 17
Szilard 1:c70bc01ebfdd 18 class Counter {
Szilard 1:c70bc01ebfdd 19 public:
Szilard 1:c70bc01ebfdd 20 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
Szilard 1:c70bc01ebfdd 21 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
Szilard 1:c70bc01ebfdd 22 }
Szilard 1:c70bc01ebfdd 23
Szilard 1:c70bc01ebfdd 24 void increment() {
Szilard 1:c70bc01ebfdd 25 _count++;
Szilard 1:c70bc01ebfdd 26 }
Szilard 1:c70bc01ebfdd 27
Szilard 1:c70bc01ebfdd 28 int read() {
Szilard 1:c70bc01ebfdd 29 return _count;
Szilard 1:c70bc01ebfdd 30 }
Szilard 1:c70bc01ebfdd 31
Szilard 1:c70bc01ebfdd 32 private:
Szilard 1:c70bc01ebfdd 33 InterruptIn _interrupt;
Szilard 1:c70bc01ebfdd 34 volatile int _count;
Szilard 1:c70bc01ebfdd 35 };
Szilard 1:c70bc01ebfdd 36
Szilard 2:f4d6c9ba05d0 37 int distTransform(float input) {
Szilard 2:f4d6c9ba05d0 38 if (input>3) return 6;
Szilard 2:f4d6c9ba05d0 39 else if (input>2.5) return 8;
Szilard 2:f4d6c9ba05d0 40 else if (input>2) return 10;
Szilard 2:f4d6c9ba05d0 41 else if (input>1.5) return 14;
Szilard 2:f4d6c9ba05d0 42 else if (input>1.1) return 22;
Szilard 2:f4d6c9ba05d0 43 else if (input>0.9) return 27;
Szilard 2:f4d6c9ba05d0 44 else if (input>0.75) return 35;
Szilard 2:f4d6c9ba05d0 45 else if (input>0.6) return 45;
Szilard 2:f4d6c9ba05d0 46 else if (input>0.5) return 60;
Szilard 2:f4d6c9ba05d0 47 else if (input>0.4) return 75;
Szilard 2:f4d6c9ba05d0 48 else return 99;
Szilard 2:f4d6c9ba05d0 49 }
Szilard 2:f4d6c9ba05d0 50
Szilard 3:905643e72bcd 51 Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake
Szilard 3:905643e72bcd 52 Motor rightMotor(p21, p7, p8, 1); // pwm, fwd, rev, can brake
Szilard 1:c70bc01ebfdd 53 Counter leftPulses(p9), rightPulses (p10);
aberk 0:be99ed42340d 54 //Tuning parameters calculated from step tests;
aberk 0:be99ed42340d 55 //see http://mbed.org/cookbook/PID for examples.
Szilard 1:c70bc01ebfdd 56 //PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td old
Szilard 3:905643e72bcd 57 PID leftPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
aberk 0:be99ed42340d 58 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
Szilard 3:905643e72bcd 59 DigitalOut led(LED1), led2(LED2);
Szilard 2:f4d6c9ba05d0 60 AnalogIn ain(p15);
Szilard 2:f4d6c9ba05d0 61 uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;
Szilard 5:a8f6ac485b5d 62 float note[18]= {3520, 3135.96, 2637.02, 2093, 2349.32, 3951.07, 2793.83, 4186.01, 3520, 3135.96, 2637.02, 2093, 2349.32, 3951.07, 2793.83, 4186.01};
Szilard 5:a8f6ac485b5d 63 float duration[18]= {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};
aberk 0:be99ed42340d 64
aberk 0:be99ed42340d 65 int main() {
Szilard 5:a8f6ac485b5d 66 uLCD.printf("\n I am on an\n important\n mission!"); //Default Green on black text
Szilard 2:f4d6c9ba05d0 67
aberk 0:be99ed42340d 68 //Input and output limits have been determined
aberk 0:be99ed42340d 69 //empirically with the specific motors being used.
aberk 0:be99ed42340d 70 //Change appropriately for different motors.
aberk 0:be99ed42340d 71 //Input units: counts per second.
aberk 0:be99ed42340d 72 //Output units: PwmOut duty cycle as %.
aberk 0:be99ed42340d 73 //Default limits are for moving forward.
aberk 0:be99ed42340d 74 leftPid.setInputLimits(0, 3000);
Szilard 1:c70bc01ebfdd 75 leftPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 76 leftPid.setMode(AUTO_MODE);
Szilard 3:905643e72bcd 77 rightPid.setInputLimits(0, 3000);
Szilard 1:c70bc01ebfdd 78 rightPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 79 rightPid.setMode(AUTO_MODE);
Szilard 2:f4d6c9ba05d0 80 Serial pc(USBTX, USBRX);
Szilard 4:48f440b9a787 81 SongPlayer mySpeaker(p23);
Szilard 1:c70bc01ebfdd 82
Szilard 3:905643e72bcd 83 int leftPrevPulses = 0, leftActPulses=0; //The previous reading of how far the left wheel
aberk 0:be99ed42340d 84 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per
Szilard 3:905643e72bcd 85 int rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel
aberk 0:be99ed42340d 86 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
Szilard 1:c70bc01ebfdd 87
Szilard 3:905643e72bcd 88 int distance = 0; //Number of pulses to travel.
Szilard 1:c70bc01ebfdd 89 led=0;
Szilard 3:905643e72bcd 90 led2=0;
Szilard 5:a8f6ac485b5d 91 uLCD.baudrate(3000000);
aberk 0:be99ed42340d 92
Szilard 4:48f440b9a787 93 wait(1); //Wait a few seconds before we start moving.
Szilard 2:f4d6c9ba05d0 94 uLCD.cls();
Szilard 3:905643e72bcd 95 uLCD.locate(1,2);
Szilard 5:a8f6ac485b5d 96 uLCD.printf("I must find\n Ben Kenobi!");
aberk 0:be99ed42340d 97
aberk 0:be99ed42340d 98 //Velocity to mantain in pulses per second.
aberk 0:be99ed42340d 99 leftPid.setSetPoint(1000);
Szilard 3:905643e72bcd 100 rightPid.setSetPoint(1000);
aberk 0:be99ed42340d 101
Szilard 3:905643e72bcd 102 while (1) {
Szilard 3:905643e72bcd 103
Szilard 3:905643e72bcd 104 if (distTransform(ain)>50) { //going straight line
Szilard 3:905643e72bcd 105 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 106 leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
Szilard 3:905643e72bcd 107 leftPrevPulses = leftActPulses;
Szilard 3:905643e72bcd 108 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 109 rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
Szilard 3:905643e72bcd 110 rightPrevPulses = rightActPulses;
Szilard 3:905643e72bcd 111
Szilard 3:905643e72bcd 112 leftPid.setProcessValue(fabs(leftVelocity));
Szilard 3:905643e72bcd 113 leftMotor.speed(leftPid.compute());
Szilard 3:905643e72bcd 114 rightPid.setProcessValue(fabs(rightVelocity));
Szilard 3:905643e72bcd 115 rightMotor.speed(rightPid.compute());
Szilard 2:f4d6c9ba05d0 116
Szilard 3:905643e72bcd 117 } else { //Don't go straight, turn!
Szilard 3:905643e72bcd 118 leftMotor.stop(0.5);
Szilard 3:905643e72bcd 119 rightMotor.stop(0.5);
Szilard 3:905643e72bcd 120 led2=1;
Szilard 5:a8f6ac485b5d 121 uLCD.cls();
Szilard 3:905643e72bcd 122 uLCD.locate(1,2);
Szilard 5:a8f6ac485b5d 123 //uLCD.printf("He is not here!");
Szilard 4:48f440b9a787 124 mySpeaker.PlaySong(note,duration);
Szilard 5:a8f6ac485b5d 125 uLCD.filled_circle(64, 64, 63, RED);
Szilard 5:a8f6ac485b5d 126 wait(0.2);
Szilard 5:a8f6ac485b5d 127 uLCD.filled_circle(64, 64, 63, 0x0000FF);
Szilard 3:905643e72bcd 128 wait(0.5);
Szilard 5:a8f6ac485b5d 129 uLCD.filled_circle(64, 64, 63, RED);
Szilard 5:a8f6ac485b5d 130 wait(0.3);
Szilard 5:a8f6ac485b5d 131 //wait(0.5);
Szilard 3:905643e72bcd 132 leftPid.setSetPoint(-500);
Szilard 3:905643e72bcd 133 rightPid.setSetPoint(500);
Szilard 3:905643e72bcd 134
Szilard 3:905643e72bcd 135 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 136 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 137 distance=leftActPulses+100;
Szilard 3:905643e72bcd 138 while (leftActPulses<distance) { //I'm turning!
Szilard 3:905643e72bcd 139 leftMotor.speed(-0.5);
Szilard 3:905643e72bcd 140 rightMotor.speed(0.5);
Szilard 3:905643e72bcd 141 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 142 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 143
Szilard 3:905643e72bcd 144 wait(0.005);
Szilard 3:905643e72bcd 145
Szilard 3:905643e72bcd 146 }//Turning while end
Szilard 3:905643e72bcd 147 leftMotor.stop(0.5);
Szilard 3:905643e72bcd 148 rightMotor.stop(0.5);
Szilard 5:a8f6ac485b5d 149 wait(0.1);
Szilard 3:905643e72bcd 150 led2=0;
Szilard 3:905643e72bcd 151 uLCD.cls();
Szilard 3:905643e72bcd 152 uLCD.locate(1,2);
Szilard 5:a8f6ac485b5d 153 uLCD.printf("I must find\n Ben Kenobi!");
Szilard 3:905643e72bcd 154
Szilard 3:905643e72bcd 155 leftPid.setSetPoint(1000);
Szilard 3:905643e72bcd 156 rightPid.setSetPoint(1000);
Szilard 1:c70bc01ebfdd 157
Szilard 3:905643e72bcd 158 } //Going straight/turning if end
Szilard 2:f4d6c9ba05d0 159
Szilard 3:905643e72bcd 160 //pc.printf("\n%i", distTransform(ain));
Szilard 3:905643e72bcd 161 //uLCD.locate(1,1);
Szilard 3:905643e72bcd 162 //uLCD.printf("Distance: %i cm", distTransform(ain));
Szilard 3:905643e72bcd 163 wait(0.01);
Szilard 1:c70bc01ebfdd 164 led=!led;
Szilard 1:c70bc01ebfdd 165
Szilard 3:905643e72bcd 166 } //end of big while loop
aberk 0:be99ed42340d 167
aberk 0:be99ed42340d 168 }