File content as of revision 0:21a2de59d16f:

#include "mbed.h"
#include "Adafruit_SSD1306.h"
#include "LSM303D.h"
#include "QEI.h"
#include "MSCFileSystem.h"
#include "SDHCFileSystem.h"
#include "PololuQik2.h"
#include "GPS.h"
#include "Servo.h"

// an SPI sub-class that provides a constructed default format and frequency
class SPI2 : public SPI
{
public:
    SPI2(PinName mosi, PinName miso, PinName clk) : SPI(mosi,miso,clk)
    {
        format(8,3);
        frequency(2000000);
    };
};

//Serial pc(USBTX,USBRX);
SPI2 gSpi(p5,p6,p7);
Adafruit_SSD1306 gOled(gSpi,p8,p11,p12);
LSM303D comp(gSpi,p15);
QEI Left(p30,p29,NC,48,QEI::X4_ENCODING);  // encoder object for Left wheel
QEI Right(p17,p16,NC,48,QEI::X4_ENCODING);  // encoder object for Left wheel
SDFileSystem sd(p5, p6, p7, p26, "sd"); // mosi, miso, sclk, cs
PololuQik2 motors(p13,NC);
Serial computer(p28, p27); // tx, rx read from bluetooth module
GPS gpsAda(NC,p14,9600);
AnalogIn IR(p18);

Timer t;

int main()
{
    int pulses[2], check, mag[3], acc[3], i=0;
    float hdg;
    int distance;                 //Variable to store the distance calculated from the sensor
    int triggerDistance = 30;     //The distance we want the robot to look for a new path
    int fDistance;                //Variable to store the distance in front of the robot
    int lDistance;                //Variable to store the distance on the left side of the robot
    int rDistance;                //Variable to store the distance on the right side of the robot
    
    computer.baud(115200);
    motors.begin();
    
    //display CoyleBot Logo
    gOled.display();
    wait(1.0);
    
    //Initialization returns 1 if talking to right device
    check=comp.initialize();
    
    //Set scales and offset for compass, could spin at start-up to get
    comp.setOffset(0, 0, 0); // example calibration
    comp.setScale(1.00, 1.00, 1.00);    // example calibration
    
    t.reset();
    t.start();
    
    while(1)
     {        
        //Compass, Accelerometer Data
        mag[0]=comp.magnitometer(1000);
        mag[1]=comp.magnitometer(1000);
        mag[2]=comp.magnitometer(1000);
        acc[0]=comp.accelerometer(1000);
        acc[1]=comp.accelerometer(1000);
        acc[2]=comp.accelerometer(1000);
        hdg=comp.heading();
        
        //Get Wheel Encoder Pulses
        pulses[0]=Left.getPulses();
        pulses[1]=Right.getPulses();
        
        //Clear screen information so it can be written to
        gOled.clearDisplay();
        
        //Go to top of screen
        gOled.setCursor(20,20);
       void moveForward();
           { 
           //Print RC Commands to SCREEN
            gOled.printf("Forward  :");
            gOled.printf("%d %d %d %d %d %d %d %d %d %d %f %f \r\n", i, t.read_ms(),mag[0],mag[1],mag[2],acc[0],acc[1],acc[2],pulses[0],pulses[1],gpsAda.latitude,gpsAda.longitude);
            gOled.display();
            //sets the speed of the motor 0, must be a value between -255 and 255
            motors.setMotor0Speed(100);  
            //sets the speed of the motor 0, must be a value between -255 and 255
            motors.setMotor1Speed(100);
            }
        void moveBackward();
            {
              //Print RC Commands to SCREEN
            gOled.printf("Backward  :");
            gOled.printf("%d %d %d %d %d %d %d %d %d %d %f %f \r\n", i, t.read_ms(),mag[0],mag[1],mag[2],acc[0],acc[1],acc[2],pulses[0],pulses[1],gpsAda.latitude,gpsAda.longitude);
            gOled.display();
              //sets the speed of the motor 0, must be a value between -255 and 255
            motors.setMotor0Speed(-100);  
            //sets the speed of the motor 0, must be a value between -255 and 255
            motors.setMotor1Speed(-100);
            }
        
       int scan();                                //Get the distance retrieved
          fDistance = distance;                  //Set that distance to the front distance
          if(fDistance < triggerDistance){       //If there is something closer than 30cm in front of us
          moveBackward();                      //Move Backward for a second
          wait(1000); 
          void moveRight();                         //Turn Right for half a second
          wait(500);
          void moveStop();                          //Stop
          scan();                              //Take a reading
          rDistance = distance;                //Store that to the distance on the right side
          void moveLeft();
          wait(1000);                         //Turn left for a second
          moveStop();                          //Stop
          scan();                              //Take a reading
          lDistance = distance;                //Store that to the distance on the left side
          if(lDistance < rDistance){           //If the distance on the left is smaller than that of the right
          moveRight();                       //Move right for a second
          wait(1000);
          moveForward();                     //Then move forward
    }
        else
        {
      moveForward();                     //If the left side is larger than the right side move forward
        }
  }
         else
         {
    moveForward();                       //If there is nothing infront of the robot move forward
        }
}

  int scan()
      {
      time = sonar.ping();                  //Send out a ping and store the time it took for it to come back in the time variable
      distance = time / US_ROUNDTRIP_CM;    //Convert that time into a distance
      if(distance == 0){                    //If no ping was recieved
        distance = 100;                     //Set the distance to max
      }
  delay(10);


    void moveBackward()
        {
      rightServo.write(180);
      leftServo.write(0); 
         }

    void moveForward()
        {
          rightServo.write(0);
          leftServo.write(180);
        }

    void moveRight()
        {
          rightServo.write(0);
          leftServo.write(0);  
        }

    void moveLeft()
        {
      rightServo.write(180);
      leftServo.write(180);
        }

    void moveStop()
        {
          rightServo.write(90);
          leftServo.write(95); 
        }

       

        i++;
        
        
    }
}