Final Version.
Dependencies: Motor Servo mbed
main.cpp
- Committer:
- raj1995
- Date:
- 2016-03-11
- Revision:
- 0:8eae572f6d20
File content as of revision 0:8eae572f6d20:
/* @Authors: Raj Patel, David Ehrlich @Project: Bluetooth Controlled Robot with Dual Servo Sensor Actuated by Mobile Phone Accelerometer @Date: 03/09/2016 */ //include necessary header files for functionality. Specifically, need servo handlers. #include "mbed.h" #include "Servo.h" Serial bluemod(p9,p10); // Serial hookup for Bluetooth module PwmOut left_led(LED3); // PWM output for left robot direction signal PwmOut right_led(LED4); // PWM output for right robot direction signal PwmOut forward_led(LED1); // PWM output for forward robot direction signal PwmOut reverse_led(LED2); // PWM output for reverse robot direction signal Servo myservolr(p21); // Signal for left/right servo Servo myservoud(p22); // Signal for up/down servo AnalogIn sensor(p15); // Signal for Sharp IR Sensor PwmOut speedA(p23); // PWM Speed pin for Motor A DigitalOut fwdA(p17); // Digital out signal for forward direction for Motor A DigitalOut revA(p18); // Digital out signal for reverse direction for Motor A PwmOut speedB(p24); // PWM Speed pin for Motor B DigitalOut fwdB(p20); // Digital out signal for forward direction for Motor B DigitalOut revB(p19); // Digital out signal for reverse direction for Motor B volatile char exc; // Variable to keep track of ! when seen in BT stream volatile char letter; // Varibale to keep track of character after ! in BT stream (A for accelerometer, B for buttons) //C union can convert 4 chars to a float - puts them in same location in memory //trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float union f_or_char { float f; char c[4]; }; int main() { char bchecksum=0; // Checksum variable char temp=0; // Temporary variable to hold BT stream data union f_or_char x,y,z; // Templatized containers for x, y, and z accelerometer readings float adjlr = 0; // Servo position variable for left/right servo float adjud = 0; // Servo position variable for up/down servo while(1) { float sensorValue = sensor.read(); //printf("Sensor: %2.2f\n\r", sensorValue); bchecksum=0; exc = bluemod.getc(); // Get the first character in the bluetooth packet if (exc=='!'){ // First character was ! meaning it could be !A or !B letter = bluemod.getc(); // Get the next character in the bluetooth packet after the ! switch (letter) { case 'A':{ // If the next character is A, it is an Accelerometer data packet for (int i=0; i<4; i++) { temp = bluemod.getc(); x.c[i] = temp; // Get x direction Accelerometer data bchecksum = bchecksum + temp; } for (int i=0; i<4; i++) { temp = bluemod.getc(); y.c[i] = temp; // Get y direction Accelerometer data bchecksum = bchecksum + temp; } for (int i=0; i<4; i++) { temp = bluemod.getc(); z.c[i] = temp; // Get z direction Accelerometer data bchecksum = bchecksum + temp; } if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK? Then proceed. // Both segments of the below code in this 'if' statement handle actuating the dual servo setup // Below code segment handles left/right movement if (y.f < -0.05 && y.f >= -0.10){ adjlr += 0.01; // If the y direction shows a small negative value, move left slowly } if (y.f < -0.10 && y.f >= -0.25){ adjlr += 0.05; // If the y direction shows a medium negative value, move left faster } if (y.f < -0.25){ adjlr += 0.09; // If the y direction shows a large negative value, move left fastest } if (y.f > 0.05 && y.f <= 0.10){ adjlr += -0.01; // If the y direction shows a small positive value, move right slowly } if (y.f > 0.10 && y.f <= 0.25){ adjlr += -0.05; // If the y direction shows a medium positive value, move right faster } if (y.f > 0.25){ adjlr += -0.09; // If the y direction shows a large positive value, move right fastest } if (adjlr < 0){ adjlr = 0; // If the value becomes less than 0, cap at 0 }else{ if (adjlr > 1){ adjlr = 1; // If the value becomes greater than 1, cap at 1 } } // Below code handles up/down movement if (x.f < -0.05 && x.f >= -0.2){ adjud += -0.01; // If the x direction shows a small negative value, move up slowly } if (x.f < -0.2 && x.f >= -0.40){ adjud += -0.05; // If the x direction shows a medium negative value, move up faster } if (x.f < -0.4){ adjud += -0.09; // If the x direction shows a large negative value, move up fastest } if (x.f > 0.05 && x.f <= 0.2){ adjud += 0.01; // If the x direction shows a small positive value, move down slowly } if (x.f > 0.2 && x.f <= 0.40){ adjud += 0.05; // If the x direction shows a medium positive value, move down faster } if (x.f > 0.40){ adjud += 0.09; // If the x direction shows a large positive value, move down fastest } if (adjud < 0){ adjud = 0; // If the value becomes less than 0, cap at 0 }else{ if (adjud > 1){ adjud = 1; // If the value becomes greater than 1, cap at 1 } } myservolr = adjlr; // Send adjusted left/right signal to left/right servo myservoud = adjud; // Send adjusted up/down signal to up/down servo } break; } char bnum; //Initialize variables to use below char bhit; //Initialize variables to use below case 'B':{ // If the next character is B, it is a Button data packet bnum = bluemod.getc(); //button number bhit = bluemod.getc(); //1=hit, 0=release if (bluemod.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? //myled = bnum - '0'; //current button number will appear on LEDs switch (bnum) { case '1': //number button 1 if (bhit=='1') { // nop } else { // nop } break; case '2': //number button 2 if (bhit=='1') { // nop } else { // nop } break; case '3': //number button 3 if (bhit=='1') { // nop } else { // nop } break; case '4': //number button 4 if (bhit=='1') { // nop } else { // nop } break; case '5': //button 5 up arrow if (bhit=='1' && sensorValue <= 0.6) { //forward //hit button forward_led=1; //Set forward LED on MBED fwdA = 1; //Assert MotorA forward revA = 0; //Deassert MotorA reverse speedA = 1; //Set SpeedA to full fwdB = 1; //Assert MotorB forward revB = 0; //Deassert MotorB reverse speedB = 1; //Set SpeedB to full } else { //Let go of button forward_led = 0; //Turn off forward LED speedA = 0; // Turn off MotorA speedB = 0; // Turn off MotorB } break; case '6': //button 6 down arrow if (bhit=='1') { //reverse //hit button reverse_led=1; //Turn reverse LED on fwdA = 0; //Deassert MotorA forward revA = 1; //Assert MotorA reverse speedA = 1; //MotorA full speed fwdB = 0; //Deassert MotorB forward revB = 1; //Assert MotorB reverse speedB = 1; //MotorB full speed } else { //let go of button reverse_led=0; //Turn reverse LED off speedA = 0; //Turn off MotorA speedB = 0; //Turn off MotorB } break; case '7': //button 7 left arrow if (bhit=='1') { //ccw //hit button left_led=1; //Turn left LED on fwdA = 1; //Assert MotorA forward revA = 0; //Deassert MotorA reverse speedA = 1; //MotorA full speed fwdB = 0; //Deassert MotorB forward revB = 1; //Assert MotorB reverse speedB = 1; //MotorB full speed } else { //let go of button left_led=0; //Turn left LED off speedA = 0; // Turn MotorA off speedB = 0; // Turn MotorB off } break; case '8': //button 8 right arrow if (bhit=='1') { //cw //hit button right_led=1; //Turn right LED on fwdA = 0; //Deassert MotorA forward revA = 1; //Assert MotorA reverse speedA = 1; //MotorA full speed fwdB = 1; //Assert MotorB forward revB = 0; //Deassert MotorB reverse speedB = 1; //MotorB full speed } else { //let go of button right_led=0; //Turn right LED off speedA = 0; //Turn MotorA off speedB = 0; //Turn MotorB off } break; default: break; } } break; } } } } }