Christopher Hasler
this is most recent published version of the mbed robots software, demonstrating object avoidance, and some logo based control functions.
Dependencies: mbed Motordriver Servo GP2xx
main.cpp@0:8fe745012832, 2010-12-01 (annotated)
- Committer:
- littlexc
- Date:
- Wed Dec 01 21:34:07 2010 +0000
- Revision:
- 0:8fe745012832
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| littlexc | 0:8fe745012832 | 1 | #include <mbed.h> |
| littlexc | 0:8fe745012832 | 2 | #include <motordriver.h> |
| littlexc | 0:8fe745012832 | 3 | #include <Servo.h> |
| littlexc | 0:8fe745012832 | 4 | #include <GP2xx.h> |
| littlexc | 0:8fe745012832 | 5 | //pc interface |
| littlexc | 0:8fe745012832 | 6 | Serial pc(USBTX, USBRX); // tx, rx |
| littlexc | 0:8fe745012832 | 7 | //servos |
| littlexc | 0:8fe745012832 | 8 | Servo LeftServo(p24); |
| littlexc | 0:8fe745012832 | 9 | Servo RightServo(p23); |
| littlexc | 0:8fe745012832 | 10 | //motors, left and right side |
| littlexc | 0:8fe745012832 | 11 | Motor leftM(p22, p6, p5, 1); // pwm, fwd, rev, can break |
| littlexc | 0:8fe745012832 | 12 | Motor rightM(p21, p7, p8, 1); // pwm, fwd, rev, can break |
| littlexc | 0:8fe745012832 | 13 | //range finders, left and right side, and left and right front |
| littlexc | 0:8fe745012832 | 14 | IRRangeFinder LS(p18,1); |
| littlexc | 0:8fe745012832 | 15 | IRRangeFinder LF(p17,1); |
| littlexc | 0:8fe745012832 | 16 | IRRangeFinder RF(p16,1); |
| littlexc | 0:8fe745012832 | 17 | IRRangeFinder RS(p15,1); |
| littlexc | 0:8fe745012832 | 18 | //debug leds. |
| littlexc | 0:8fe745012832 | 19 | DigitalOut led1 (LED1); |
| littlexc | 0:8fe745012832 | 20 | DigitalOut led2 (LED2); |
| littlexc | 0:8fe745012832 | 21 | DigitalOut led3 (LED3); |
| littlexc | 0:8fe745012832 | 22 | DigitalOut led4 (LED4); |
| littlexc | 0:8fe745012832 | 23 | DigitalOut ledleft (p14); |
| littlexc | 0:8fe745012832 | 24 | DigitalOut ledright (p13); |
| littlexc | 0:8fe745012832 | 25 | DigitalOut ledfront (p12); |
| littlexc | 0:8fe745012832 | 26 | /** simple object avoidance using a logo based set of functions. it works and provides some intresting ways |
| littlexc | 0:8fe745012832 | 27 | * to control behaviour. |
| littlexc | 0:8fe745012832 | 28 | * |
| littlexc | 0:8fe745012832 | 29 | */ |
| littlexc | 0:8fe745012832 | 30 | |
| littlexc | 0:8fe745012832 | 31 | |
| littlexc | 0:8fe745012832 | 32 | /** function to spin the robot on the spot. technicaly redundant, as turn can do the same, but clearer when coding |
| littlexc | 0:8fe745012832 | 33 | * @param time, how long to spin for. |
| littlexc | 0:8fe745012832 | 34 | * @param speed how fast to run the motors. |
| littlexc | 0:8fe745012832 | 35 | * |
| littlexc | 0:8fe745012832 | 36 | */ |
| littlexc | 0:8fe745012832 | 37 | float spin (float time, float speed);//-1 to 1 |
| littlexc | 0:8fe745012832 | 38 | /** function to make the robot go fowards in a straight line |
| littlexc | 0:8fe745012832 | 39 | * @param time, how long to go straight for. |
| littlexc | 0:8fe745012832 | 40 | * @param speed how fast to run the motors. -1 is reverse, 1 is foward, 0 is technicaly possible but means the robot coasts. |
| littlexc | 0:8fe745012832 | 41 | * |
| littlexc | 0:8fe745012832 | 42 | */ |
| littlexc | 0:8fe745012832 | 43 | float straight (float time, float speed);//0 to 1 |
| littlexc | 0:8fe745012832 | 44 | /** function to make the robot turn |
| littlexc | 0:8fe745012832 | 45 | * @param time, how long to turn for. |
| littlexc | 0:8fe745012832 | 46 | * @param speed how fast to run the motors. -1 is right, 1 is left, 0 is technicaly possible but means the robot coasts. |
| littlexc | 0:8fe745012832 | 47 | * @param diffrence, from 1 to -1. how much slower the opposite side is -1 is same speed and direction, 1 is same speed opposite direction |
| littlexc | 0:8fe745012832 | 48 | */ |
| littlexc | 0:8fe745012832 | 49 | float turn (float time, float speed, float diffrence); |
| littlexc | 0:8fe745012832 | 50 | /** function to make the robot stop |
| littlexc | 0:8fe745012832 | 51 | * @param time, how long to stop for |
| littlexc | 0:8fe745012832 | 52 | * @param duty how hard to stop the motors. 1 is full on, 0 is technicaly possible but means the robot coasts. |
| littlexc | 0:8fe745012832 | 53 | * |
| littlexc | 0:8fe745012832 | 54 | */ |
| littlexc | 0:8fe745012832 | 55 | float stop (float time, float duty); |
| littlexc | 0:8fe745012832 | 56 | |
| littlexc | 0:8fe745012832 | 57 | int main() { |
| littlexc | 0:8fe745012832 | 58 | float tempLF = 0, tempRF = 0; |
| littlexc | 0:8fe745012832 | 59 | while (1) { |
| littlexc | 0:8fe745012832 | 60 | tempLF = LS.read();//cuts down function calls, and therefore clock cycles used |
| littlexc | 0:8fe745012832 | 61 | tempRF = RS.read(); |
| littlexc | 0:8fe745012832 | 62 | |
| littlexc | 0:8fe745012832 | 63 | if (tempLF < 12) { |
| littlexc | 0:8fe745012832 | 64 | if (tempLF > tempRF) { |
| littlexc | 0:8fe745012832 | 65 | turn(0.1,-0.8,0.5); |
| littlexc | 0:8fe745012832 | 66 | } else if (tempLF < tempRF) { |
| littlexc | 0:8fe745012832 | 67 | turn(0.1,1,0.5); |
| littlexc | 0:8fe745012832 | 68 | } else { |
| littlexc | 0:8fe745012832 | 69 | spin(0.1,1); |
| littlexc | 0:8fe745012832 | 70 | } |
| littlexc | 0:8fe745012832 | 71 | } else if (tempRF < 12) { |
| littlexc | 0:8fe745012832 | 72 | if (tempLF < tempRF) { |
| littlexc | 0:8fe745012832 | 73 | turn(0.1,-1,0.5); |
| littlexc | 0:8fe745012832 | 74 | } else if (tempLF > tempRF) { |
| littlexc | 0:8fe745012832 | 75 | turn(0.1,1,0.5); |
| littlexc | 0:8fe745012832 | 76 | } else { |
| littlexc | 0:8fe745012832 | 77 | spin(0.1,1); |
| littlexc | 0:8fe745012832 | 78 | } |
| littlexc | 0:8fe745012832 | 79 | } else { |
| littlexc | 0:8fe745012832 | 80 | straight(0.1, 0.7); |
| littlexc | 0:8fe745012832 | 81 | } |
| littlexc | 0:8fe745012832 | 82 | } |
| littlexc | 0:8fe745012832 | 83 | } |
| littlexc | 0:8fe745012832 | 84 | |
| littlexc | 0:8fe745012832 | 85 | float spin (float time, float speed) { |
| littlexc | 0:8fe745012832 | 86 | float temp1 = 0, temp2 = 0; |
| littlexc | 0:8fe745012832 | 87 | temp1 = leftM.speed(speed); |
| littlexc | 0:8fe745012832 | 88 | temp2 = rightM.speed(-speed); |
| littlexc | 0:8fe745012832 | 89 | wait(time); |
| littlexc | 0:8fe745012832 | 90 | leftM.coast(): |
| littlexc | 0:8fe745012832 | 91 | rightM.coast(); |
| littlexc | 0:8fe745012832 | 92 | return temp1+temp2;//should return 0. |
| littlexc | 0:8fe745012832 | 93 | } |
| littlexc | 0:8fe745012832 | 94 | |
| littlexc | 0:8fe745012832 | 95 | float straight (float time, float speed) { |
| littlexc | 0:8fe745012832 | 96 | float temp1 = 0, temp2 = 0; |
| littlexc | 0:8fe745012832 | 97 | temp1 = leftM.speed(speed); |
| littlexc | 0:8fe745012832 | 98 | temp2 = rightM.speed(speed); |
| littlexc | 0:8fe745012832 | 99 | wait(time); |
| littlexc | 0:8fe745012832 | 100 | leftM.coast(): |
| littlexc | 0:8fe745012832 | 101 | rightM.coast(); |
| littlexc | 0:8fe745012832 | 102 | return temp1-temp2;//should return 0. |
| littlexc | 0:8fe745012832 | 103 | } |
| littlexc | 0:8fe745012832 | 104 | |
| littlexc | 0:8fe745012832 | 105 | float stop (float time, float duty) { |
| littlexc | 0:8fe745012832 | 106 | float temp1 = 0, temp2 = 0; |
| littlexc | 0:8fe745012832 | 107 | temp1 = leftM.stop(duty); |
| littlexc | 0:8fe745012832 | 108 | temp2 = rightM.stop(duty); |
| littlexc | 0:8fe745012832 | 109 | wait(time); |
| littlexc | 0:8fe745012832 | 110 | leftM.coast(): |
| littlexc | 0:8fe745012832 | 111 | rightM.coast(); |
| littlexc | 0:8fe745012832 | 112 | return temp1-temp2;//should return 0. |
| littlexc | 0:8fe745012832 | 113 | } |
| littlexc | 0:8fe745012832 | 114 | |
| littlexc | 0:8fe745012832 | 115 | float turn (float time, float speed, float diffrence) { |
| littlexc | 0:8fe745012832 | 116 | float temp1 = -speed*diffrence; |
| littlexc | 0:8fe745012832 | 117 | if (speed<0) { |
| littlexc | 0:8fe745012832 | 118 | leftM.speed(speed); |
| littlexc | 0:8fe745012832 | 119 | rightM.speed(temp1); |
| littlexc | 0:8fe745012832 | 120 | } else if (speed>0) { |
| littlexc | 0:8fe745012832 | 121 | leftM.speed(temp1); |
| littlexc | 0:8fe745012832 | 122 | rightM.speed(speed); |
| littlexc | 0:8fe745012832 | 123 | } else if (speed == 0) { |
| littlexc | 0:8fe745012832 | 124 | leftM.coast(); |
| littlexc | 0:8fe745012832 | 125 | rightM.coast(); |
| littlexc | 0:8fe745012832 | 126 | } else { |
| littlexc | 0:8fe745012832 | 127 | return -5; |
| littlexc | 0:8fe745012832 | 128 | } |
| littlexc | 0:8fe745012832 | 129 | wait(time); |
| littlexc | 0:8fe745012832 | 130 | leftM.coast(): |
| littlexc | 0:8fe745012832 | 131 | rightM.coast(); |
| littlexc | 0:8fe745012832 | 132 | return temp1;//should return -speed*diffrence. |
| littlexc | 0:8fe745012832 | 133 | } |
| littlexc | 0:8fe745012832 | 134 | |
| littlexc | 0:8fe745012832 | 135 | |
| littlexc | 0:8fe745012832 | 136 | |
| littlexc | 0:8fe745012832 | 137 |