Raj Patel
/
4180_Lab_4_Basic
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Motor Servo mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 00002 00003 @Authors: Raj Patel, David Ehrlich 00004 @Project: Bluetooth Controlled Robot with Dual Servo Sensor Actuated by Mobile Phone Accelerometer 00005 @Date: 03/09/2016 00006 00007 */ 00008 00009 //include necessary header files for functionality. Specifically, need servo handlers. 00010 00011 #include "mbed.h" 00012 #include "Servo.h" 00013 00014 Serial bluemod(p9,p10); // Serial hookup for Bluetooth module 00015 PwmOut left_led(LED3); // PWM output for left robot direction signal 00016 PwmOut right_led(LED4); // PWM output for right robot direction signal 00017 PwmOut forward_led(LED1); // PWM output for forward robot direction signal 00018 PwmOut reverse_led(LED2); // PWM output for reverse robot direction signal 00019 Servo myservolr(p21); // Signal for left/right servo 00020 Servo myservoud(p22); // Signal for up/down servo 00021 AnalogIn sensor(p15); // Signal for Sharp IR Sensor 00022 00023 PwmOut speedA(p23); // PWM Speed pin for Motor A 00024 DigitalOut fwdA(p17); // Digital out signal for forward direction for Motor A 00025 DigitalOut revA(p18); // Digital out signal for reverse direction for Motor A 00026 00027 PwmOut speedB(p24); // PWM Speed pin for Motor B 00028 DigitalOut fwdB(p20); // Digital out signal for forward direction for Motor B 00029 DigitalOut revB(p19); // Digital out signal for reverse direction for Motor B 00030 00031 00032 volatile char exc; // Variable to keep track of ! when seen in BT stream 00033 volatile char letter; // Varibale to keep track of character after ! in BT stream (A for accelerometer, B for buttons) 00034 00035 //C union can convert 4 chars to a float - puts them in same location in memory 00036 //trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float 00037 union f_or_char { 00038 float f; 00039 char c[4]; 00040 }; 00041 00042 int main() 00043 { 00044 char bchecksum=0; // Checksum variable 00045 char temp=0; // Temporary variable to hold BT stream data 00046 union f_or_char x,y,z; // Templatized containers for x, y, and z accelerometer readings 00047 float adjlr = 0; // Servo position variable for left/right servo 00048 float adjud = 0; // Servo position variable for up/down servo 00049 while(1) { 00050 float sensorValue = sensor.read(); 00051 //printf("Sensor: %2.2f\n\r", sensorValue); 00052 bchecksum=0; 00053 exc = bluemod.getc(); // Get the first character in the bluetooth packet 00054 if (exc=='!'){ // First character was ! meaning it could be !A or !B 00055 letter = bluemod.getc(); // Get the next character in the bluetooth packet after the ! 00056 switch (letter) { 00057 case 'A':{ // If the next character is A, it is an Accelerometer data packet 00058 for (int i=0; i<4; i++) { 00059 temp = bluemod.getc(); 00060 x.c[i] = temp; // Get x direction Accelerometer data 00061 bchecksum = bchecksum + temp; 00062 } 00063 for (int i=0; i<4; i++) { 00064 temp = bluemod.getc(); 00065 y.c[i] = temp; // Get y direction Accelerometer data 00066 bchecksum = bchecksum + temp; 00067 } 00068 for (int i=0; i<4; i++) { 00069 temp = bluemod.getc(); 00070 z.c[i] = temp; // Get z direction Accelerometer data 00071 bchecksum = bchecksum + temp; 00072 } 00073 if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK? Then proceed. 00074 00075 // Both segments of the below code in this 'if' statement handle actuating the dual servo setup 00076 00077 // Below code segment handles left/right movement 00078 if (y.f < -0.05 && y.f >= -0.10){ 00079 adjlr += 0.01; // If the y direction shows a small negative value, move left slowly 00080 } 00081 if (y.f < -0.10 && y.f >= -0.25){ 00082 adjlr += 0.05; // If the y direction shows a medium negative value, move left faster 00083 } 00084 if (y.f < -0.25){ 00085 adjlr += 0.09; // If the y direction shows a large negative value, move left fastest 00086 } 00087 if (y.f > 0.05 && y.f <= 0.10){ 00088 adjlr += -0.01; // If the y direction shows a small positive value, move right slowly 00089 } 00090 if (y.f > 0.10 && y.f <= 0.25){ 00091 adjlr += -0.05; // If the y direction shows a medium positive value, move right faster 00092 } 00093 if (y.f > 0.25){ 00094 adjlr += -0.09; // If the y direction shows a large positive value, move right fastest 00095 } 00096 if (adjlr < 0){ 00097 adjlr = 0; // If the value becomes less than 0, cap at 0 00098 }else{ 00099 if (adjlr > 1){ 00100 adjlr = 1; // If the value becomes greater than 1, cap at 1 00101 } 00102 } 00103 00104 // Below code handles up/down movement 00105 if (x.f < -0.05 && x.f >= -0.2){ 00106 adjud += -0.01; // If the x direction shows a small negative value, move up slowly 00107 } 00108 if (x.f < -0.2 && x.f >= -0.40){ 00109 adjud += -0.05; // If the x direction shows a medium negative value, move up faster 00110 } 00111 if (x.f < -0.4){ 00112 adjud += -0.09; // If the x direction shows a large negative value, move up fastest 00113 } 00114 if (x.f > 0.05 && x.f <= 0.2){ 00115 adjud += 0.01; // If the x direction shows a small positive value, move down slowly 00116 } 00117 if (x.f > 0.2 && x.f <= 0.40){ 00118 adjud += 0.05; // If the x direction shows a medium positive value, move down faster 00119 } 00120 if (x.f > 0.40){ 00121 adjud += 0.09; // If the x direction shows a large positive value, move down fastest 00122 } 00123 if (adjud < 0){ 00124 adjud = 0; // If the value becomes less than 0, cap at 0 00125 }else{ 00126 if (adjud > 1){ 00127 adjud = 1; // If the value becomes greater than 1, cap at 1 00128 } 00129 } 00130 00131 myservolr = adjlr; // Send adjusted left/right signal to left/right servo 00132 myservoud = adjud; // Send adjusted up/down signal to up/down servo 00133 00134 00135 } 00136 break; 00137 } 00138 char bnum; //Initialize variables to use below 00139 char bhit; //Initialize variables to use below 00140 00141 case 'B':{ // If the next character is B, it is a Button data packet 00142 00143 bnum = bluemod.getc(); //button number 00144 bhit = bluemod.getc(); //1=hit, 0=release 00145 if (bluemod.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? 00146 //myled = bnum - '0'; //current button number will appear on LEDs 00147 switch (bnum) { 00148 case '1': //number button 1 00149 if (bhit=='1') { 00150 // nop 00151 } else { 00152 // nop 00153 } 00154 break; 00155 case '2': //number button 2 00156 if (bhit=='1') { 00157 // nop 00158 } else { 00159 // nop 00160 } 00161 break; 00162 case '3': //number button 3 00163 if (bhit=='1') { 00164 // nop 00165 } else { 00166 // nop 00167 } 00168 break; 00169 case '4': //number button 4 00170 if (bhit=='1') { 00171 // nop 00172 } else { 00173 // nop 00174 } 00175 break; 00176 case '5': //button 5 up arrow 00177 if (bhit=='1' && sensorValue <= 0.6) { 00178 //forward 00179 //hit button 00180 forward_led=1; //Set forward LED on MBED 00181 fwdA = 1; //Assert MotorA forward 00182 revA = 0; //Deassert MotorA reverse 00183 speedA = 1; //Set SpeedA to full 00184 fwdB = 1; //Assert MotorB forward 00185 revB = 0; //Deassert MotorB reverse 00186 speedB = 1; //Set SpeedB to full 00187 } else { 00188 //Let go of button 00189 forward_led = 0; //Turn off forward LED 00190 speedA = 0; // Turn off MotorA 00191 speedB = 0; // Turn off MotorB 00192 } 00193 break; 00194 case '6': //button 6 down arrow 00195 if (bhit=='1') { 00196 //reverse 00197 //hit button 00198 reverse_led=1; //Turn reverse LED on 00199 fwdA = 0; //Deassert MotorA forward 00200 revA = 1; //Assert MotorA reverse 00201 speedA = 1; //MotorA full speed 00202 fwdB = 0; //Deassert MotorB forward 00203 revB = 1; //Assert MotorB reverse 00204 speedB = 1; //MotorB full speed 00205 00206 00207 } else { 00208 //let go of button 00209 reverse_led=0; //Turn reverse LED off 00210 speedA = 0; //Turn off MotorA 00211 speedB = 0; //Turn off MotorB 00212 } 00213 break; 00214 case '7': //button 7 left arrow 00215 if (bhit=='1') { 00216 //ccw 00217 //hit button 00218 left_led=1; //Turn left LED on 00219 fwdA = 1; //Assert MotorA forward 00220 revA = 0; //Deassert MotorA reverse 00221 speedA = 1; //MotorA full speed 00222 fwdB = 0; //Deassert MotorB forward 00223 revB = 1; //Assert MotorB reverse 00224 speedB = 1; //MotorB full speed 00225 00226 00227 } else { 00228 //let go of button 00229 left_led=0; //Turn left LED off 00230 speedA = 0; // Turn MotorA off 00231 speedB = 0; // Turn MotorB off 00232 } 00233 break; 00234 case '8': //button 8 right arrow 00235 if (bhit=='1') { 00236 //cw 00237 //hit button 00238 right_led=1; //Turn right LED on 00239 fwdA = 0; //Deassert MotorA forward 00240 revA = 1; //Assert MotorA reverse 00241 speedA = 1; //MotorA full speed 00242 fwdB = 1; //Assert MotorB forward 00243 revB = 0; //Deassert MotorB reverse 00244 speedB = 1; //MotorB full speed 00245 00246 } else { 00247 //let go of button 00248 right_led=0; //Turn right LED off 00249 speedA = 0; //Turn MotorA off 00250 speedB = 0; //Turn MotorB off 00251 } 00252 break; 00253 default: 00254 break; 00255 } 00256 } 00257 break; 00258 } 00259 00260 } 00261 00262 } 00263 } 00264 }
Generated on Mon Jul 18 2022 07:54:32 by
1.7.2