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@4:48f440b9a787, 2016-03-16 (annotated)

Committer:
Szilard
Date:
Wed Mar 16 06:52:07 2016 +0000
Revision:
4:48f440b9a787
Parent:
3:905643e72bcd
Child:
5:a8f6ac485b5d
basic sound 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 4:48f440b9a787 62 float note[18]= {1568.0,1396.9,1244.5};
Szilard 4:48f440b9a787 63 float duration[18]= {0.48,0.24,0.72};
aberk 0:be99ed42340d 64
aberk 0:be99ed42340d 65 int main() {
Szilard 4:48f440b9a787 66 uLCD.printf("\nI must find Ben Kenobi!\n"); //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;
aberk 0:be99ed42340d 91
Szilard 4:48f440b9a787 92 wait(1); //Wait a few seconds before we start moving.
Szilard 2:f4d6c9ba05d0 93 uLCD.cls();
Szilard 3:905643e72bcd 94 uLCD.locate(1,2);
Szilard 3:905643e72bcd 95 uLCD.printf("I go straight!");
aberk 0:be99ed42340d 96
aberk 0:be99ed42340d 97 //Velocity to mantain in pulses per second.
aberk 0:be99ed42340d 98 leftPid.setSetPoint(1000);
Szilard 3:905643e72bcd 99 rightPid.setSetPoint(1000);
aberk 0:be99ed42340d 100
Szilard 3:905643e72bcd 101 while (1) {
Szilard 3:905643e72bcd 102
Szilard 3:905643e72bcd 103 if (distTransform(ain)>50) { //going straight line
Szilard 3:905643e72bcd 104 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 105 leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
Szilard 3:905643e72bcd 106 leftPrevPulses = leftActPulses;
Szilard 3:905643e72bcd 107 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 108 rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
Szilard 3:905643e72bcd 109 rightPrevPulses = rightActPulses;
Szilard 3:905643e72bcd 110
Szilard 3:905643e72bcd 111 leftPid.setProcessValue(fabs(leftVelocity));
Szilard 3:905643e72bcd 112 leftMotor.speed(leftPid.compute());
Szilard 3:905643e72bcd 113 rightPid.setProcessValue(fabs(rightVelocity));
Szilard 3:905643e72bcd 114 rightMotor.speed(rightPid.compute());
Szilard 2:f4d6c9ba05d0 115
Szilard 3:905643e72bcd 116 } else { //Don't go straight, turn!
Szilard 3:905643e72bcd 117 leftMotor.stop(0.5);
Szilard 3:905643e72bcd 118 rightMotor.stop(0.5);
Szilard 3:905643e72bcd 119 led2=1;
Szilard 3:905643e72bcd 120 uLCD.locate(1,2);
Szilard 4:48f440b9a787 121 uLCD.printf("Ben Kenobi is not here!");
Szilard 4:48f440b9a787 122 mySpeaker.PlaySong(note,duration);
Szilard 3:905643e72bcd 123 wait(0.5);
Szilard 3:905643e72bcd 124 leftPid.setSetPoint(-500);
Szilard 3:905643e72bcd 125 rightPid.setSetPoint(500);
Szilard 3:905643e72bcd 126
Szilard 3:905643e72bcd 127 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 128 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 129 distance=leftActPulses+100;
Szilard 3:905643e72bcd 130 while (leftActPulses<distance) { //I'm turning!
Szilard 3:905643e72bcd 131 leftMotor.speed(-0.5);
Szilard 3:905643e72bcd 132 rightMotor.speed(0.5);
Szilard 3:905643e72bcd 133 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 134 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 135
Szilard 3:905643e72bcd 136 wait(0.005);
Szilard 3:905643e72bcd 137
Szilard 3:905643e72bcd 138 }//Turning while end
Szilard 3:905643e72bcd 139 leftMotor.stop(0.5);
Szilard 3:905643e72bcd 140 rightMotor.stop(0.5);
Szilard 3:905643e72bcd 141 wait(0.5);
Szilard 3:905643e72bcd 142 led2=0;
Szilard 3:905643e72bcd 143 uLCD.cls();
Szilard 3:905643e72bcd 144 uLCD.locate(1,2);
Szilard 3:905643e72bcd 145 uLCD.printf("I go straight!");
Szilard 3:905643e72bcd 146
Szilard 3:905643e72bcd 147 leftPid.setSetPoint(1000);
Szilard 3:905643e72bcd 148 rightPid.setSetPoint(1000);
Szilard 1:c70bc01ebfdd 149
Szilard 3:905643e72bcd 150 } //Going straight/turning if end
Szilard 2:f4d6c9ba05d0 151
Szilard 3:905643e72bcd 152 //pc.printf("\n%i", distTransform(ain));
Szilard 3:905643e72bcd 153 //uLCD.locate(1,1);
Szilard 3:905643e72bcd 154 //uLCD.printf("Distance: %i cm", distTransform(ain));
Szilard 3:905643e72bcd 155 wait(0.01);
Szilard 1:c70bc01ebfdd 156 led=!led;
Szilard 1:c70bc01ebfdd 157
Szilard 3:905643e72bcd 158 } //end of big while loop
aberk 0:be99ed42340d 159
aberk 0:be99ed42340d 160 }