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@6:9dc165a89453, 2016-03-16 (annotated)

Committer:
Szilard
Date:
Wed Mar 16 17:37:23 2016 +0000
Revision:
6:9dc165a89453
Parent:
5:a8f6ac485b5d 
published version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:be99ed42340d 1 /**
Szilard 6:9dc165a89453 2 * R2D2 is a mbed robot with the Shadowrobot chassis, two DC motors with feedback control,
Szilard 6:9dc165a89453 3 * IR distance sensor, a speaker and a uLCD
Szilard 6:9dc165a89453 4 */
Szilard 1:c70bc01ebfdd 5
Szilard 1:c70bc01ebfdd 6 #include "mbed.h"
Szilard 1:c70bc01ebfdd 7 #include "motordriver.h"
Szilard 1:c70bc01ebfdd 8 #include "PID.h"
Szilard 2:f4d6c9ba05d0 9 #include "uLCD_4DGL.h"
Szilard 4:48f440b9a787 10 #include "SongPlayer.h"
Szilard 6:9dc165a89453 11 //one full on this wheel is ~193 counts
aberk 0:be99ed42340d 12
Szilard 6:9dc165a89453 13 class Counter { //interrupt driven rotation counter to be used with the feedback control
Szilard 1:c70bc01ebfdd 14 public:
Szilard 1:c70bc01ebfdd 15 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
Szilard 1:c70bc01ebfdd 16 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
Szilard 1:c70bc01ebfdd 17 }
Szilard 1:c70bc01ebfdd 18
Szilard 1:c70bc01ebfdd 19 void increment() {
Szilard 1:c70bc01ebfdd 20 _count++;
Szilard 1:c70bc01ebfdd 21 }
Szilard 1:c70bc01ebfdd 22
Szilard 1:c70bc01ebfdd 23 int read() {
Szilard 1:c70bc01ebfdd 24 return _count;
Szilard 1:c70bc01ebfdd 25 }
Szilard 1:c70bc01ebfdd 26
Szilard 1:c70bc01ebfdd 27 private:
Szilard 1:c70bc01ebfdd 28 InterruptIn _interrupt;
Szilard 1:c70bc01ebfdd 29 volatile int _count;
Szilard 1:c70bc01ebfdd 30 };
Szilard 1:c70bc01ebfdd 31
Szilard 6:9dc165a89453 32 int distTransform(float input) { //stepwise transform the IR output voltage to distance
Szilard 6:9dc165a89453 33 if (input>3) return 6; //IR sensor datasheet: www.sharp-world.com/products/device/lineup/data/pdf/datasheet/gp2y0a21yk_e.pdf
Szilard 2:f4d6c9ba05d0 34 else if (input>2.5) return 8;
Szilard 2:f4d6c9ba05d0 35 else if (input>2) return 10;
Szilard 2:f4d6c9ba05d0 36 else if (input>1.5) return 14;
Szilard 2:f4d6c9ba05d0 37 else if (input>1.1) return 22;
Szilard 2:f4d6c9ba05d0 38 else if (input>0.9) return 27;
Szilard 2:f4d6c9ba05d0 39 else if (input>0.75) return 35;
Szilard 2:f4d6c9ba05d0 40 else if (input>0.6) return 45;
Szilard 2:f4d6c9ba05d0 41 else if (input>0.5) return 60;
Szilard 2:f4d6c9ba05d0 42 else if (input>0.4) return 75;
Szilard 2:f4d6c9ba05d0 43 else return 99;
Szilard 2:f4d6c9ba05d0 44 }
Szilard 2:f4d6c9ba05d0 45
Szilard 3:905643e72bcd 46 Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake
Szilard 3:905643e72bcd 47 Motor rightMotor(p21, p7, p8, 1); // pwm, fwd, rev, can brake
Szilard 1:c70bc01ebfdd 48 Counter leftPulses(p9), rightPulses (p10);
aberk 0:be99ed42340d 49 //Tuning parameters calculated from step tests;
aberk 0:be99ed42340d 50 //see http://mbed.org/cookbook/PID for examples.
Szilard 3:905643e72bcd 51 PID leftPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
aberk 0:be99ed42340d 52 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
Szilard 6:9dc165a89453 53 DigitalOut led(LED1), led2(LED2); //LED feedback
Szilard 6:9dc165a89453 54 AnalogIn ain(p15); //A/D converter for the IR sensor
Szilard 6:9dc165a89453 55 uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin for the uLCD
Szilard 6:9dc165a89453 56 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}; //R2D2 sound effect
Szilard 6:9dc165a89453 57 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}; //for a bit of variety, multiple sound samples could be chosen at random
aberk 0:be99ed42340d 58
aberk 0:be99ed42340d 59 int main() {
Szilard 6:9dc165a89453 60 uLCD.printf("\n I am on an\n important\n mission!"); //Initialization
Szilard 2:f4d6c9ba05d0 61
Szilard 6:9dc165a89453 62 //Tune PID controllers, based on mbed Rover: https://developer.mbed.org/cookbook/mbed-Rover
Szilard 6:9dc165a89453 63 leftPid.setInputLimits(0, 3000);
Szilard 1:c70bc01ebfdd 64 leftPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 65 leftPid.setMode(AUTO_MODE);
Szilard 3:905643e72bcd 66 rightPid.setInputLimits(0, 3000);
Szilard 1:c70bc01ebfdd 67 rightPid.setOutputLimits(0.0, 0.8);
aberk 0:be99ed42340d 68 rightPid.setMode(AUTO_MODE);
Szilard 6:9dc165a89453 69
Szilard 2:f4d6c9ba05d0 70 Serial pc(USBTX, USBRX);
Szilard 4:48f440b9a787 71 SongPlayer mySpeaker(p23);
Szilard 1:c70bc01ebfdd 72
Szilard 6:9dc165a89453 73 int leftPrevPulses = 0, leftActPulses=0; //pulses from left motor
Szilard 6:9dc165a89453 74 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per second
Szilard 6:9dc165a89453 75 int rightPrevPulses = 0, rightActPulses=0; //pulses from right motor
Szilard 6:9dc165a89453 76 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
Szilard 1:c70bc01ebfdd 77
Szilard 3:905643e72bcd 78 int distance = 0; //Number of pulses to travel.
Szilard 1:c70bc01ebfdd 79 led=0;
Szilard 3:905643e72bcd 80 led2=0;
Szilard 6:9dc165a89453 81 uLCD.baudrate(3000000); //uLCD baud rate for fast display
aberk 0:be99ed42340d 82
Szilard 6:9dc165a89453 83 wait(1); //Wait one second before we start moving.
Szilard 2:f4d6c9ba05d0 84 uLCD.cls();
Szilard 3:905643e72bcd 85 uLCD.locate(1,2);
Szilard 5:a8f6ac485b5d 86 uLCD.printf("I must find\n Ben Kenobi!");
aberk 0:be99ed42340d 87
Szilard 6:9dc165a89453 88 //optional motor soft starting to reduce high inrush current
Szilard 6:9dc165a89453 89 /*leftMotor.speed(0.1);
Szilard 6:9dc165a89453 90 rightMotor.speed(0.1);
Szilard 6:9dc165a89453 91 wait(0.1);*/
Szilard 6:9dc165a89453 92
Szilard 6:9dc165a89453 93 leftPid.setSetPoint(1000); //set velocity goals for PID
Szilard 3:905643e72bcd 94 rightPid.setSetPoint(1000);
aberk 0:be99ed42340d 95
Szilard 6:9dc165a89453 96 while (1) { //start of big while loop
Szilard 3:905643e72bcd 97
Szilard 6:9dc165a89453 98 if (distTransform(ain)>50) { //if no barrier within 50 cm go in a straight line!
Szilard 3:905643e72bcd 99 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 100 leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
Szilard 3:905643e72bcd 101 leftPrevPulses = leftActPulses;
Szilard 3:905643e72bcd 102 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 103 rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
Szilard 3:905643e72bcd 104 rightPrevPulses = rightActPulses;
Szilard 3:905643e72bcd 105
Szilard 3:905643e72bcd 106 leftPid.setProcessValue(fabs(leftVelocity));
Szilard 3:905643e72bcd 107 leftMotor.speed(leftPid.compute());
Szilard 3:905643e72bcd 108 rightPid.setProcessValue(fabs(rightVelocity));
Szilard 3:905643e72bcd 109 rightMotor.speed(rightPid.compute());
Szilard 2:f4d6c9ba05d0 110
Szilard 6:9dc165a89453 111 } else { //if there is a barrier within 50 cm, don't go straight, turn!
Szilard 6:9dc165a89453 112 leftMotor.stop(0.1);
Szilard 6:9dc165a89453 113 rightMotor.stop(0.1);
Szilard 6:9dc165a89453 114 led2=1; //turn on LED2 when it is turning
Szilard 5:a8f6ac485b5d 115 uLCD.cls();
Szilard 6:9dc165a89453 116 mySpeaker.PlaySong(note,duration); //play R2D2 sound effects
Szilard 6:9dc165a89453 117 uLCD.filled_circle(64, 64, 63, RED); //display R2D2 visual effects
Szilard 5:a8f6ac485b5d 118 wait(0.2);
Szilard 6:9dc165a89453 119 uLCD.filled_circle(64, 64, 63, 0x0000FF); //light blue color
Szilard 3:905643e72bcd 120 wait(0.5);
Szilard 5:a8f6ac485b5d 121 uLCD.filled_circle(64, 64, 63, RED);
Szilard 5:a8f6ac485b5d 122 wait(0.3);
Szilard 6:9dc165a89453 123 //wait(0.5);
Szilard 3:905643e72bcd 124
Szilard 3:905643e72bcd 125 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 126 rightActPulses=rightPulses.read();
Szilard 6:9dc165a89453 127 distance=leftActPulses+100;
Szilard 6:9dc165a89453 128 while (leftActPulses<distance) { //turn approximately half a revolution
Szilard 6:9dc165a89453 129 leftMotor.speed(-0.5); //rotate to the right
Szilard 6:9dc165a89453 130 rightMotor.speed(0.5); //open loop, because the PID class can't handle negative values
Szilard 3:905643e72bcd 131 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 132 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 133
Szilard 3:905643e72bcd 134 wait(0.005);
Szilard 3:905643e72bcd 135
Szilard 3:905643e72bcd 136 }//Turning while end
Szilard 6:9dc165a89453 137 leftMotor.stop(0.1);
Szilard 6:9dc165a89453 138 rightMotor.stop(0.1);
Szilard 5:a8f6ac485b5d 139 wait(0.1);
Szilard 3:905643e72bcd 140 led2=0;
Szilard 3:905643e72bcd 141 uLCD.cls();
Szilard 3:905643e72bcd 142 uLCD.locate(1,2);
Szilard 5:a8f6ac485b5d 143 uLCD.printf("I must find\n Ben Kenobi!");
Szilard 3:905643e72bcd 144
Szilard 6:9dc165a89453 145 leftPid.setSetPoint(1000); //go straight
Szilard 3:905643e72bcd 146 rightPid.setSetPoint(1000);
Szilard 1:c70bc01ebfdd 147
Szilard 3:905643e72bcd 148 } //Going straight/turning if end
Szilard 2:f4d6c9ba05d0 149
Szilard 6:9dc165a89453 150 //pc.printf("\n%i", distTransform(ain)); //for debugging purposes you can read the distance reading
Szilard 3:905643e72bcd 151 //uLCD.locate(1,1);
Szilard 3:905643e72bcd 152 //uLCD.printf("Distance: %i cm", distTransform(ain));
Szilard 3:905643e72bcd 153 wait(0.01);
Szilard 6:9dc165a89453 154 led=!led; //blink led1 to follow changes
Szilard 1:c70bc01ebfdd 155
Szilard 3:905643e72bcd 156 } //end of big while loop
aberk 0:be99ed42340d 157
aberk 0:be99ed42340d 158 }