Final Version.
Dependencies: Motor Servo mbed
Diff: main.cpp
- Revision:
- 0:8eae572f6d20
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 11 18:59:21 2016 +0000 @@ -0,0 +1,264 @@ +/* + +@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; + } + + } + + } + } +} \ No newline at end of file