John Edwards / Mbed 2 deprecated Magician_Gesture_Controlled_Robot

Post-demo commit. Motor speeds and encoder threshold values reflect the non-linearity of our robot. Users should update motor speeds and encoder values to reflect their own robots.

Dependencies:   Magician_Motor_Test Motordriver mbed

Fork of Magician_Gesture_Controlled_Robot by John Edwards

main.cpp@1:30e01a866efa, 2015-04-22 (annotated)

Committer:
jodoedjr
Date:
Wed Apr 22 21:36:17 2015 +0000
Revision:
1:30e01a866efa
Parent:
0:5c26b3940003
Child:
2:e2b6fe03e630
First Commit. Speed and encoder presets need refining for final demo;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jodoedjr 1:30e01a866efa 1 // ECE 4180 Gesture Sensor Controlled Robot Project: Robot Comm, Heading and Motor Control, Obstacle Detection
4180_1 0:5c26b3940003 2
4180_1 0:5c26b3940003 3 #include "mbed.h"
4180_1 0:5c26b3940003 4 #include "motordriver.h"
jodoedjr 1:30e01a866efa 5
jodoedjr 1:30e01a866efa 6 //#define DEBUG
4180_1 0:5c26b3940003 7
jodoedjr 1:30e01a866efa 8 DigitalOut led1(LED1);
jodoedjr 1:30e01a866efa 9 DigitalOut led2(LED2);
jodoedjr 1:30e01a866efa 10 DigitalOut led3(LED3);
jodoedjr 1:30e01a866efa 11 DigitalOut led4(LED4);
jodoedjr 1:30e01a866efa 12 AnalogIn IR(p20);//Input for IR sensor, 0.60 seems like a good object detect distance
jodoedjr 1:30e01a866efa 13 InterruptIn REN(p15);//INTERRUPT IN PIN FOR RIGHT ENCODER
jodoedjr 1:30e01a866efa 14 InterruptIn LEN(p16);//INTERRUPT IN PIN FOR LEFT ENCODER
jodoedjr 1:30e01a866efa 15 int countR = 0;
jodoedjr 1:30e01a866efa 16 int countL = 0;
jodoedjr 1:30e01a866efa 17
jodoedjr 1:30e01a866efa 18
jodoedjr 1:30e01a866efa 19 Serial pc(USBTX, USBRX);//PC serial for debug
jodoedjr 1:30e01a866efa 20 Serial xbee1(p13, p14);//xbee serial connection
jodoedjr 1:30e01a866efa 21
jodoedjr 1:30e01a866efa 22 int count = 0; //counter for reading through debug command array;
jodoedjr 1:30e01a866efa 23 char debugcommand[] = "FRFRFRFRS";//direction commands for debug: forward, right, forward, back, forward, left, forward, stop
jodoedjr 1:30e01a866efa 24 //int num = 0;
jodoedjr 1:30e01a866efa 25 char command = 'W'; // switch case variable to be used for commands from wireless comm, default w for wait
jodoedjr 1:30e01a866efa 26 bool collision_flag = 0;
jodoedjr 1:30e01a866efa 27
4180_1 0:5c26b3940003 28 Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
4180_1 0:5c26b3940003 29 Motor right(p26, p25, p24, 1);
jodoedjr 1:30e01a866efa 30 bool x = true;
jodoedjr 1:30e01a866efa 31 //Functions for updating/reading encoder
jodoedjr 1:30e01a866efa 32 void Rcount() //count Right encoder increments
jodoedjr 1:30e01a866efa 33 {
jodoedjr 1:30e01a866efa 34 countR++;
jodoedjr 1:30e01a866efa 35 }
jodoedjr 1:30e01a866efa 36 void Lcount() //count Left encoder increments
jodoedjr 1:30e01a866efa 37 {
jodoedjr 1:30e01a866efa 38 countL++;
jodoedjr 1:30e01a866efa 39 }
jodoedjr 1:30e01a866efa 40 void clearEN() //clear the encoder values
jodoedjr 1:30e01a866efa 41 {
jodoedjr 1:30e01a866efa 42 countR = 0;
jodoedjr 1:30e01a866efa 43 countL = 0;
jodoedjr 1:30e01a866efa 44 }
4180_1 0:5c26b3940003 45
jodoedjr 1:30e01a866efa 46
jodoedjr 1:30e01a866efa 47 int main()
jodoedjr 1:30e01a866efa 48 {
jodoedjr 1:30e01a866efa 49 float sl = 0.65;//preset wheel speed left
jodoedjr 1:30e01a866efa 50 float sr = 0.65*0.92;//preset wheel speed right
jodoedjr 1:30e01a866efa 51 //************************************************************
jodoedjr 1:30e01a866efa 52
jodoedjr 1:30e01a866efa 53
jodoedjr 1:30e01a866efa 54
jodoedjr 1:30e01a866efa 55 //************************************************************
jodoedjr 1:30e01a866efa 56 REN.fall(&Rcount);
jodoedjr 1:30e01a866efa 57 LEN.fall(&Lcount);
jodoedjr 1:30e01a866efa 58
jodoedjr 1:30e01a866efa 59 led1 = 1;
jodoedjr 1:30e01a866efa 60 led2 = 1;
jodoedjr 1:30e01a866efa 61 led3 = 1;
jodoedjr 1:30e01a866efa 62 led4 = 1;
jodoedjr 1:30e01a866efa 63
jodoedjr 1:30e01a866efa 64 wait(7);
jodoedjr 1:30e01a866efa 65 led1 = 0;
jodoedjr 1:30e01a866efa 66 led2 = 0;
jodoedjr 1:30e01a866efa 67 led3 = 0;
jodoedjr 1:30e01a866efa 68 led4 = 0;
jodoedjr 1:30e01a866efa 69
jodoedjr 1:30e01a866efa 70 bool x = true;//loop variable
jodoedjr 1:30e01a866efa 71
jodoedjr 1:30e01a866efa 72
jodoedjr 1:30e01a866efa 73 while (x) {//program loop
jodoedjr 1:30e01a866efa 74
jodoedjr 1:30e01a866efa 75 //************************************************************
jodoedjr 1:30e01a866efa 76 if(IR >=0.60) {
jodoedjr 1:30e01a866efa 77 left.stop(1);
jodoedjr 1:30e01a866efa 78 right.stop(1);
jodoedjr 1:30e01a866efa 79 collision_flag = 1;
jodoedjr 1:30e01a866efa 80 }
jodoedjr 1:30e01a866efa 81 if(collision_flag) {
jodoedjr 1:30e01a866efa 82 xbee1.putc('C');
jodoedjr 1:30e01a866efa 83 pc.printf("C\n\r");
jodoedjr 1:30e01a866efa 84 collision_flag = 0;
jodoedjr 1:30e01a866efa 85 }
jodoedjr 1:30e01a866efa 86 if(xbee1.readable()) {
jodoedjr 1:30e01a866efa 87 command = xbee1.getc();
jodoedjr 1:30e01a866efa 88 pc.printf("Command: %c\n\r",command);
4180_1 0:5c26b3940003 89 }
jodoedjr 1:30e01a866efa 90
jodoedjr 1:30e01a866efa 91 //************************************************************
jodoedjr 1:30e01a866efa 92
jodoedjr 1:30e01a866efa 93 switch ( command ) {//switch statement for robot motor control
jodoedjr 1:30e01a866efa 94 case 'F': //move forward
jodoedjr 1:30e01a866efa 95 pc.printf("FORWARD\n\r");
jodoedjr 1:30e01a866efa 96 led2 = 1;
jodoedjr 1:30e01a866efa 97 led3 = 1;
jodoedjr 1:30e01a866efa 98 clearEN();
jodoedjr 1:30e01a866efa 99 while ((countL <= 500)||(countR <=340)) {//loop specified distance
jodoedjr 1:30e01a866efa 100 if(IR >=0.60) {
jodoedjr 1:30e01a866efa 101 left.stop(1);
jodoedjr 1:30e01a866efa 102 right.stop(1);
jodoedjr 1:30e01a866efa 103 collision_flag = 1;
jodoedjr 1:30e01a866efa 104 break;
jodoedjr 1:30e01a866efa 105 } //if IR has detected a close object, stop
jodoedjr 1:30e01a866efa 106 if(xbee1.readable()) {
jodoedjr 1:30e01a866efa 107 command = xbee1.getc();
jodoedjr 1:30e01a866efa 108 pc.printf("Command: %c\n\r",command);
jodoedjr 1:30e01a866efa 109 break;
jodoedjr 1:30e01a866efa 110 }
jodoedjr 1:30e01a866efa 111 left.speed(sl);
jodoedjr 1:30e01a866efa 112 right.speed(sr);//drift factor associated with our build
jodoedjr 1:30e01a866efa 113 pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
jodoedjr 1:30e01a866efa 114 wait(.01);
jodoedjr 1:30e01a866efa 115 }
jodoedjr 1:30e01a866efa 116 led2 = 0;
jodoedjr 1:30e01a866efa 117 led3 = 0;
jodoedjr 1:30e01a866efa 118 left.stop(1);
jodoedjr 1:30e01a866efa 119 right.stop(1);
jodoedjr 1:30e01a866efa 120 command = 'W';
jodoedjr 1:30e01a866efa 121 break;
jodoedjr 1:30e01a866efa 122
jodoedjr 1:30e01a866efa 123 case 'R': //turn 90 deg right
jodoedjr 1:30e01a866efa 124 pc.printf("RIGHT\n\r");
jodoedjr 1:30e01a866efa 125 led1 = 1;
jodoedjr 1:30e01a866efa 126 led2 = 1;
jodoedjr 1:30e01a866efa 127 clearEN();
jodoedjr 1:30e01a866efa 128 while ((countL <= 80)) {//loop turn for specified angle
jodoedjr 1:30e01a866efa 129 //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL);
jodoedjr 1:30e01a866efa 130 left.speed(sl);
jodoedjr 1:30e01a866efa 131 right.speed(-sr);//right.speed(-sr);
jodoedjr 1:30e01a866efa 132 if(xbee1.readable()) {
jodoedjr 1:30e01a866efa 133 command = xbee1.getc();
jodoedjr 1:30e01a866efa 134 pc.printf("Command: %c\n\r",command);
jodoedjr 1:30e01a866efa 135 break;
jodoedjr 1:30e01a866efa 136 }
jodoedjr 1:30e01a866efa 137 }
jodoedjr 1:30e01a866efa 138 led1 = 0;
jodoedjr 1:30e01a866efa 139 led2 = 0;
jodoedjr 1:30e01a866efa 140 left.stop(1);
jodoedjr 1:30e01a866efa 141 right.stop(1);
jodoedjr 1:30e01a866efa 142 command = 'W';
jodoedjr 1:30e01a866efa 143 //pc.printf("END OF COMMAND:: RIGHT: %d, LEFT: %d\n\r", countR, countL);
jodoedjr 1:30e01a866efa 144 break;
jodoedjr 1:30e01a866efa 145
jodoedjr 1:30e01a866efa 146 case 'L': //turn 90 deg left
jodoedjr 1:30e01a866efa 147 pc.printf("LEFT\n\r");
jodoedjr 1:30e01a866efa 148 led3 = 1;
jodoedjr 1:30e01a866efa 149 led4 = 1;
jodoedjr 1:30e01a866efa 150
jodoedjr 1:30e01a866efa 151 clearEN();
jodoedjr 1:30e01a866efa 152 //loop turn for specified angle
jodoedjr 1:30e01a866efa 153 while (( countR<=55)) {
jodoedjr 1:30e01a866efa 154 left.speed(-sl);
jodoedjr 1:30e01a866efa 155 right.speed(sr);
jodoedjr 1:30e01a866efa 156 if(xbee1.readable()) {
jodoedjr 1:30e01a866efa 157 command = xbee1.getc();
jodoedjr 1:30e01a866efa 158 pc.printf("Command: %c\n\r",command);
jodoedjr 1:30e01a866efa 159 break;
jodoedjr 1:30e01a866efa 160 }
jodoedjr 1:30e01a866efa 161 }
jodoedjr 1:30e01a866efa 162 led3 = 0;
jodoedjr 1:30e01a866efa 163 led4 = 0;
jodoedjr 1:30e01a866efa 164 left.stop(1);
jodoedjr 1:30e01a866efa 165 right.stop(1);
jodoedjr 1:30e01a866efa 166 command = 'W';
jodoedjr 1:30e01a866efa 167 break;
jodoedjr 1:30e01a866efa 168
jodoedjr 1:30e01a866efa 169 case 'B': //turn 180 deg left
jodoedjr 1:30e01a866efa 170 pc.printf("BACKWARDS\n\r");
jodoedjr 1:30e01a866efa 171 led1 = 1;
jodoedjr 1:30e01a866efa 172 led4 = 1;
jodoedjr 1:30e01a866efa 173 left.speed(-sl);
jodoedjr 1:30e01a866efa 174 right.speed(-sr);
jodoedjr 1:30e01a866efa 175 wait(.5);//bump robot backwards by reversing both wheels for half a second
jodoedjr 1:30e01a866efa 176
jodoedjr 1:30e01a866efa 177 led1 = 0;
jodoedjr 1:30e01a866efa 178 led4 = 0;
jodoedjr 1:30e01a866efa 179 left.stop(1);
jodoedjr 1:30e01a866efa 180 right.stop(1);
jodoedjr 1:30e01a866efa 181 command = 'W';
jodoedjr 1:30e01a866efa 182 break;
jodoedjr 1:30e01a866efa 183
jodoedjr 1:30e01a866efa 184 case 'W': //wait for a new command
jodoedjr 1:30e01a866efa 185 pc.printf("WAIT\n\r");
jodoedjr 1:30e01a866efa 186 wait(.5);
jodoedjr 1:30e01a866efa 187 // Code
jodoedjr 1:30e01a866efa 188 #ifdef DEBUG
jodoedjr 1:30e01a866efa 189 /*num = rand()%3;
jodoedjr 1:30e01a866efa 190 switch( num ) {
jodoedjr 1:30e01a866efa 191 case 1:
jodoedjr 1:30e01a866efa 192 command = 'f';
jodoedjr 1:30e01a866efa 193 break;
jodoedjr 1:30e01a866efa 194 case 2:
jodoedjr 1:30e01a866efa 195 command = 'r';
jodoedjr 1:30e01a866efa 196 break;
jodoedjr 1:30e01a866efa 197 case 3:
jodoedjr 1:30e01a866efa 198 command = 'l';
jodoedjr 1:30e01a866efa 199 break;
jodoedjr 1:30e01a866efa 200 default:
jodoedjr 1:30e01a866efa 201 command = 'b';
jodoedjr 1:30e01a866efa 202 break;
jodoedjr 1:30e01a866efa 203 }*/
jodoedjr 1:30e01a866efa 204 command = debugcommand[count];
jodoedjr 1:30e01a866efa 205 count++;
jodoedjr 1:30e01a866efa 206 pc.printf("NEW COMMAND: %c", command);
jodoedjr 1:30e01a866efa 207 #endif
jodoedjr 1:30e01a866efa 208 command = 'W';
jodoedjr 1:30e01a866efa 209 break;
jodoedjr 1:30e01a866efa 210 default: //wait and show error
jodoedjr 1:30e01a866efa 211 pc.printf("DEFAULT\n\r");
jodoedjr 1:30e01a866efa 212 left.stop(0);
jodoedjr 1:30e01a866efa 213 right.stop(0);
jodoedjr 1:30e01a866efa 214 led1 = 1;
jodoedjr 1:30e01a866efa 215 led4 = 1;
jodoedjr 1:30e01a866efa 216 wait(.5);
jodoedjr 1:30e01a866efa 217 led2 = 1;
jodoedjr 1:30e01a866efa 218 led3 = 1;
jodoedjr 1:30e01a866efa 219 led1 =0;
jodoedjr 1:30e01a866efa 220 led4 = 0;
jodoedjr 1:30e01a866efa 221 wait(.5);
jodoedjr 1:30e01a866efa 222 led2 = 0;
jodoedjr 1:30e01a866efa 223 led3 = 0;
jodoedjr 1:30e01a866efa 224 break;
jodoedjr 1:30e01a866efa 225 }
4180_1 0:5c26b3940003 226 }
jodoedjr 1:30e01a866efa 227 }