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@3:905643e72bcd, 2016-03-16 (annotated)

Committer:
Szilard
Date:
Wed Mar 16 06:38:03 2016 +0000
Revision:
3:905643e72bcd
Parent:
2:f4d6c9ba05d0
Child:
4:48f440b9a787
Open loop turning 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 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 3:905643e72bcd 50 Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake
Szilard 3:905643e72bcd 51 Motor rightMotor(p21, p7, p8, 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 3:905643e72bcd 56 PID leftPid(0.4620, 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 3:905643e72bcd 58 DigitalOut led(LED1), led2(LED2);
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 3:905643e72bcd 74 rightPid.setInputLimits(0, 3000);
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 3:905643e72bcd 79 int leftPrevPulses = 0, leftActPulses=0; //The previous reading of how far the left wheel
aberk 0:be99ed42340d 80 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per
Szilard 3:905643e72bcd 81 int rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel
aberk 0:be99ed42340d 82 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
Szilard 1:c70bc01ebfdd 83
Szilard 3:905643e72bcd 84 int distance = 0; //Number of pulses to travel.
Szilard 1:c70bc01ebfdd 85 led=0;
Szilard 3:905643e72bcd 86 led2=0;
aberk 0:be99ed42340d 87
Szilard 1:c70bc01ebfdd 88 wait(0.5); //Wait a few seconds before we start moving.
Szilard 2:f4d6c9ba05d0 89 uLCD.cls();
Szilard 3:905643e72bcd 90 uLCD.locate(1,2);
Szilard 3:905643e72bcd 91 uLCD.printf("I go straight!");
aberk 0:be99ed42340d 92
aberk 0:be99ed42340d 93 //Velocity to mantain in pulses per second.
aberk 0:be99ed42340d 94 leftPid.setSetPoint(1000);
Szilard 3:905643e72bcd 95 rightPid.setSetPoint(1000);
aberk 0:be99ed42340d 96
Szilard 3:905643e72bcd 97 while (1) {
Szilard 3:905643e72bcd 98
Szilard 3:905643e72bcd 99 if (distTransform(ain)>50) { //going straight line
Szilard 3:905643e72bcd 100 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 101 leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
Szilard 3:905643e72bcd 102 leftPrevPulses = leftActPulses;
Szilard 3:905643e72bcd 103 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 104 rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
Szilard 3:905643e72bcd 105 rightPrevPulses = rightActPulses;
Szilard 3:905643e72bcd 106
Szilard 3:905643e72bcd 107 leftPid.setProcessValue(fabs(leftVelocity));
Szilard 3:905643e72bcd 108 leftMotor.speed(leftPid.compute());
Szilard 3:905643e72bcd 109 rightPid.setProcessValue(fabs(rightVelocity));
Szilard 3:905643e72bcd 110 rightMotor.speed(rightPid.compute());
Szilard 2:f4d6c9ba05d0 111
Szilard 3:905643e72bcd 112 } else { //Don't go straight, turn!
Szilard 3:905643e72bcd 113 leftMotor.stop(0.5);
Szilard 3:905643e72bcd 114 rightMotor.stop(0.5);
Szilard 3:905643e72bcd 115 led2=1;
Szilard 3:905643e72bcd 116 uLCD.locate(1,2);
Szilard 3:905643e72bcd 117 uLCD.printf("I'm turning!");
Szilard 3:905643e72bcd 118 wait(0.5);
Szilard 3:905643e72bcd 119 leftPid.setSetPoint(-500);
Szilard 3:905643e72bcd 120 rightPid.setSetPoint(500);
Szilard 3:905643e72bcd 121
Szilard 3:905643e72bcd 122 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 123 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 124 distance=leftActPulses+100;
Szilard 3:905643e72bcd 125 while (leftActPulses<distance) { //I'm turning!
Szilard 3:905643e72bcd 126 leftMotor.speed(-0.5);
Szilard 3:905643e72bcd 127 rightMotor.speed(0.5);
Szilard 3:905643e72bcd 128 leftActPulses=leftPulses.read();
Szilard 3:905643e72bcd 129 rightActPulses=rightPulses.read();
Szilard 3:905643e72bcd 130
Szilard 3:905643e72bcd 131 wait(0.005);
Szilard 3:905643e72bcd 132
Szilard 3:905643e72bcd 133 }//Turning while end
Szilard 3:905643e72bcd 134 leftMotor.stop(0.5);
Szilard 3:905643e72bcd 135 rightMotor.stop(0.5);
Szilard 3:905643e72bcd 136 wait(0.5);
Szilard 3:905643e72bcd 137 led2=0;
Szilard 3:905643e72bcd 138 uLCD.cls();
Szilard 3:905643e72bcd 139 uLCD.locate(1,2);
Szilard 3:905643e72bcd 140 uLCD.printf("I go straight!");
Szilard 3:905643e72bcd 141
Szilard 3:905643e72bcd 142 leftPid.setSetPoint(1000);
Szilard 3:905643e72bcd 143 rightPid.setSetPoint(1000);
Szilard 1:c70bc01ebfdd 144
Szilard 3:905643e72bcd 145 } //Going straight/turning if end
Szilard 2:f4d6c9ba05d0 146
Szilard 3:905643e72bcd 147 //pc.printf("\n%i", distTransform(ain));
Szilard 3:905643e72bcd 148 //uLCD.locate(1,1);
Szilard 3:905643e72bcd 149 //uLCD.printf("Distance: %i cm", distTransform(ain));
Szilard 3:905643e72bcd 150 wait(0.01);
Szilard 1:c70bc01ebfdd 151 led=!led;
Szilard 1:c70bc01ebfdd 152
Szilard 3:905643e72bcd 153 } //end of big while loop
aberk 0:be99ed42340d 154
aberk 0:be99ed42340d 155 }