File content as of revision 3:01b5e80d842d:

#define PI 3.141

#include "math.h"
#include "mbed.h"
#include "PID.h"
#include "Encoder.h"
#include "lineSensor.h"
#include "P.h"
#include "Wheel.h"
#include "Robot.h"


//blue D8
//Green D9
//Red D5
int i = 0;
int main() {
    Serial pc(USBTX, USBRX);
    
    DigitalOut enable(PA_13);
    enable.write(1);
    
    Encoder* RE = new Encoder(PB_3,PB_5);
    Encoder* LE = new Encoder(PB_10,PB_4);
    Wheel* rightWheel = new Wheel(RE,PC_8,PA_9, PA_14);
    Wheel* leftWheel = new Wheel(LE,PC_6,PA_8, PA_7);
    //an array of lineSensor pointers params: lineSensor(PinName emitter Pin, PinName reciever Pin, make sure it is from LEFT TO RIGHT
    
    
    pc.printf("Stage 1 speed %f\r\n", rightWheel->returnAngularVelocity());
    
    rightWheel->init(0);
    while(rightWheel->returnAngularVelocity() != 0)
    {
     pc.printf("Stage 2, speed %f\r\n",rightWheel->returnAngularVelocity()); 
    }
    leftWheel->init(0);
    while(leftWheel->returnAngularVelocity() != 0)
    {
     pc.printf("Stage 2, speed %f\r\n",leftWheel->returnAngularVelocity()); 
    }

    while(1)
    {
        i++;
        if (i >= 10)
        {
        i = 0;
        pc.printf("speed %f\r\n", rightWheel->returnAngularVelocity()); 
        pc.printf("speed %f\r\n", leftWheel->returnAngularVelocity());
        }
    }
}

/* ineSensor* sensorArray[6] = {new lineSensor(D11,A0),new lineSensor(D7,A1),new lineSensor(D6,A2),new lineSensor(D5,A3),new lineSensor(D4,A4),new lineSensor(D3,A5)}; */