Raj Patel / Mbed 2 deprecated 4180_Lab_4_Basic

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?

UserRevisionLine numberNew 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 }