Final Version.
Dependencies: Motor Servo mbed
main.cpp@0:8eae572f6d20, 2016-03-11 (annotated)
- Committer:
- raj1995
- Date:
- Fri Mar 11 18:59:21 2016 +0000
- Revision:
- 0:8eae572f6d20
Final version of lab4.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
raj1995 | 0:8eae572f6d20 | 1 | /* |
raj1995 | 0:8eae572f6d20 | 2 | |
raj1995 | 0:8eae572f6d20 | 3 | @Authors: Raj Patel, David Ehrlich |
raj1995 | 0:8eae572f6d20 | 4 | @Project: Bluetooth Controlled Robot with Dual Servo Sensor Actuated by Mobile Phone Accelerometer |
raj1995 | 0:8eae572f6d20 | 5 | @Date: 03/09/2016 |
raj1995 | 0:8eae572f6d20 | 6 | |
raj1995 | 0:8eae572f6d20 | 7 | */ |
raj1995 | 0:8eae572f6d20 | 8 | |
raj1995 | 0:8eae572f6d20 | 9 | //include necessary header files for functionality. Specifically, need servo handlers. |
raj1995 | 0:8eae572f6d20 | 10 | |
raj1995 | 0:8eae572f6d20 | 11 | #include "mbed.h" |
raj1995 | 0:8eae572f6d20 | 12 | #include "Servo.h" |
raj1995 | 0:8eae572f6d20 | 13 | |
raj1995 | 0:8eae572f6d20 | 14 | Serial bluemod(p9,p10); // Serial hookup for Bluetooth module |
raj1995 | 0:8eae572f6d20 | 15 | PwmOut left_led(LED3); // PWM output for left robot direction signal |
raj1995 | 0:8eae572f6d20 | 16 | PwmOut right_led(LED4); // PWM output for right robot direction signal |
raj1995 | 0:8eae572f6d20 | 17 | PwmOut forward_led(LED1); // PWM output for forward robot direction signal |
raj1995 | 0:8eae572f6d20 | 18 | PwmOut reverse_led(LED2); // PWM output for reverse robot direction signal |
raj1995 | 0:8eae572f6d20 | 19 | Servo myservolr(p21); // Signal for left/right servo |
raj1995 | 0:8eae572f6d20 | 20 | Servo myservoud(p22); // Signal for up/down servo |
raj1995 | 0:8eae572f6d20 | 21 | AnalogIn sensor(p15); // Signal for Sharp IR Sensor |
raj1995 | 0:8eae572f6d20 | 22 | |
raj1995 | 0:8eae572f6d20 | 23 | PwmOut speedA(p23); // PWM Speed pin for Motor A |
raj1995 | 0:8eae572f6d20 | 24 | DigitalOut fwdA(p17); // Digital out signal for forward direction for Motor A |
raj1995 | 0:8eae572f6d20 | 25 | DigitalOut revA(p18); // Digital out signal for reverse direction for Motor A |
raj1995 | 0:8eae572f6d20 | 26 | |
raj1995 | 0:8eae572f6d20 | 27 | PwmOut speedB(p24); // PWM Speed pin for Motor B |
raj1995 | 0:8eae572f6d20 | 28 | DigitalOut fwdB(p20); // Digital out signal for forward direction for Motor B |
raj1995 | 0:8eae572f6d20 | 29 | DigitalOut revB(p19); // Digital out signal for reverse direction for Motor B |
raj1995 | 0:8eae572f6d20 | 30 | |
raj1995 | 0:8eae572f6d20 | 31 | |
raj1995 | 0:8eae572f6d20 | 32 | volatile char exc; // Variable to keep track of ! when seen in BT stream |
raj1995 | 0:8eae572f6d20 | 33 | volatile char letter; // Varibale to keep track of character after ! in BT stream (A for accelerometer, B for buttons) |
raj1995 | 0:8eae572f6d20 | 34 | |
raj1995 | 0:8eae572f6d20 | 35 | //C union can convert 4 chars to a float - puts them in same location in memory |
raj1995 | 0:8eae572f6d20 | 36 | //trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float |
raj1995 | 0:8eae572f6d20 | 37 | union f_or_char { |
raj1995 | 0:8eae572f6d20 | 38 | float f; |
raj1995 | 0:8eae572f6d20 | 39 | char c[4]; |
raj1995 | 0:8eae572f6d20 | 40 | }; |
raj1995 | 0:8eae572f6d20 | 41 | |
raj1995 | 0:8eae572f6d20 | 42 | int main() |
raj1995 | 0:8eae572f6d20 | 43 | { |
raj1995 | 0:8eae572f6d20 | 44 | char bchecksum=0; // Checksum variable |
raj1995 | 0:8eae572f6d20 | 45 | char temp=0; // Temporary variable to hold BT stream data |
raj1995 | 0:8eae572f6d20 | 46 | union f_or_char x,y,z; // Templatized containers for x, y, and z accelerometer readings |
raj1995 | 0:8eae572f6d20 | 47 | float adjlr = 0; // Servo position variable for left/right servo |
raj1995 | 0:8eae572f6d20 | 48 | float adjud = 0; // Servo position variable for up/down servo |
raj1995 | 0:8eae572f6d20 | 49 | while(1) { |
raj1995 | 0:8eae572f6d20 | 50 | float sensorValue = sensor.read(); |
raj1995 | 0:8eae572f6d20 | 51 | //printf("Sensor: %2.2f\n\r", sensorValue); |
raj1995 | 0:8eae572f6d20 | 52 | bchecksum=0; |
raj1995 | 0:8eae572f6d20 | 53 | exc = bluemod.getc(); // Get the first character in the bluetooth packet |
raj1995 | 0:8eae572f6d20 | 54 | if (exc=='!'){ // First character was ! meaning it could be !A or !B |
raj1995 | 0:8eae572f6d20 | 55 | letter = bluemod.getc(); // Get the next character in the bluetooth packet after the ! |
raj1995 | 0:8eae572f6d20 | 56 | switch (letter) { |
raj1995 | 0:8eae572f6d20 | 57 | case 'A':{ // If the next character is A, it is an Accelerometer data packet |
raj1995 | 0:8eae572f6d20 | 58 | for (int i=0; i<4; i++) { |
raj1995 | 0:8eae572f6d20 | 59 | temp = bluemod.getc(); |
raj1995 | 0:8eae572f6d20 | 60 | x.c[i] = temp; // Get x direction Accelerometer data |
raj1995 | 0:8eae572f6d20 | 61 | bchecksum = bchecksum + temp; |
raj1995 | 0:8eae572f6d20 | 62 | } |
raj1995 | 0:8eae572f6d20 | 63 | for (int i=0; i<4; i++) { |
raj1995 | 0:8eae572f6d20 | 64 | temp = bluemod.getc(); |
raj1995 | 0:8eae572f6d20 | 65 | y.c[i] = temp; // Get y direction Accelerometer data |
raj1995 | 0:8eae572f6d20 | 66 | bchecksum = bchecksum + temp; |
raj1995 | 0:8eae572f6d20 | 67 | } |
raj1995 | 0:8eae572f6d20 | 68 | for (int i=0; i<4; i++) { |
raj1995 | 0:8eae572f6d20 | 69 | temp = bluemod.getc(); |
raj1995 | 0:8eae572f6d20 | 70 | z.c[i] = temp; // Get z direction Accelerometer data |
raj1995 | 0:8eae572f6d20 | 71 | bchecksum = bchecksum + temp; |
raj1995 | 0:8eae572f6d20 | 72 | } |
raj1995 | 0:8eae572f6d20 | 73 | if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK? Then proceed. |
raj1995 | 0:8eae572f6d20 | 74 | |
raj1995 | 0:8eae572f6d20 | 75 | // Both segments of the below code in this 'if' statement handle actuating the dual servo setup |
raj1995 | 0:8eae572f6d20 | 76 | |
raj1995 | 0:8eae572f6d20 | 77 | // Below code segment handles left/right movement |
raj1995 | 0:8eae572f6d20 | 78 | if (y.f < -0.05 && y.f >= -0.10){ |
raj1995 | 0:8eae572f6d20 | 79 | adjlr += 0.01; // If the y direction shows a small negative value, move left slowly |
raj1995 | 0:8eae572f6d20 | 80 | } |
raj1995 | 0:8eae572f6d20 | 81 | if (y.f < -0.10 && y.f >= -0.25){ |
raj1995 | 0:8eae572f6d20 | 82 | adjlr += 0.05; // If the y direction shows a medium negative value, move left faster |
raj1995 | 0:8eae572f6d20 | 83 | } |
raj1995 | 0:8eae572f6d20 | 84 | if (y.f < -0.25){ |
raj1995 | 0:8eae572f6d20 | 85 | adjlr += 0.09; // If the y direction shows a large negative value, move left fastest |
raj1995 | 0:8eae572f6d20 | 86 | } |
raj1995 | 0:8eae572f6d20 | 87 | if (y.f > 0.05 && y.f <= 0.10){ |
raj1995 | 0:8eae572f6d20 | 88 | adjlr += -0.01; // If the y direction shows a small positive value, move right slowly |
raj1995 | 0:8eae572f6d20 | 89 | } |
raj1995 | 0:8eae572f6d20 | 90 | if (y.f > 0.10 && y.f <= 0.25){ |
raj1995 | 0:8eae572f6d20 | 91 | adjlr += -0.05; // If the y direction shows a medium positive value, move right faster |
raj1995 | 0:8eae572f6d20 | 92 | } |
raj1995 | 0:8eae572f6d20 | 93 | if (y.f > 0.25){ |
raj1995 | 0:8eae572f6d20 | 94 | adjlr += -0.09; // If the y direction shows a large positive value, move right fastest |
raj1995 | 0:8eae572f6d20 | 95 | } |
raj1995 | 0:8eae572f6d20 | 96 | if (adjlr < 0){ |
raj1995 | 0:8eae572f6d20 | 97 | adjlr = 0; // If the value becomes less than 0, cap at 0 |
raj1995 | 0:8eae572f6d20 | 98 | }else{ |
raj1995 | 0:8eae572f6d20 | 99 | if (adjlr > 1){ |
raj1995 | 0:8eae572f6d20 | 100 | adjlr = 1; // If the value becomes greater than 1, cap at 1 |
raj1995 | 0:8eae572f6d20 | 101 | } |
raj1995 | 0:8eae572f6d20 | 102 | } |
raj1995 | 0:8eae572f6d20 | 103 | |
raj1995 | 0:8eae572f6d20 | 104 | // Below code handles up/down movement |
raj1995 | 0:8eae572f6d20 | 105 | if (x.f < -0.05 && x.f >= -0.2){ |
raj1995 | 0:8eae572f6d20 | 106 | adjud += -0.01; // If the x direction shows a small negative value, move up slowly |
raj1995 | 0:8eae572f6d20 | 107 | } |
raj1995 | 0:8eae572f6d20 | 108 | if (x.f < -0.2 && x.f >= -0.40){ |
raj1995 | 0:8eae572f6d20 | 109 | adjud += -0.05; // If the x direction shows a medium negative value, move up faster |
raj1995 | 0:8eae572f6d20 | 110 | } |
raj1995 | 0:8eae572f6d20 | 111 | if (x.f < -0.4){ |
raj1995 | 0:8eae572f6d20 | 112 | adjud += -0.09; // If the x direction shows a large negative value, move up fastest |
raj1995 | 0:8eae572f6d20 | 113 | } |
raj1995 | 0:8eae572f6d20 | 114 | if (x.f > 0.05 && x.f <= 0.2){ |
raj1995 | 0:8eae572f6d20 | 115 | adjud += 0.01; // If the x direction shows a small positive value, move down slowly |
raj1995 | 0:8eae572f6d20 | 116 | } |
raj1995 | 0:8eae572f6d20 | 117 | if (x.f > 0.2 && x.f <= 0.40){ |
raj1995 | 0:8eae572f6d20 | 118 | adjud += 0.05; // If the x direction shows a medium positive value, move down faster |
raj1995 | 0:8eae572f6d20 | 119 | } |
raj1995 | 0:8eae572f6d20 | 120 | if (x.f > 0.40){ |
raj1995 | 0:8eae572f6d20 | 121 | adjud += 0.09; // If the x direction shows a large positive value, move down fastest |
raj1995 | 0:8eae572f6d20 | 122 | } |
raj1995 | 0:8eae572f6d20 | 123 | if (adjud < 0){ |
raj1995 | 0:8eae572f6d20 | 124 | adjud = 0; // If the value becomes less than 0, cap at 0 |
raj1995 | 0:8eae572f6d20 | 125 | }else{ |
raj1995 | 0:8eae572f6d20 | 126 | if (adjud > 1){ |
raj1995 | 0:8eae572f6d20 | 127 | adjud = 1; // If the value becomes greater than 1, cap at 1 |
raj1995 | 0:8eae572f6d20 | 128 | } |
raj1995 | 0:8eae572f6d20 | 129 | } |
raj1995 | 0:8eae572f6d20 | 130 | |
raj1995 | 0:8eae572f6d20 | 131 | myservolr = adjlr; // Send adjusted left/right signal to left/right servo |
raj1995 | 0:8eae572f6d20 | 132 | myservoud = adjud; // Send adjusted up/down signal to up/down servo |
raj1995 | 0:8eae572f6d20 | 133 | |
raj1995 | 0:8eae572f6d20 | 134 | |
raj1995 | 0:8eae572f6d20 | 135 | } |
raj1995 | 0:8eae572f6d20 | 136 | break; |
raj1995 | 0:8eae572f6d20 | 137 | } |
raj1995 | 0:8eae572f6d20 | 138 | char bnum; //Initialize variables to use below |
raj1995 | 0:8eae572f6d20 | 139 | char bhit; //Initialize variables to use below |
raj1995 | 0:8eae572f6d20 | 140 | |
raj1995 | 0:8eae572f6d20 | 141 | case 'B':{ // If the next character is B, it is a Button data packet |
raj1995 | 0:8eae572f6d20 | 142 | |
raj1995 | 0:8eae572f6d20 | 143 | bnum = bluemod.getc(); //button number |
raj1995 | 0:8eae572f6d20 | 144 | bhit = bluemod.getc(); //1=hit, 0=release |
raj1995 | 0:8eae572f6d20 | 145 | if (bluemod.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? |
raj1995 | 0:8eae572f6d20 | 146 | //myled = bnum - '0'; //current button number will appear on LEDs |
raj1995 | 0:8eae572f6d20 | 147 | switch (bnum) { |
raj1995 | 0:8eae572f6d20 | 148 | case '1': //number button 1 |
raj1995 | 0:8eae572f6d20 | 149 | if (bhit=='1') { |
raj1995 | 0:8eae572f6d20 | 150 | // nop |
raj1995 | 0:8eae572f6d20 | 151 | } else { |
raj1995 | 0:8eae572f6d20 | 152 | // nop |
raj1995 | 0:8eae572f6d20 | 153 | } |
raj1995 | 0:8eae572f6d20 | 154 | break; |
raj1995 | 0:8eae572f6d20 | 155 | case '2': //number button 2 |
raj1995 | 0:8eae572f6d20 | 156 | if (bhit=='1') { |
raj1995 | 0:8eae572f6d20 | 157 | // nop |
raj1995 | 0:8eae572f6d20 | 158 | } else { |
raj1995 | 0:8eae572f6d20 | 159 | // nop |
raj1995 | 0:8eae572f6d20 | 160 | } |
raj1995 | 0:8eae572f6d20 | 161 | break; |
raj1995 | 0:8eae572f6d20 | 162 | case '3': //number button 3 |
raj1995 | 0:8eae572f6d20 | 163 | if (bhit=='1') { |
raj1995 | 0:8eae572f6d20 | 164 | // nop |
raj1995 | 0:8eae572f6d20 | 165 | } else { |
raj1995 | 0:8eae572f6d20 | 166 | // nop |
raj1995 | 0:8eae572f6d20 | 167 | } |
raj1995 | 0:8eae572f6d20 | 168 | break; |
raj1995 | 0:8eae572f6d20 | 169 | case '4': //number button 4 |
raj1995 | 0:8eae572f6d20 | 170 | if (bhit=='1') { |
raj1995 | 0:8eae572f6d20 | 171 | // nop |
raj1995 | 0:8eae572f6d20 | 172 | } else { |
raj1995 | 0:8eae572f6d20 | 173 | // nop |
raj1995 | 0:8eae572f6d20 | 174 | } |
raj1995 | 0:8eae572f6d20 | 175 | break; |
raj1995 | 0:8eae572f6d20 | 176 | case '5': //button 5 up arrow |
raj1995 | 0:8eae572f6d20 | 177 | if (bhit=='1' && sensorValue <= 0.6) { |
raj1995 | 0:8eae572f6d20 | 178 | //forward |
raj1995 | 0:8eae572f6d20 | 179 | //hit button |
raj1995 | 0:8eae572f6d20 | 180 | forward_led=1; //Set forward LED on MBED |
raj1995 | 0:8eae572f6d20 | 181 | fwdA = 1; //Assert MotorA forward |
raj1995 | 0:8eae572f6d20 | 182 | revA = 0; //Deassert MotorA reverse |
raj1995 | 0:8eae572f6d20 | 183 | speedA = 1; //Set SpeedA to full |
raj1995 | 0:8eae572f6d20 | 184 | fwdB = 1; //Assert MotorB forward |
raj1995 | 0:8eae572f6d20 | 185 | revB = 0; //Deassert MotorB reverse |
raj1995 | 0:8eae572f6d20 | 186 | speedB = 1; //Set SpeedB to full |
raj1995 | 0:8eae572f6d20 | 187 | } else { |
raj1995 | 0:8eae572f6d20 | 188 | //Let go of button |
raj1995 | 0:8eae572f6d20 | 189 | forward_led = 0; //Turn off forward LED |
raj1995 | 0:8eae572f6d20 | 190 | speedA = 0; // Turn off MotorA |
raj1995 | 0:8eae572f6d20 | 191 | speedB = 0; // Turn off MotorB |
raj1995 | 0:8eae572f6d20 | 192 | } |
raj1995 | 0:8eae572f6d20 | 193 | break; |
raj1995 | 0:8eae572f6d20 | 194 | case '6': //button 6 down arrow |
raj1995 | 0:8eae572f6d20 | 195 | if (bhit=='1') { |
raj1995 | 0:8eae572f6d20 | 196 | //reverse |
raj1995 | 0:8eae572f6d20 | 197 | //hit button |
raj1995 | 0:8eae572f6d20 | 198 | reverse_led=1; //Turn reverse LED on |
raj1995 | 0:8eae572f6d20 | 199 | fwdA = 0; //Deassert MotorA forward |
raj1995 | 0:8eae572f6d20 | 200 | revA = 1; //Assert MotorA reverse |
raj1995 | 0:8eae572f6d20 | 201 | speedA = 1; //MotorA full speed |
raj1995 | 0:8eae572f6d20 | 202 | fwdB = 0; //Deassert MotorB forward |
raj1995 | 0:8eae572f6d20 | 203 | revB = 1; //Assert MotorB reverse |
raj1995 | 0:8eae572f6d20 | 204 | speedB = 1; //MotorB full speed |
raj1995 | 0:8eae572f6d20 | 205 | |
raj1995 | 0:8eae572f6d20 | 206 | |
raj1995 | 0:8eae572f6d20 | 207 | } else { |
raj1995 | 0:8eae572f6d20 | 208 | //let go of button |
raj1995 | 0:8eae572f6d20 | 209 | reverse_led=0; //Turn reverse LED off |
raj1995 | 0:8eae572f6d20 | 210 | speedA = 0; //Turn off MotorA |
raj1995 | 0:8eae572f6d20 | 211 | speedB = 0; //Turn off MotorB |
raj1995 | 0:8eae572f6d20 | 212 | } |
raj1995 | 0:8eae572f6d20 | 213 | break; |
raj1995 | 0:8eae572f6d20 | 214 | case '7': //button 7 left arrow |
raj1995 | 0:8eae572f6d20 | 215 | if (bhit=='1') { |
raj1995 | 0:8eae572f6d20 | 216 | //ccw |
raj1995 | 0:8eae572f6d20 | 217 | //hit button |
raj1995 | 0:8eae572f6d20 | 218 | left_led=1; //Turn left LED on |
raj1995 | 0:8eae572f6d20 | 219 | fwdA = 1; //Assert MotorA forward |
raj1995 | 0:8eae572f6d20 | 220 | revA = 0; //Deassert MotorA reverse |
raj1995 | 0:8eae572f6d20 | 221 | speedA = 1; //MotorA full speed |
raj1995 | 0:8eae572f6d20 | 222 | fwdB = 0; //Deassert MotorB forward |
raj1995 | 0:8eae572f6d20 | 223 | revB = 1; //Assert MotorB reverse |
raj1995 | 0:8eae572f6d20 | 224 | speedB = 1; //MotorB full speed |
raj1995 | 0:8eae572f6d20 | 225 | |
raj1995 | 0:8eae572f6d20 | 226 | |
raj1995 | 0:8eae572f6d20 | 227 | } else { |
raj1995 | 0:8eae572f6d20 | 228 | //let go of button |
raj1995 | 0:8eae572f6d20 | 229 | left_led=0; //Turn left LED off |
raj1995 | 0:8eae572f6d20 | 230 | speedA = 0; // Turn MotorA off |
raj1995 | 0:8eae572f6d20 | 231 | speedB = 0; // Turn MotorB off |
raj1995 | 0:8eae572f6d20 | 232 | } |
raj1995 | 0:8eae572f6d20 | 233 | break; |
raj1995 | 0:8eae572f6d20 | 234 | case '8': //button 8 right arrow |
raj1995 | 0:8eae572f6d20 | 235 | if (bhit=='1') { |
raj1995 | 0:8eae572f6d20 | 236 | //cw |
raj1995 | 0:8eae572f6d20 | 237 | //hit button |
raj1995 | 0:8eae572f6d20 | 238 | right_led=1; //Turn right LED on |
raj1995 | 0:8eae572f6d20 | 239 | fwdA = 0; //Deassert MotorA forward |
raj1995 | 0:8eae572f6d20 | 240 | revA = 1; //Assert MotorA reverse |
raj1995 | 0:8eae572f6d20 | 241 | speedA = 1; //MotorA full speed |
raj1995 | 0:8eae572f6d20 | 242 | fwdB = 1; //Assert MotorB forward |
raj1995 | 0:8eae572f6d20 | 243 | revB = 0; //Deassert MotorB reverse |
raj1995 | 0:8eae572f6d20 | 244 | speedB = 1; //MotorB full speed |
raj1995 | 0:8eae572f6d20 | 245 | |
raj1995 | 0:8eae572f6d20 | 246 | } else { |
raj1995 | 0:8eae572f6d20 | 247 | //let go of button |
raj1995 | 0:8eae572f6d20 | 248 | right_led=0; //Turn right LED off |
raj1995 | 0:8eae572f6d20 | 249 | speedA = 0; //Turn MotorA off |
raj1995 | 0:8eae572f6d20 | 250 | speedB = 0; //Turn MotorB off |
raj1995 | 0:8eae572f6d20 | 251 | } |
raj1995 | 0:8eae572f6d20 | 252 | break; |
raj1995 | 0:8eae572f6d20 | 253 | default: |
raj1995 | 0:8eae572f6d20 | 254 | break; |
raj1995 | 0:8eae572f6d20 | 255 | } |
raj1995 | 0:8eae572f6d20 | 256 | } |
raj1995 | 0:8eae572f6d20 | 257 | break; |
raj1995 | 0:8eae572f6d20 | 258 | } |
raj1995 | 0:8eae572f6d20 | 259 | |
raj1995 | 0:8eae572f6d20 | 260 | } |
raj1995 | 0:8eae572f6d20 | 261 | |
raj1995 | 0:8eae572f6d20 | 262 | } |
raj1995 | 0:8eae572f6d20 | 263 | } |
raj1995 | 0:8eae572f6d20 | 264 | } |