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@2:e2b6fe03e630, 2015-04-27 (annotated)

Committer:
jodoedjr
Date:
Mon Apr 27 15:33:02 2015 +0000
Revision:
2:e2b6fe03e630
Parent:
1:30e01a866efa 
Post-demo code; Functions work as expected for non-linearity specific to our robot. Individual users should update motor speed and encoder count threshholds for their individual robots.

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