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;
            }
            
            }
            
        }
    }
}