File content as of revision 0:3c1eead7416c:

#include "ultrasonic.h"
#include "motordriver.h"

//ADXL
SPI acc(p11,p12,p13); // set up SPI interface on pins 11,12,13
DigitalOut cs(p14); // use pin 14 as chip select

//FRONT MOTORS
Motor FL(p22, p6, p5, 1); // pwm, fwd, rev, can brake 
Motor FR(p21, p7, p8, 1); 

//BACK MOTORS
Motor BL(p23, p17, p18, 1); 
Motor BR(p24, p15, p16, 1);

void drivefront(void);
void driveback(void);
void driveleft(void);
void driveright(void);
void dynamicdrivefront(void);

Serial pc(USBTX, USBRX);

char buffer[6]; //raw data array type char
int16_t data[3]; // 16-bit twos-complement integer data
float x, y, z; // floating point data, to be displayed on-screen

int sl,sr; //left sensor and right sensor
void dist(int distance)
{
    
    //put code here to execute when the distance has changed
    
  sl = distance;
  pc.printf("sl %d mm\r\n", sl);
}
void gist(int distance)
{
    
    //put code here to execute when the distance has changed
    
  sr = distance;
  pc.printf("sr %d mm\r\n", sr);
}
 
ultrasonic mu(p27, p28, .1, 1, &gist);    //Set the trigger pin to p27 and the echo pin to p28
ultrasonic lu(p10, p9, .1, 1, &dist);                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes
 
int main()
{ 
//ADXL{
cs=1; //initially ADXL345 is not activated
acc.format(8,3); // 8 bit data, Mode 3
acc.frequency(2000000); // 2MHz clock rate
cs=0; //select the device
acc.write(0x31); // data format register
acc.write(0x0B); // format +/-16g, 0.004g/LSB
cs=1; //end of transmission
cs=0; //start a new transmission
acc.write(0x2D); // power ctrl register
acc.write(0x08); // measure mode
cs=1; //end of transmission
//}ADXL
    lu.startUpdates();//start measuring the distance    
    mu.startUpdates();//start measuring the distance
    while(1)
    { 
        cs=0; //start a transmission
        acc.write(0x80|0x40|0x32); // RW bit high, MB bit high, plus address
        for (int i = 0;i<=5;i++) {
        buffer[i]=acc.write(0x00); // read back 6 data bytes
        }
        cs=1; //end of transmission
        data[0] = buffer[1]<<8 | buffer[0]; // combine MSB and LSB
        data[1] = buffer[3]<<8 | buffer[2];
        data[2] = buffer[5]<<8 | buffer[4];
        x=0.004*data[0]; y=0.004*data[1]; z=0.004*data[2]; // convert to float,
        //actual g value

      // pc.printf("x = %+1.2fg\t y = %+1.2fg\t z = %+1.2fg\n\r", x, y,z); //print 
    
     lu.checkDistance(); 
        mu.checkDistance(); 
        
            if (sl>200 && sr>200){
                            if(y>=0.26){//o.26=sin(15) to signify elevation above that point
                            dynamicdrivefront();
                            }
                            else{
                                drivefront();
                            }
                            }
                           else if(sl >150 && sr<=150){
                                driveleft();
                                }
                               else if(sr >150 && sl<=150){
                                     driveright();
                                     }
                                   else{
                                        driveback();
                                        wait(0.6);
                                        if(sl<sr){
                                           driveright();}
                                           else{
                                                driveleft();
                                                    }           
                                                }
                                        }
        
       
                            
    }


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void drivefront(void)
{ 
FL.speed(0.4);
FR.speed(0.4);
BL.speed(0.4);
BR.speed(0.4);
//wait(0.1);
}

void driveback(void)
{
FL.speed(-0.4);
FR.speed(-0.4);
BL.speed(-0.4);
BR.speed(-0.4);
//wait(0.1);   
    }
void driveleft(void)
{
FL.speed(-0.5);
FR.speed(0.4);
BL.speed(-0.5);
BR.speed(0.3);
//wait(0.1);
    }
void driveright(void)
{
FL.speed(0.4);
FR.speed(-0.5);
BL.speed(0.3);
BR.speed(-0.5);
//wait(0.1);
    }
void dynamicdrivefront(void)
{ 
FL.speed(0.5+((y-0.1)/2));
FR.speed(0.5+((y-0.1)/2));
BL.speed(0.5+((y-0.1)/2));
BR.speed(0.5+((y-0.1)/2));
//wait(0.1);
}