File content as of revision 0:b5ac3f893191:

#include "mbed.h"
#include "Servo.h"

Serial bt(p28,p27); //tx,rx

Servo pan(p24);
Servo tilt(p23);

PwmOut left(p21);
PwmOut right(p22);

DigitalOut fwdl(p5);
DigitalOut revl(p6);

DigitalOut laser(p7);

DigitalOut fwdr(p11);
DigitalOut revr(p12);

union packet {
    
    float f;
    char c[4];
    
};

int main() {
    
    char button, temp = 0;
    union packet x, y; // range is -10 to 10

    // initialize servo to middle, motors off, and laser off
    fwdl = revl = fwdr = revr = 0;
    left = right = 0.0;
    pan = 0.5;
    tilt = 0.225;
    laser = 0;
    
    while (1) {
        
        if (bt.getc() == '!') {
            
            button = bt.getc();
            switch (button) {

                case 'B':  // if button
                
                    button = bt.getc();
                    //state = bt.getc();
                    
                    switch (button) {
                        
                        case '1': // toggle the laser/digital output device on pin 7
                        
                            //if (state == '1') {
                                
                            laser = !laser;
                            break;    
                            
                            //}      
                                          
                        case '2': // recenter the servos
                        
                            //if (state == '1') {
                                
                            tilt = 0.225;
                            pan = 0.5;
                            wait(0.2);
                            break;
                            
                            //}
                        
                        case '5': // forward
                        
                            //if (state == '1') {
                                
                            if (tilt <= 0.05) {tilt = 0.0;}
                            else {tilt = tilt - 0.05;}
                            wait(0.1);
                            break;
                            
                            //}
                            
                        case '6': // backward
                            
                            //if (state == '1') {
                                
                            if (tilt >= 0.40) {tilt = 0.45;}
                            else {tilt = tilt + 0.05;}
                            wait(0.1);
                            break;
                            
                            //}
                            
                        case '7': // right
                        
                            //if (state == '1') {
                                
                            if (pan <= 0.05) {pan = 0.0;}
                            else {pan = pan - 0.05;}
                            wait(0.1);
                            break;
                            
                            //}
                            
                            
                        case '8': // left
                            
                            //if (state == '1') {
                                
                            if (pan >= 0.95) {pan = 1.0;}
                            else {pan = pan + 0.05;}
                            wait(0.1);
                            break;
                            
                           // }
                        
                    }
                    break; 
                
                case 'A': // otherwise accelerometer 
                    
                    // fetch and store accelerometer data
                    
                    for (int i = 0; i < 4; i++) {
                        
                        temp = bt.getc();
                        x.c[i] = temp;
                        
                    }
                    
                    for (int i = 0; i < 4; i++) {
                        
                        temp = bt.getc();
                        y.c[i] = temp;
                        
                    }
                    
                    // final loop to capture Z accelerometer data (NOT USED)
                    for (int i = 0; i < 4; i++) {

                        temp = bt.getc();

                    }

                    temp = bt.getc();
                    
                    // read accelerometer data and control motors
                    
                    if (x.f <= -2.0) {   // left
                    
                        fwdl = fwdr = 1;
                        revl = revr = 0;
                        left = 0.1;
                        right = 1.0;
                        
                    }
                    
                    else if (x.f >= 2.0) {  // right
                        
                        fwdl = fwdr = 1;
                        revl = revr = 0;
                        left = 1.0;
                        right = 0.1;
                            
                    }
                    
                    else { // if no tilt, make sure that motors are disengaged
                        
                        fwdl = fwdr = revl = revr = 0;
                        left = right = 0.0;    
                    }
                    
                    if (y.f <= -2.0) {   // reverse
                    
                        fwdl = fwdr = 0;
                        revl = revr = 1;
                        right = left = 1.0;
                            
                    }
                    
                    else if (y.f >= 2.0) {  // forward
                        
                        fwdl = fwdr = 1;
                        revl = revr = 0;
                        right = left = 1.0;
                            
                    }
        
                    break;

            }
        }
    }
}