This code is for controlling the motors on the robot based on the values received on the xbee coming from the flex sensors.
main.cpp@0:a66bf82d70ed, 2018-04-26 (annotated)
- Committer:
- fyousuf3
- Date:
- Thu Apr 26 15:37:12 2018 +0000
- Revision:
- 0:a66bf82d70ed
Code for the motor control from glove inputs
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fyousuf3 | 0:a66bf82d70ed | 1 | // test code, this demonstrates working motor drivers. |
fyousuf3 | 0:a66bf82d70ed | 2 | // full reverse to full stop, dynamicaly brake and switch off. |
fyousuf3 | 0:a66bf82d70ed | 3 | #include "mbed.h" |
fyousuf3 | 0:a66bf82d70ed | 4 | #include "Motor.h" |
fyousuf3 | 0:a66bf82d70ed | 5 | Motor A(p22, p6, p5); // pwm, fwd, rev, can brake |
fyousuf3 | 0:a66bf82d70ed | 6 | Motor B(p21, p7, p8); // pwm, fwd, rev, can brake |
fyousuf3 | 0:a66bf82d70ed | 7 | |
fyousuf3 | 0:a66bf82d70ed | 8 | |
fyousuf3 | 0:a66bf82d70ed | 9 | //End Device Setup for XBEE End Device |
fyousuf3 | 0:a66bf82d70ed | 10 | Serial xbee2(p9,p10); |
fyousuf3 | 0:a66bf82d70ed | 11 | DigitalOut rst1(p11); |
fyousuf3 | 0:a66bf82d70ed | 12 | |
fyousuf3 | 0:a66bf82d70ed | 13 | //Movement Indicators |
fyousuf3 | 0:a66bf82d70ed | 14 | DigitalOut myled1(LED1); |
fyousuf3 | 0:a66bf82d70ed | 15 | DigitalOut myled2(LED2); |
fyousuf3 | 0:a66bf82d70ed | 16 | DigitalOut myled3(LED2); |
fyousuf3 | 0:a66bf82d70ed | 17 | DigitalOut myled4(LED2); |
fyousuf3 | 0:a66bf82d70ed | 18 | |
fyousuf3 | 0:a66bf82d70ed | 19 | // Default Value for command to wait |
fyousuf3 | 0:a66bf82d70ed | 20 | char command = 'W'; |
fyousuf3 | 0:a66bf82d70ed | 21 | |
fyousuf3 | 0:a66bf82d70ed | 22 | int main() { |
fyousuf3 | 0:a66bf82d70ed | 23 | rst1 = 0; //Set reset pin to 0 |
fyousuf3 | 0:a66bf82d70ed | 24 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 25 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 26 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 27 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 28 | wait_ms(1); |
fyousuf3 | 0:a66bf82d70ed | 29 | rst1 = 1; //Set reset pin to 1 |
fyousuf3 | 0:a66bf82d70ed | 30 | wait_ms(1); |
fyousuf3 | 0:a66bf82d70ed | 31 | |
fyousuf3 | 0:a66bf82d70ed | 32 | while (1){ |
fyousuf3 | 0:a66bf82d70ed | 33 | if(xbee2.readable()) {//if new command readable |
fyousuf3 | 0:a66bf82d70ed | 34 | command = xbee2.getc(); |
fyousuf3 | 0:a66bf82d70ed | 35 | //pc.printf("Command: %c\n\r",command); |
fyousuf3 | 0:a66bf82d70ed | 36 | } |
fyousuf3 | 0:a66bf82d70ed | 37 | |
fyousuf3 | 0:a66bf82d70ed | 38 | switch (command){ |
fyousuf3 | 0:a66bf82d70ed | 39 | case 'A': // Move back speed 1 |
fyousuf3 | 0:a66bf82d70ed | 40 | A.speed(-0.5); |
fyousuf3 | 0:a66bf82d70ed | 41 | B.speed(-0.5); |
fyousuf3 | 0:a66bf82d70ed | 42 | |
fyousuf3 | 0:a66bf82d70ed | 43 | myled1 = 1; |
fyousuf3 | 0:a66bf82d70ed | 44 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 45 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 46 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 47 | |
fyousuf3 | 0:a66bf82d70ed | 48 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 49 | break; |
fyousuf3 | 0:a66bf82d70ed | 50 | case 'B': // Move back speed 2 |
fyousuf3 | 0:a66bf82d70ed | 51 | A.speed(-1); |
fyousuf3 | 0:a66bf82d70ed | 52 | B.speed(-1); |
fyousuf3 | 0:a66bf82d70ed | 53 | |
fyousuf3 | 0:a66bf82d70ed | 54 | myled1 = 1; |
fyousuf3 | 0:a66bf82d70ed | 55 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 56 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 57 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 58 | |
fyousuf3 | 0:a66bf82d70ed | 59 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 60 | break; |
fyousuf3 | 0:a66bf82d70ed | 61 | case 'C': // Move right speed 1 |
fyousuf3 | 0:a66bf82d70ed | 62 | A.speed(0.5); |
fyousuf3 | 0:a66bf82d70ed | 63 | B.speed(0); |
fyousuf3 | 0:a66bf82d70ed | 64 | |
fyousuf3 | 0:a66bf82d70ed | 65 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 66 | myled2 = 1; |
fyousuf3 | 0:a66bf82d70ed | 67 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 68 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 69 | |
fyousuf3 | 0:a66bf82d70ed | 70 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 71 | break; |
fyousuf3 | 0:a66bf82d70ed | 72 | case 'D': // Move right speed 2 |
fyousuf3 | 0:a66bf82d70ed | 73 | A.speed(1); |
fyousuf3 | 0:a66bf82d70ed | 74 | B.speed(0); |
fyousuf3 | 0:a66bf82d70ed | 75 | |
fyousuf3 | 0:a66bf82d70ed | 76 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 77 | myled2 = 1; |
fyousuf3 | 0:a66bf82d70ed | 78 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 79 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 80 | |
fyousuf3 | 0:a66bf82d70ed | 81 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 82 | break; |
fyousuf3 | 0:a66bf82d70ed | 83 | case 'E': // Move forward speed 1 |
fyousuf3 | 0:a66bf82d70ed | 84 | A.speed(0.5); |
fyousuf3 | 0:a66bf82d70ed | 85 | B.speed(0.5); |
fyousuf3 | 0:a66bf82d70ed | 86 | |
fyousuf3 | 0:a66bf82d70ed | 87 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 88 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 89 | myled3 = 1; |
fyousuf3 | 0:a66bf82d70ed | 90 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 91 | |
fyousuf3 | 0:a66bf82d70ed | 92 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 93 | break; |
fyousuf3 | 0:a66bf82d70ed | 94 | case 'F': // Move forward speed 2 |
fyousuf3 | 0:a66bf82d70ed | 95 | A.speed(1); |
fyousuf3 | 0:a66bf82d70ed | 96 | B.speed(1); |
fyousuf3 | 0:a66bf82d70ed | 97 | |
fyousuf3 | 0:a66bf82d70ed | 98 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 99 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 100 | myled3 = 1; |
fyousuf3 | 0:a66bf82d70ed | 101 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 102 | |
fyousuf3 | 0:a66bf82d70ed | 103 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 104 | break; |
fyousuf3 | 0:a66bf82d70ed | 105 | case 'G': // Move left speed 1 |
fyousuf3 | 0:a66bf82d70ed | 106 | A.speed(0); |
fyousuf3 | 0:a66bf82d70ed | 107 | B.speed(0.5); |
fyousuf3 | 0:a66bf82d70ed | 108 | |
fyousuf3 | 0:a66bf82d70ed | 109 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 110 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 111 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 112 | myled4 = 1; |
fyousuf3 | 0:a66bf82d70ed | 113 | |
fyousuf3 | 0:a66bf82d70ed | 114 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 115 | break; |
fyousuf3 | 0:a66bf82d70ed | 116 | case 'H': // Move left speed 2 |
fyousuf3 | 0:a66bf82d70ed | 117 | A.speed(0); |
fyousuf3 | 0:a66bf82d70ed | 118 | B.speed(1); |
fyousuf3 | 0:a66bf82d70ed | 119 | |
fyousuf3 | 0:a66bf82d70ed | 120 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 121 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 122 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 123 | myled4 = 1; |
fyousuf3 | 0:a66bf82d70ed | 124 | |
fyousuf3 | 0:a66bf82d70ed | 125 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 126 | break; |
fyousuf3 | 0:a66bf82d70ed | 127 | case 'I': // Stop |
fyousuf3 | 0:a66bf82d70ed | 128 | A.speed(-0.5); |
fyousuf3 | 0:a66bf82d70ed | 129 | B.speed(-0.5); |
fyousuf3 | 0:a66bf82d70ed | 130 | |
fyousuf3 | 0:a66bf82d70ed | 131 | myled1 = 1; |
fyousuf3 | 0:a66bf82d70ed | 132 | myled2 = 1; |
fyousuf3 | 0:a66bf82d70ed | 133 | myled3 = 1; |
fyousuf3 | 0:a66bf82d70ed | 134 | myled4 = 1; |
fyousuf3 | 0:a66bf82d70ed | 135 | |
fyousuf3 | 0:a66bf82d70ed | 136 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 137 | |
fyousuf3 | 0:a66bf82d70ed | 138 | case 'W': // Stop if nothing else is coming in |
fyousuf3 | 0:a66bf82d70ed | 139 | A.speed(0); |
fyousuf3 | 0:a66bf82d70ed | 140 | B.speed(0); |
fyousuf3 | 0:a66bf82d70ed | 141 | |
fyousuf3 | 0:a66bf82d70ed | 142 | myled1 = 0; |
fyousuf3 | 0:a66bf82d70ed | 143 | myled2 = 0; |
fyousuf3 | 0:a66bf82d70ed | 144 | myled3 = 0; |
fyousuf3 | 0:a66bf82d70ed | 145 | myled4 = 0; |
fyousuf3 | 0:a66bf82d70ed | 146 | |
fyousuf3 | 0:a66bf82d70ed | 147 | wait(0.5); |
fyousuf3 | 0:a66bf82d70ed | 148 | |
fyousuf3 | 0:a66bf82d70ed | 149 | } |
fyousuf3 | 0:a66bf82d70ed | 150 | } |
fyousuf3 | 0:a66bf82d70ed | 151 | } |