File content as of revision 0:d200f455dbd4:

/*
ME503 Unmanned Autonomous Vehicle Systems
Dr. E. Coyle
Wall Following Assignment
L. Cantero, B. Wallace
*/

#include "mbed.h"
#include "Adafruit_SSD1306.h"
#include "LSM303D.h"
#include "QEI.h"
#include "MSCFileSystem.h"
#include "SDHCFileSystem.h"
#include "GPS.h"
#include "PololuQik2.h"
#include <math.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);

// Function prototypes
void splash(); 
void move(int speed0, int speed1);
        
int main()
{   
    computer.baud(230400);
    int i=0, speed0 = 60, speed1 = 60, read;
    double percent0 = 1.00, percent1 = 1.00;
    
    motors.begin();                // Initiate motor control
    splash();                      // Display splash screen
    
    move(speed0, speed1);
            
    while(1)
    {
        if(computer.readable())
        {
            read=computer.getc();
            
            if(read != 255)
            {
                
                percent0 = (read / 100.00);
                percent1 = (2 - percent0);
                
                move(floor(percent0*speed0), floor(percent1*speed1));
            }
            
            else
            {
                move(0,0);
            }
        }
        
        gOled.clearDisplay();
        gOled.setCursor(0,0);   
        gOled.printf("Left Motor: %.2f%%\nRight Motor: %.2f%%\nTime: %d",percent0*100, percent1*100,i);
        gOled.display();  
        i++;
    } 
}


// FUNCTIONS______________________________________________________

void splash() // Displays splash screen for 3 seconds, then clears buffer and resets cursor
{
    gOled.display();
    wait(3.0);
    gOled.clearDisplay();
    gOled.setCursor(0,0); // Splash screen stays until next display call
}

void move(int speed0, int speed1) // Move robot using defined wheel speeds (corrects for wiring-polarity issue and mirroring of right motor)
{
    motors.setMotor0Speed(speed0);
    motors.setMotor1Speed(-speed1);
}