Marcus Pereira / Mbed 2 deprecated FinalProject_Mutex

Leader Robot

Dependencies:   Motor PID Servo mbed-rtos mbed

main.cpp@0:35cf52223527, 2016-12-11 (annotated)

Committer:
mpereira30
Date:
Sun Dec 11 13:37:12 2016 +0000
Revision:
0:35cf52223527
Final Project Leader Robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mpereira30 0:35cf52223527 1 #include "mbed.h"
mpereira30 0:35cf52223527 2 #include "Servo.h"
mpereira30 0:35cf52223527 3 #include "Motor.h"
mpereira30 0:35cf52223527 4 #include "PID.h"
mpereira30 0:35cf52223527 5 #include "rtos.h"
mpereira30 0:35cf52223527 6 #include <stdlib.h>
mpereira30 0:35cf52223527 7
mpereira30 0:35cf52223527 8 #define PI 3.1415926
mpereira30 0:35cf52223527 9 #define threshold 27
mpereira30 0:35cf52223527 10
mpereira30 0:35cf52223527 11
mpereira30 0:35cf52223527 12 //objects
mpereira30 0:35cf52223527 13 Motor rightMotor(p24, p28, p27); // pwm, fwd(in1), rev(in2) Motor B (right wheel)
mpereira30 0:35cf52223527 14 Motor leftMotor(p22, p29, p30); // pwm, fwd(in1), rev(in2) Motor A (left wheel)
mpereira30 0:35cf52223527 15 Timer timer;
mpereira30 0:35cf52223527 16 AnalogIn ain(p20);
mpereira30 0:35cf52223527 17 AnalogIn ainL(p18);
mpereira30 0:35cf52223527 18 AnalogIn ainR(p19);
mpereira30 0:35cf52223527 19 Servo myservo(p26);
mpereira30 0:35cf52223527 20 Serial pc(USBTX, USBRX);
mpereira30 0:35cf52223527 21 Serial xbee1(p13, p14);
mpereira30 0:35cf52223527 22 Mutex wheelMutex;
mpereira30 0:35cf52223527 23
mpereira30 0:35cf52223527 24
mpereira30 0:35cf52223527 25 //globals
mpereira30 0:35cf52223527 26 float max_angle = 0.0;
mpereira30 0:35cf52223527 27 int leftPrevPulses = 0;
mpereira30 0:35cf52223527 28 int leftActPulses=0; //pulses from left motor
mpereira30 0:35cf52223527 29 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per second
mpereira30 0:35cf52223527 30 int rightPrevPulses = 0;
mpereira30 0:35cf52223527 31 int rightActPulses=0; //pulses from right motor
mpereira30 0:35cf52223527 32 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
mpereira30 0:35cf52223527 33 int turnDistance = 0; //Number of pulses to travel.
mpereira30 0:35cf52223527 34 int distanceR = 0; // in mm
mpereira30 0:35cf52223527 35 int distanceL = 0; // in mm
mpereira30 0:35cf52223527 36 int distanceRold = 0; // in mm
mpereira30 0:35cf52223527 37 int distanceLold = 0; // in mm
mpereira30 0:35cf52223527 38 float X = 0.0;
mpereira30 0:35cf52223527 39 float Y = 0.0;
mpereira30 0:35cf52223527 40 float inc_X = 1.0;
mpereira30 0:35cf52223527 41 float inc_Y = 0.0;
mpereira30 0:35cf52223527 42 float cur_dir = 0.0;
mpereira30 0:35cf52223527 43 bool go;
mpereira30 0:35cf52223527 44 int DistancesR[4];
mpereira30 0:35cf52223527 45 int DistancesL[4];
mpereira30 0:35cf52223527 46
mpereira30 0:35cf52223527 47
mpereira30 0:35cf52223527 48 //functions
mpereira30 0:35cf52223527 49 void lookaround(void);
mpereira30 0:35cf52223527 50 void go_fwd(void);
mpereira30 0:35cf52223527 51 void turn_R(float);
mpereira30 0:35cf52223527 52 void turn_L(float);
mpereira30 0:35cf52223527 53 void stop(void);
mpereira30 0:35cf52223527 54 void distR(int);
mpereira30 0:35cf52223527 55 void distL(int);
mpereira30 0:35cf52223527 56 int distTransform(float input);
mpereira30 0:35cf52223527 57 void tunePID(void);
mpereira30 0:35cf52223527 58 float find_avg(float a[5]);
mpereira30 0:35cf52223527 59 float to_Rad(float);
mpereira30 0:35cf52223527 60 void go_fwd_thread(void const *argument);
mpereira30 0:35cf52223527 61 void grab_init_direct(void);
mpereira30 0:35cf52223527 62
mpereira30 0:35cf52223527 63 class Counter { //interrupt driven rotation counter to be used with the feedback control
mpereira30 0:35cf52223527 64 public:
mpereira30 0:35cf52223527 65 Counter(PinName pin) : _interrupt(pin) { // create the InterruptIn on the pin specified to Counter
mpereira30 0:35cf52223527 66 _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
mpereira30 0:35cf52223527 67 }
mpereira30 0:35cf52223527 68
mpereira30 0:35cf52223527 69 void increment() {
mpereira30 0:35cf52223527 70 _count++;
mpereira30 0:35cf52223527 71 }
mpereira30 0:35cf52223527 72
mpereira30 0:35cf52223527 73 int read() {
mpereira30 0:35cf52223527 74 return _count;
mpereira30 0:35cf52223527 75 }
mpereira30 0:35cf52223527 76
mpereira30 0:35cf52223527 77 private:
mpereira30 0:35cf52223527 78 InterruptIn _interrupt;
mpereira30 0:35cf52223527 79 volatile int _count;
mpereira30 0:35cf52223527 80 };
mpereira30 0:35cf52223527 81
mpereira30 0:35cf52223527 82 Counter leftPulses(p9), rightPulses (p10);
mpereira30 0:35cf52223527 83 //PID leftPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
mpereira30 0:35cf52223527 84 //PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
mpereira30 0:35cf52223527 85
mpereira30 0:35cf52223527 86 PID leftPid(0.462, 0.1, 0.2, 0.01); //Kc, Ti, Td
mpereira30 0:35cf52223527 87 PID rightPid(0.462, 0.1, 0.1, 0.01); //Kc, Ti, Td
mpereira30 0:35cf52223527 88
mpereira30 0:35cf52223527 89 int main()
mpereira30 0:35cf52223527 90 {
mpereira30 0:35cf52223527 91 tunePID();
mpereira30 0:35cf52223527 92 xbee1.baud(115200);
mpereira30 0:35cf52223527 93 myservo = 0.65;
mpereira30 0:35cf52223527 94 leftPid.setSetPoint(975); //set velocity goals for PID
mpereira30 0:35cf52223527 95 rightPid.setSetPoint(990);
mpereira30 0:35cf52223527 96 Thread fwdThread(go_fwd_thread);
mpereira30 0:35cf52223527 97 grab_init_direct();
mpereira30 0:35cf52223527 98 go = true;
mpereira30 0:35cf52223527 99 float middle[5];
mpereira30 0:35cf52223527 100 float left[5];
mpereira30 0:35cf52223527 101 float right[5];
mpereira30 0:35cf52223527 102
mpereira30 0:35cf52223527 103 while(1){
mpereira30 0:35cf52223527 104 float mid_avg = 0.0;
mpereira30 0:35cf52223527 105 float left_avg = 0.0;
mpereira30 0:35cf52223527 106 float right_avg = 0.0;
mpereira30 0:35cf52223527 107 timer.start();
mpereira30 0:35cf52223527 108 while (mid_avg <0.4 && left_avg <0.7 && right_avg <0.4)
mpereira30 0:35cf52223527 109 {
mpereira30 0:35cf52223527 110 middle[0] = ain;
mpereira30 0:35cf52223527 111 middle[1] = ain;
mpereira30 0:35cf52223527 112 middle[2] = ain;
mpereira30 0:35cf52223527 113 middle[3] = ain;
mpereira30 0:35cf52223527 114 middle[4] = ain;
mpereira30 0:35cf52223527 115 mid_avg = find_avg(middle);
mpereira30 0:35cf52223527 116
mpereira30 0:35cf52223527 117 left[0] = ainL;
mpereira30 0:35cf52223527 118 left[1] = ainL;
mpereira30 0:35cf52223527 119 left[2] = ainL;
mpereira30 0:35cf52223527 120 left[3] = ainL;
mpereira30 0:35cf52223527 121 left[4] = ainL;
mpereira30 0:35cf52223527 122 left_avg = find_avg(left);
mpereira30 0:35cf52223527 123
mpereira30 0:35cf52223527 124 right[0] = ainR;
mpereira30 0:35cf52223527 125 right[1] = ainR;
mpereira30 0:35cf52223527 126 right[2] = ainR;
mpereira30 0:35cf52223527 127 right[3] = ainR;
mpereira30 0:35cf52223527 128 right[4] = ainR;
mpereira30 0:35cf52223527 129 right_avg = find_avg(right);
mpereira30 0:35cf52223527 130
mpereira30 0:35cf52223527 131 X = X + inc_X;
mpereira30 0:35cf52223527 132 Y = Y + inc_Y;
mpereira30 0:35cf52223527 133 xbee1.printf("+\n");
mpereira30 0:35cf52223527 134 xbee1.printf("%.2f\n%.2f\n" ,X ,Y);
mpereira30 0:35cf52223527 135 }
mpereira30 0:35cf52223527 136 go = false;
mpereira30 0:35cf52223527 137 stop();
mpereira30 0:35cf52223527 138 xbee1.printf("*\n");
mpereira30 0:35cf52223527 139 xbee1.printf("%4d",timer.read_ms());
mpereira30 0:35cf52223527 140 timer.stop();
mpereira30 0:35cf52223527 141 timer.reset();
mpereira30 0:35cf52223527 142 lookaround();
mpereira30 0:35cf52223527 143 //fwdThread.signal_set(0x1);
mpereira30 0:35cf52223527 144
mpereira30 0:35cf52223527 145 if (max_angle >= 45.0)
mpereira30 0:35cf52223527 146 {
mpereira30 0:35cf52223527 147 turn_R((max_angle - 45.0));
mpereira30 0:35cf52223527 148 cur_dir = cur_dir - (max_angle - 45.0);
mpereira30 0:35cf52223527 149 inc_X = cos(to_Rad(cur_dir));
mpereira30 0:35cf52223527 150 inc_Y = sin(to_Rad(cur_dir));
mpereira30 0:35cf52223527 151 xbee1.printf("R\n");
mpereira30 0:35cf52223527 152 xbee1.printf("%2.1f",(max_angle - 45.0));
mpereira30 0:35cf52223527 153 }
mpereira30 0:35cf52223527 154 else if (max_angle < 45.0)
mpereira30 0:35cf52223527 155 {
mpereira30 0:35cf52223527 156 turn_L((45.0 - max_angle));
mpereira30 0:35cf52223527 157 cur_dir = cur_dir + (45.0 - max_angle);
mpereira30 0:35cf52223527 158 inc_X = cos(to_Rad(cur_dir));
mpereira30 0:35cf52223527 159 inc_Y = sin(to_Rad(cur_dir));
mpereira30 0:35cf52223527 160 xbee1.printf("L\n");
mpereira30 0:35cf52223527 161 xbee1.printf("%2.1f",(45.0 - max_angle));
mpereira30 0:35cf52223527 162 }
mpereira30 0:35cf52223527 163
mpereira30 0:35cf52223527 164 myservo = 0.65;
mpereira30 0:35cf52223527 165 stop();
mpereira30 0:35cf52223527 166 wait(0.1);
mpereira30 0:35cf52223527 167 go = true;
mpereira30 0:35cf52223527 168
mpereira30 0:35cf52223527 169 } // End of while(1)loop - main thread
mpereira30 0:35cf52223527 170 }// End of main()
mpereira30 0:35cf52223527 171
mpereira30 0:35cf52223527 172 void grab_init_direct(void)
mpereira30 0:35cf52223527 173 {
mpereira30 0:35cf52223527 174 char initdirect[4];
mpereira30 0:35cf52223527 175 int Count = 0;
mpereira30 0:35cf52223527 176 initdirect[Count] = xbee1.getc();
mpereira30 0:35cf52223527 177 while(initdirect[Count] != '+')
mpereira30 0:35cf52223527 178 {
mpereira30 0:35cf52223527 179 Count++;
mpereira30 0:35cf52223527 180 initdirect[Count] = xbee1.getc();
mpereira30 0:35cf52223527 181 }
mpereira30 0:35cf52223527 182
mpereira30 0:35cf52223527 183 char initdirect2[Count];
mpereira30 0:35cf52223527 184 for(int i=0; i<Count; i++)
mpereira30 0:35cf52223527 185 {
mpereira30 0:35cf52223527 186 initdirect2[i] = initdirect[i];
mpereira30 0:35cf52223527 187 }
mpereira30 0:35cf52223527 188
mpereira30 0:35cf52223527 189 float init_turn = atof(initdirect2);
mpereira30 0:35cf52223527 190 cur_dir = init_turn;
mpereira30 0:35cf52223527 191 turn_R(init_turn);
mpereira30 0:35cf52223527 192
mpereira30 0:35cf52223527 193 }
mpereira30 0:35cf52223527 194
mpereira30 0:35cf52223527 195 float find_avg(float a[5])
mpereira30 0:35cf52223527 196 {
mpereira30 0:35cf52223527 197 return ((a[0] + a[1] + a[2] + a[3] + a[4])/5.0);
mpereira30 0:35cf52223527 198 }
mpereira30 0:35cf52223527 199
mpereira30 0:35cf52223527 200 void tunePID(void){
mpereira30 0:35cf52223527 201 leftPid.setInputLimits(0, 3000);
mpereira30 0:35cf52223527 202 leftPid.setOutputLimits(0.0, 0.5);
mpereira30 0:35cf52223527 203 leftPid.setMode(AUTO_MODE);
mpereira30 0:35cf52223527 204 rightPid.setInputLimits(0, 3000);
mpereira30 0:35cf52223527 205 //rightPid.setOutputLimits(0.0, 0.465);
mpereira30 0:35cf52223527 206 rightPid.setOutputLimits(0.0, 0.48);
mpereira30 0:35cf52223527 207 rightPid.setMode(AUTO_MODE);
mpereira30 0:35cf52223527 208 }
mpereira30 0:35cf52223527 209
mpereira30 0:35cf52223527 210
mpereira30 0:35cf52223527 211 void stop(void)
mpereira30 0:35cf52223527 212 {
mpereira30 0:35cf52223527 213 wheelMutex.lock();
mpereira30 0:35cf52223527 214
mpereira30 0:35cf52223527 215 leftMotor.speed(0.0);
mpereira30 0:35cf52223527 216 rightMotor.speed(0.0);
mpereira30 0:35cf52223527 217
mpereira30 0:35cf52223527 218 wheelMutex.unlock();
mpereira30 0:35cf52223527 219 }
mpereira30 0:35cf52223527 220
mpereira30 0:35cf52223527 221
mpereira30 0:35cf52223527 222 void turn_R(float deg)
mpereira30 0:35cf52223527 223 {
mpereira30 0:35cf52223527 224 wheelMutex.lock();
mpereira30 0:35cf52223527 225
mpereira30 0:35cf52223527 226 leftActPulses=leftPulses.read();
mpereira30 0:35cf52223527 227 rightActPulses=rightPulses.read();
mpereira30 0:35cf52223527 228 turnDistance=rightActPulses+(int)deg;
mpereira30 0:35cf52223527 229 while (rightActPulses<turnDistance) { //turn approximately half a revolution
mpereira30 0:35cf52223527 230 leftMotor.speed(0.4); //rotate to the right
mpereira30 0:35cf52223527 231 rightMotor.speed(-0.4); //open loop, because the PID class can't handle negative values
mpereira30 0:35cf52223527 232 leftActPulses=leftPulses.read();
mpereira30 0:35cf52223527 233 rightActPulses=rightPulses.read();
mpereira30 0:35cf52223527 234 wait(0.005);
mpereira30 0:35cf52223527 235 }
mpereira30 0:35cf52223527 236
mpereira30 0:35cf52223527 237 wheelMutex.unlock();
mpereira30 0:35cf52223527 238 }
mpereira30 0:35cf52223527 239
mpereira30 0:35cf52223527 240 void turn_L(float deg)
mpereira30 0:35cf52223527 241 {
mpereira30 0:35cf52223527 242
mpereira30 0:35cf52223527 243 wheelMutex.lock();
mpereira30 0:35cf52223527 244
mpereira30 0:35cf52223527 245 leftActPulses=leftPulses.read();
mpereira30 0:35cf52223527 246 rightActPulses=rightPulses.read();
mpereira30 0:35cf52223527 247 turnDistance=leftActPulses+(int)deg;
mpereira30 0:35cf52223527 248 while (leftActPulses<turnDistance) { //turn approximately half a revolution
mpereira30 0:35cf52223527 249 leftMotor.speed(-0.4); //rotate to the right
mpereira30 0:35cf52223527 250 rightMotor.speed(0.4); //open loop, because the PID class can't handle negative values
mpereira30 0:35cf52223527 251 leftActPulses=leftPulses.read();
mpereira30 0:35cf52223527 252 rightActPulses=rightPulses.read();
mpereira30 0:35cf52223527 253 wait(0.005);
mpereira30 0:35cf52223527 254 }
mpereira30 0:35cf52223527 255
mpereira30 0:35cf52223527 256 wheelMutex.unlock();
mpereira30 0:35cf52223527 257 }
mpereira30 0:35cf52223527 258
mpereira30 0:35cf52223527 259 void lookaround(void) //returns a value between 0 and 90 that represents the best path in the cone of observation
mpereira30 0:35cf52223527 260 {
mpereira30 0:35cf52223527 261 float max;
mpereira30 0:35cf52223527 262 max_angle = 0.0;
mpereira30 0:35cf52223527 263 max = ain;
mpereira30 0:35cf52223527 264 for(float p=0; p<1.0; p += 0.05)
mpereira30 0:35cf52223527 265 {
mpereira30 0:35cf52223527 266 myservo = p;
mpereira30 0:35cf52223527 267 if (max > ain)
mpereira30 0:35cf52223527 268 {
mpereira30 0:35cf52223527 269 max = ain;
mpereira30 0:35cf52223527 270 max_angle = p;
mpereira30 0:35cf52223527 271 }
mpereira30 0:35cf52223527 272
mpereira30 0:35cf52223527 273 //wait(0.25);
mpereira30 0:35cf52223527 274 wait(0.1);
mpereira30 0:35cf52223527 275 }
mpereira30 0:35cf52223527 276
mpereira30 0:35cf52223527 277 if (max>0.45){
mpereira30 0:35cf52223527 278 turn_L(180.0);
mpereira30 0:35cf52223527 279 myservo = 0.65;
mpereira30 0:35cf52223527 280 wait(0.5);
mpereira30 0:35cf52223527 281 }
mpereira30 0:35cf52223527 282 else{
mpereira30 0:35cf52223527 283 myservo = max_angle;
mpereira30 0:35cf52223527 284 wait(0.5);
mpereira30 0:35cf52223527 285 max_angle = max_angle*90.0 - 13.5;
mpereira30 0:35cf52223527 286 //max_angle = max_angle*90.0;
mpereira30 0:35cf52223527 287 }
mpereira30 0:35cf52223527 288
mpereira30 0:35cf52223527 289 }
mpereira30 0:35cf52223527 290
mpereira30 0:35cf52223527 291 float to_Rad(float deg)
mpereira30 0:35cf52223527 292 {
mpereira30 0:35cf52223527 293 return deg * ((PI)/180);
mpereira30 0:35cf52223527 294 }
mpereira30 0:35cf52223527 295
mpereira30 0:35cf52223527 296 void go_fwd_thread(void const *argument) {
mpereira30 0:35cf52223527 297 while(1){
mpereira30 0:35cf52223527 298 if(!go) {Thread::yield();}
mpereira30 0:35cf52223527 299 while (go) {
mpereira30 0:35cf52223527 300 wheelMutex.lock();
mpereira30 0:35cf52223527 301
mpereira30 0:35cf52223527 302 leftActPulses=leftPulses.read();
mpereira30 0:35cf52223527 303 leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
mpereira30 0:35cf52223527 304 leftPrevPulses = leftActPulses;
mpereira30 0:35cf52223527 305 rightActPulses=rightPulses.read();
mpereira30 0:35cf52223527 306 rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
mpereira30 0:35cf52223527 307 rightPrevPulses = rightActPulses;
mpereira30 0:35cf52223527 308 leftPid.setProcessValue(fabs(leftVelocity));
mpereira30 0:35cf52223527 309 leftMotor.speed(leftPid.compute()+0.0075);
mpereira30 0:35cf52223527 310 rightPid.setProcessValue(fabs(rightVelocity));
mpereira30 0:35cf52223527 311 rightMotor.speed(rightPid.compute());
mpereira30 0:35cf52223527 312
mpereira30 0:35cf52223527 313 wheelMutex.unlock();
mpereira30 0:35cf52223527 314 }
mpereira30 0:35cf52223527 315 }
mpereira30 0:35cf52223527 316 }