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
Diff: main.cpp
- Revision:
- 1:30e01a866efa
- Parent:
- 0:5c26b3940003
- Child:
- 2:e2b6fe03e630
diff -r 5c26b3940003 -r 30e01a866efa main.cpp --- a/main.cpp Wed Oct 26 01:12:51 2011 +0000 +++ b/main.cpp Wed Apr 22 21:36:17 2015 +0000 @@ -1,28 +1,227 @@ -// Magician robot motor test +// ECE 4180 Gesture Sensor Controlled Robot Project: Robot Comm, Heading and Motor Control, Obstacle Detection #include "mbed.h" #include "motordriver.h" - -DigitalOut myled(LED1); + +//#define DEBUG -//See http://mbed.org/cookbook/Motor -//Connections to dual H-brdige driver for the two drive motors +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); +AnalogIn IR(p20);//Input for IR sensor, 0.60 seems like a good object detect distance +InterruptIn REN(p15);//INTERRUPT IN PIN FOR RIGHT ENCODER +InterruptIn LEN(p16);//INTERRUPT IN PIN FOR LEFT ENCODER +int countR = 0; +int countL = 0; + + +Serial pc(USBTX, USBRX);//PC serial for debug +Serial xbee1(p13, p14);//xbee serial connection + +int count = 0; //counter for reading through debug command array; +char debugcommand[] = "FRFRFRFRS";//direction commands for debug: forward, right, forward, back, forward, left, forward, stop +//int num = 0; +char command = 'W'; // switch case variable to be used for commands from wireless comm, default w for wait +bool collision_flag = 0; + Motor left(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature Motor right(p26, p25, p24, 1); +bool x = true; +//Functions for updating/reading encoder +void Rcount() //count Right encoder increments +{ + countR++; +} +void Lcount() //count Left encoder increments +{ + countL++; +} +void clearEN() //clear the encoder values +{ + countR = 0; + countL = 0; +} -int main() { - while (1) { - myled=1; -// Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0) - for (float s= -1.0; s < 1.0 ; s += 0.01) { - left.speed(s); - right.speed(s); - wait(0.02); + +int main() +{ + float sl = 0.65;//preset wheel speed left + float sr = 0.65*0.92;//preset wheel speed right + //************************************************************ + + + + //************************************************************ + REN.fall(&Rcount); + LEN.fall(&Lcount); + + led1 = 1; + led2 = 1; + led3 = 1; + led4 = 1; + + wait(7); + led1 = 0; + led2 = 0; + led3 = 0; + led4 = 0; + + bool x = true;//loop variable + + + while (x) {//program loop + + //************************************************************ + if(IR >=0.60) { + left.stop(1); + right.stop(1); + collision_flag = 1; + } + if(collision_flag) { + xbee1.putc('C'); + pc.printf("C\n\r"); + collision_flag = 0; + } + if(xbee1.readable()) { + command = xbee1.getc(); + pc.printf("Command: %c\n\r",command); } -// Turn off motors - coast one and brake one - left.coast(); - right.stop(1); - myled=0; - wait(1); + + //************************************************************ + + switch ( command ) {//switch statement for robot motor control + case 'F': //move forward + pc.printf("FORWARD\n\r"); + led2 = 1; + led3 = 1; + clearEN(); + while ((countL <= 500)||(countR <=340)) {//loop specified distance + if(IR >=0.60) { + left.stop(1); + right.stop(1); + collision_flag = 1; + break; + } //if IR has detected a close object, stop + if(xbee1.readable()) { + command = xbee1.getc(); + pc.printf("Command: %c\n\r",command); + break; + } + left.speed(sl); + right.speed(sr);//drift factor associated with our build + pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL); + wait(.01); + } + led2 = 0; + led3 = 0; + left.stop(1); + right.stop(1); + command = 'W'; + break; + + case 'R': //turn 90 deg right + pc.printf("RIGHT\n\r"); + led1 = 1; + led2 = 1; + clearEN(); + while ((countL <= 80)) {//loop turn for specified angle + //pc.printf("RIGHT: %d, LEFT: %d\n\r", countR, countL); + left.speed(sl); + right.speed(-sr);//right.speed(-sr); + if(xbee1.readable()) { + command = xbee1.getc(); + pc.printf("Command: %c\n\r",command); + break; + } + } + led1 = 0; + led2 = 0; + left.stop(1); + right.stop(1); + command = 'W'; + //pc.printf("END OF COMMAND:: RIGHT: %d, LEFT: %d\n\r", countR, countL); + break; + + case 'L': //turn 90 deg left + pc.printf("LEFT\n\r"); + led3 = 1; + led4 = 1; + + clearEN(); + //loop turn for specified angle + while (( countR<=55)) { + left.speed(-sl); + right.speed(sr); + if(xbee1.readable()) { + command = xbee1.getc(); + pc.printf("Command: %c\n\r",command); + break; + } + } + led3 = 0; + led4 = 0; + left.stop(1); + right.stop(1); + command = 'W'; + break; + + case 'B': //turn 180 deg left + pc.printf("BACKWARDS\n\r"); + led1 = 1; + led4 = 1; + left.speed(-sl); + right.speed(-sr); + wait(.5);//bump robot backwards by reversing both wheels for half a second + + led1 = 0; + led4 = 0; + left.stop(1); + right.stop(1); + command = 'W'; + break; + + case 'W': //wait for a new command + pc.printf("WAIT\n\r"); + wait(.5); + // Code +#ifdef DEBUG + /*num = rand()%3; + switch( num ) { + case 1: + command = 'f'; + break; + case 2: + command = 'r'; + break; + case 3: + command = 'l'; + break; + default: + command = 'b'; + break; + }*/ + command = debugcommand[count]; + count++; + pc.printf("NEW COMMAND: %c", command); +#endif + command = 'W'; + break; + default: //wait and show error + pc.printf("DEFAULT\n\r"); + left.stop(0); + right.stop(0); + led1 = 1; + led4 = 1; + wait(.5); + led2 = 1; + led3 = 1; + led1 =0; + led4 = 0; + wait(.5); + led2 = 0; + led3 = 0; + break; + } } -} +} \ No newline at end of file