Marcus Pereira / Mbed 2 deprecated FollowerBot_Mutex

Follower Robot

Dependencies:   Motor2 PID mbed-rtos mbed

main.cpp@0:4d45673148c2, 2016-12-11 (annotated)

Committer:
mpereira30
Date:
Sun Dec 11 13:47:17 2016 +0000
Revision:
0:4d45673148c2
Follower Bot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mpereira30 0:4d45673148c2 1 #include "mbed.h"
mpereira30 0:4d45673148c2 2 #include "Motor.h"
mpereira30 0:4d45673148c2 3 #include "PID.h"
mpereira30 0:4d45673148c2 4 #include <stdlib.h>
mpereira30 0:4d45673148c2 5 #include "rtos.h"
mpereira30 0:4d45673148c2 6
mpereira30 0:4d45673148c2 7 #define PI 3.1415926
mpereira30 0:4d45673148c2 8 #define threshold 27
mpereira30 0:4d45673148c2 9 //objects:
mpereira30 0:4d45673148c2 10 Motor rightMotor(p24, p28, p27); // pwm, fwd(in1), rev(in2) Motor B (right wheel)
mpereira30 0:4d45673148c2 11 Motor leftMotor(p22, p29, p30); // pwm, fwd(in1), rev(in2) Motor A (left wheel)
mpereira30 0:4d45673148c2 12 Timer timer;
mpereira30 0:4d45673148c2 13 Serial xbee(p13, p14);
mpereira30 0:4d45673148c2 14 Serial pc(USBTX,USBRX);
mpereira30 0:4d45673148c2 15 Mutex wheelMutex;
mpereira30 0:4d45673148c2 16 Mutex timermutex;
mpereira30 0:4d45673148c2 17
mpereira30 0:4d45673148c2 18 //functions
mpereira30 0:4d45673148c2 19 void go_fwd(float);
mpereira30 0:4d45673148c2 20 void turn_R(float);
mpereira30 0:4d45673148c2 21 void turn_L(float);
mpereira30 0:4d45673148c2 22 void stop(float);
mpereira30 0:4d45673148c2 23 void tunePID(void);
mpereira30 0:4d45673148c2 24 void go_fwd_thread(void const *argument);
mpereira30 0:4d45673148c2 25
mpereira30 0:4d45673148c2 26 //globals:
mpereira30 0:4d45673148c2 27 int leftPrevPulses = 0;
mpereira30 0:4d45673148c2 28 int leftActPulses=0; //pulses from left motor
mpereira30 0:4d45673148c2 29 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per second
mpereira30 0:4d45673148c2 30 int rightPrevPulses = 0;
mpereira30 0:4d45673148c2 31 int rightActPulses=0; //pulses from right motor
mpereira30 0:4d45673148c2 32 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
mpereira30 0:4d45673148c2 33 int turnDistance = 0; //Number of pulses to travel.
mpereira30 0:4d45673148c2 34 int distanceR = 0; // in mm
mpereira30 0:4d45673148c2 35 int distanceL = 0; // in mm
mpereira30 0:4d45673148c2 36 int distanceRold = 0; // in mm
mpereira30 0:4d45673148c2 37 int distanceLold = 0; // in mm
mpereira30 0:4d45673148c2 38 bool go;
mpereira30 0:4d45673148c2 39 float go_time = 0.0;
mpereira30 0:4d45673148c2 40
mpereira30 0:4d45673148c2 41 class Counter { //interrupt driven rotation counter to be used with the feedback control
mpereira30 0:4d45673148c2 42 public:
mpereira30 0:4d45673148c2 43 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
mpereira30 0:4d45673148c2 44 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
mpereira30 0:4d45673148c2 45 }
mpereira30 0:4d45673148c2 46
mpereira30 0:4d45673148c2 47 void increment() {
mpereira30 0:4d45673148c2 48 _count++;
mpereira30 0:4d45673148c2 49 }
mpereira30 0:4d45673148c2 50
mpereira30 0:4d45673148c2 51 int read() {
mpereira30 0:4d45673148c2 52 return _count;
mpereira30 0:4d45673148c2 53 }
mpereira30 0:4d45673148c2 54
mpereira30 0:4d45673148c2 55 private:
mpereira30 0:4d45673148c2 56 InterruptIn _interrupt;
mpereira30 0:4d45673148c2 57 volatile int _count;
mpereira30 0:4d45673148c2 58 };
mpereira30 0:4d45673148c2 59
mpereira30 0:4d45673148c2 60
mpereira30 0:4d45673148c2 61 Counter leftPulses(p9), rightPulses (p10);
mpereira30 0:4d45673148c2 62 PID leftPid(0.4620, 0.1, 0.1, 0.01); //Kc, Ti, Td
mpereira30 0:4d45673148c2 63 PID rightPid(0.4620, 0.1, 0.1, 0.01); //Kc, Ti, Td
mpereira30 0:4d45673148c2 64 //PID rightPid(0.515, 0.1, 0.0, 0.01); //Kc, Ti, Td
mpereira30 0:4d45673148c2 65
mpereira30 0:4d45673148c2 66 int main()
mpereira30 0:4d45673148c2 67 {
mpereira30 0:4d45673148c2 68 tunePID();
mpereira30 0:4d45673148c2 69 xbee.baud(19200);
mpereira30 0:4d45673148c2 70 leftPid.setSetPoint(975); //set velocity goals for PID
mpereira30 0:4d45673148c2 71 rightPid.setSetPoint(990);
mpereira30 0:4d45673148c2 72 wait(1);
mpereira30 0:4d45673148c2 73 Thread fwdThread(go_fwd_thread);
mpereira30 0:4d45673148c2 74 go = false;
mpereira30 0:4d45673148c2 75 char tag;
mpereira30 0:4d45673148c2 76 char turnDirectionBuf[5];
mpereira30 0:4d45673148c2 77 float turnDirection;
mpereira30 0:4d45673148c2 78 char distToTravelBuf[5];
mpereira30 0:4d45673148c2 79 float distToTravel;
mpereira30 0:4d45673148c2 80 while(1)
mpereira30 0:4d45673148c2 81 {
mpereira30 0:4d45673148c2 82 tag = xbee.getc();
mpereira30 0:4d45673148c2 83
mpereira30 0:4d45673148c2 84 if(tag == '*')
mpereira30 0:4d45673148c2 85 {
mpereira30 0:4d45673148c2 86 //pc.printf("tag=%c\r\n",tag);
mpereira30 0:4d45673148c2 87 distToTravelBuf[0] = xbee.getc();
mpereira30 0:4d45673148c2 88 distToTravelBuf[1] = xbee.getc();
mpereira30 0:4d45673148c2 89 distToTravelBuf[2] = xbee.getc();
mpereira30 0:4d45673148c2 90 distToTravelBuf[3] = xbee.getc();
mpereira30 0:4d45673148c2 91 distToTravelBuf[4] = xbee.getc();
mpereira30 0:4d45673148c2 92 //pc.printf("dist = %s\r\n",distToTravelBuf);
mpereira30 0:4d45673148c2 93 distToTravel = atof(distToTravelBuf);
mpereira30 0:4d45673148c2 94 //pc.printf("traveldist=%f\r\n",distToTravel);
mpereira30 0:4d45673148c2 95 //go_fwd(distToTravel);
mpereira30 0:4d45673148c2 96 timermutex.lock();
mpereira30 0:4d45673148c2 97 go_time = distToTravel;
mpereira30 0:4d45673148c2 98 timermutex.unlock();
mpereira30 0:4d45673148c2 99 go = true;
mpereira30 0:4d45673148c2 100
mpereira30 0:4d45673148c2 101 }
mpereira30 0:4d45673148c2 102 else if(tag == 'R')
mpereira30 0:4d45673148c2 103 {
mpereira30 0:4d45673148c2 104 //pc.printf("tag=%c\r\n",tag);
mpereira30 0:4d45673148c2 105 turnDirectionBuf[0] = xbee.getc();
mpereira30 0:4d45673148c2 106 turnDirectionBuf[1] = xbee.getc();
mpereira30 0:4d45673148c2 107 turnDirectionBuf[2] = xbee.getc();
mpereira30 0:4d45673148c2 108 turnDirectionBuf[3] = xbee.getc();
mpereira30 0:4d45673148c2 109 turnDirectionBuf[4] = xbee.getc();
mpereira30 0:4d45673148c2 110 //pc.printf("turn right %s\n\r",turnDirectionBuf);
mpereira30 0:4d45673148c2 111 turnDirection = atof(turnDirectionBuf);
mpereira30 0:4d45673148c2 112 //pc.printf("turn right %f\n\r",turnDirection);
mpereira30 0:4d45673148c2 113 stop(1);
mpereira30 0:4d45673148c2 114 turn_R(turnDirection);
mpereira30 0:4d45673148c2 115 }
mpereira30 0:4d45673148c2 116 else if(tag == 'L')
mpereira30 0:4d45673148c2 117 {
mpereira30 0:4d45673148c2 118 //pc.printf("tag=%c\r\n",tag);
mpereira30 0:4d45673148c2 119 turnDirectionBuf[0] = xbee.getc();
mpereira30 0:4d45673148c2 120 turnDirectionBuf[1] = xbee.getc();
mpereira30 0:4d45673148c2 121 turnDirectionBuf[2] = xbee.getc();
mpereira30 0:4d45673148c2 122 turnDirectionBuf[3] = xbee.getc();
mpereira30 0:4d45673148c2 123 turnDirectionBuf[4] = xbee.getc();
mpereira30 0:4d45673148c2 124 //pc.printf("turn left %s\n\r",turnDirectionBuf);
mpereira30 0:4d45673148c2 125 turnDirection = atof(turnDirectionBuf);
mpereira30 0:4d45673148c2 126 //pc.printf("turn left %f\n\r",turnDirection);
mpereira30 0:4d45673148c2 127 stop(1);
mpereira30 0:4d45673148c2 128 turn_L(turnDirection);
mpereira30 0:4d45673148c2 129 }
mpereira30 0:4d45673148c2 130 }
mpereira30 0:4d45673148c2 131 }
mpereira30 0:4d45673148c2 132
mpereira30 0:4d45673148c2 133 void tunePID(void){
mpereira30 0:4d45673148c2 134 leftPid.setInputLimits(0, 3000);
mpereira30 0:4d45673148c2 135 leftPid.setOutputLimits(0.0, 0.4);
mpereira30 0:4d45673148c2 136 leftPid.setMode(AUTO_MODE);
mpereira30 0:4d45673148c2 137 rightPid.setInputLimits(0, 3000);
mpereira30 0:4d45673148c2 138 rightPid.setOutputLimits(0.0, 0.4);
mpereira30 0:4d45673148c2 139 rightPid.setMode(AUTO_MODE);
mpereira30 0:4d45673148c2 140 }
mpereira30 0:4d45673148c2 141
mpereira30 0:4d45673148c2 142 //void go_fwd(float time)
mpereira30 0:4d45673148c2 143 //{
mpereira30 0:4d45673148c2 144 void go_fwd_thread(void const *argument) {
mpereira30 0:4d45673148c2 145 while(1){
mpereira30 0:4d45673148c2 146 if(!go) {Thread::yield();}
mpereira30 0:4d45673148c2 147 while (go) {
mpereira30 0:4d45673148c2 148 wheelMutex.lock();
mpereira30 0:4d45673148c2 149
mpereira30 0:4d45673148c2 150 timer.start();
mpereira30 0:4d45673148c2 151 while(timer.read_ms() < go_time){
mpereira30 0:4d45673148c2 152 leftActPulses=leftPulses.read();
mpereira30 0:4d45673148c2 153 leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
mpereira30 0:4d45673148c2 154 leftPrevPulses = leftActPulses;
mpereira30 0:4d45673148c2 155 rightActPulses=rightPulses.read();
mpereira30 0:4d45673148c2 156 rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
mpereira30 0:4d45673148c2 157 rightPrevPulses = rightActPulses;
mpereira30 0:4d45673148c2 158 leftPid.setProcessValue(fabs(leftVelocity));
mpereira30 0:4d45673148c2 159 leftMotor.speed(leftPid.compute());
mpereira30 0:4d45673148c2 160 rightPid.setProcessValue(fabs(rightVelocity));
mpereira30 0:4d45673148c2 161 rightMotor.speed(rightPid.compute());
mpereira30 0:4d45673148c2 162 }
mpereira30 0:4d45673148c2 163 timer.stop();
mpereira30 0:4d45673148c2 164 timer.reset();
mpereira30 0:4d45673148c2 165 go = false;
mpereira30 0:4d45673148c2 166 timermutex.lock();
mpereira30 0:4d45673148c2 167 go_time = 0.0;
mpereira30 0:4d45673148c2 168 timermutex.unlock();
mpereira30 0:4d45673148c2 169 wheelMutex.unlock();
mpereira30 0:4d45673148c2 170 }
mpereira30 0:4d45673148c2 171
mpereira30 0:4d45673148c2 172 }
mpereira30 0:4d45673148c2 173 }
mpereira30 0:4d45673148c2 174
mpereira30 0:4d45673148c2 175
mpereira30 0:4d45673148c2 176 void stop(float t)
mpereira30 0:4d45673148c2 177 {
mpereira30 0:4d45673148c2 178 wheelMutex.lock();
mpereira30 0:4d45673148c2 179
mpereira30 0:4d45673148c2 180 leftMotor.speed(0.0);
mpereira30 0:4d45673148c2 181 rightMotor.speed(0.0);
mpereira30 0:4d45673148c2 182 //wait(t);
mpereira30 0:4d45673148c2 183 wheelMutex.unlock();
mpereira30 0:4d45673148c2 184 }
mpereira30 0:4d45673148c2 185
mpereira30 0:4d45673148c2 186
mpereira30 0:4d45673148c2 187
mpereira30 0:4d45673148c2 188
mpereira30 0:4d45673148c2 189 void turn_R(float deg)
mpereira30 0:4d45673148c2 190 {
mpereira30 0:4d45673148c2 191 wheelMutex.lock();
mpereira30 0:4d45673148c2 192 leftActPulses=leftPulses.read();
mpereira30 0:4d45673148c2 193 rightActPulses=rightPulses.read();
mpereira30 0:4d45673148c2 194 turnDistance=rightActPulses+(int)deg;
mpereira30 0:4d45673148c2 195 while (rightActPulses<turnDistance) { //turn approximately half a revolution
mpereira30 0:4d45673148c2 196 leftMotor.speed(0.35); //rotate to the right
mpereira30 0:4d45673148c2 197 rightMotor.speed(-0.35); //open loop, because the PID class can't handle negative values
mpereira30 0:4d45673148c2 198 leftActPulses=leftPulses.read();
mpereira30 0:4d45673148c2 199 rightActPulses=rightPulses.read();
mpereira30 0:4d45673148c2 200 wait(0.005);
mpereira30 0:4d45673148c2 201 }
mpereira30 0:4d45673148c2 202 wheelMutex.unlock();
mpereira30 0:4d45673148c2 203 stop(1);
mpereira30 0:4d45673148c2 204 }
mpereira30 0:4d45673148c2 205
mpereira30 0:4d45673148c2 206 void turn_L(float deg)
mpereira30 0:4d45673148c2 207 {
mpereira30 0:4d45673148c2 208 wheelMutex.lock();
mpereira30 0:4d45673148c2 209 leftActPulses=leftPulses.read();
mpereira30 0:4d45673148c2 210 rightActPulses=rightPulses.read();
mpereira30 0:4d45673148c2 211 turnDistance=leftActPulses+(int)deg;
mpereira30 0:4d45673148c2 212 while (leftActPulses<turnDistance) { //turn approximately half a revolution
mpereira30 0:4d45673148c2 213 leftMotor.speed(-0.35); //rotate to the right
mpereira30 0:4d45673148c2 214 rightMotor.speed(0.35); //open loop, because the PID class can't handle negative values
mpereira30 0:4d45673148c2 215 leftActPulses=leftPulses.read();
mpereira30 0:4d45673148c2 216 rightActPulses=rightPulses.read();
mpereira30 0:4d45673148c2 217 wait(0.005);
mpereira30 0:4d45673148c2 218 }
mpereira30 0:4d45673148c2 219 wheelMutex.unlock();
mpereira30 0:4d45673148c2 220 stop(1);
mpereira30 0:4d45673148c2 221 }
mpereira30 0:4d45673148c2 222
mpereira30 0:4d45673148c2 223
mpereira30 0:4d45673148c2 224