File content as of revision 0:9d67126e11c8:

#include <mbed.h>


/* One enable, forward or backwards, controlling both sides
4 PWM lines, two for each side, one controlling forward speed and one for backward speed for EACH side
driving sequence is pwm off, enable toggle, pwm on */

// motor "enable"
//DigitalOut motorEnableLeft(PTC12); // right motor "enable" (1 = forward, 0 = reverse)
//DigitalOut motorEnableRight(PTA13);


// motor PWM controls
// left motors forward speed control (blue wire) -> PTA5
// right motors forward speed control (green wire) -> PTA4
// left motors backward speed control (yellow wire) -> PTD4
// right motors backward speed control (white wire) -> PTA12

//Left Enable (... wire) -> PTC12
//Right Enable (... wire) -> PTA13

// In order to stop, PWM duty cycle must be = 0
// Enable controls direction wheels spin'


typedef struct PwmDir{
    //PwmDir contains two PWM, on for the left motor one for the right motor
    DigitalOut motorEnable;
    PwmOut motorBw;
    PwmOut motorFw;
    
    //constructor, when create a struct, need to specify the pin it will take.
    //will need to set the pwm value outisde the struct
    PwmDir(PinName pin1, PinName pin2, PinName pin3) : motorEnable(pin1), motorBw(pin2), motorFw(pin3) {};
    
    }PwmDir;

//initialize the pwms and their pin numbers here, 1 enable 2 PwmBw 3 PwmFw
    PwmDir pwmLeft(PTC12,PTD4,PTA5);
    PwmDir pwmRight(PTA13,PTA12,PTA4);
    
    
PwmOut* SetDirection(bool direction,bool pwmSelect){
    /*set direction of one of the sides depending on pwmSelect
    direction : 0 go forward  ,1 go backwards
    pwmSelect : 0 left motors ,1 right motors */    
    
    if(!pwmSelect){
    //modify the left motor's direction
    //set every pwm to 0
    pwmLeft.motorBw.write(0);
    pwmLeft.motorFw.write(0);
    
    wait(0.1);
        
        if(!direction){
            pwmLeft.motorEnable = false;
            return &(pwmLeft.motorFw);
            }
        else{
            pwmLeft.motorEnable = true;
            return &(pwmLeft.motorBw);
            }  
    }else{
    //modify right motors direction
    //set every pwm to 0
    pwmRight.motorBw.write(0);
    pwmRight.motorFw.write(0);
    
    wait(0.1);
    
        if(!direction){
            pwmRight.motorEnable = false;
            return &(pwmRight.motorFw);
            }
        else{
            pwmRight.motorEnable = true;
            return &(pwmRight.motorBw);
            }
        }
    }

void SetSpeed(float leftSpeed, float rightSpeed, PwmOut *pwmLeftPtr, PwmOut *pwmRightPtr) {
    // set speed of either forward or backward PWMs
    // set both wheels to same speed
    // NOTE: set enable BEFORE calling function
    // speed: speed to be set
    // mRight & mLeft: pass PwmOut names of relevant Right and Left motor controls
    
    if(speed<=1.0 && speed > 0.0){
        pwmLeftPtr->write(speed);
        pwmRightPtr->write(speed);
    }else if(speed < 0 && speed >= 1.0){
        SetDirection(0, 0);
        SetDirection(0, 1);   
    }
    
}

void SetSpeed(float speed, PwmOut *pwmDirPtr) {
    // set speed of passed the pointer to PwmOut  
    // sets speed to only right or left depending on which pointer is passed
    
    if(speed<=1.0){
        (*pwmDirPtr).write(speed);
    }
}
//-----------------------------------------------------------
    

    
typedef struct prevDirect{
    bool prevDirL;
    bool prevDirR; 
}prevDir;


//sampling is a repeated interrupt that gathers the data from the IRsensors
Ticker sampling;

DigitalIn leftIR(PTC6);
DigitalIn midLeftIR(PTC5);
DigitalIn midIR(PTC4);
DigitalIn midRightIR(PTC3);
DigitalIn rightIR(PTC0);

bool lineSensor[5], toggle2 = false;


void Sample(){
    //function attached to the ticker
    //assigns the data recieved for each digital in into an array
    //toggle is toggled at every ISR, signifying the ISR has occured
    
    lineSensor[0] = leftIR;
    lineSensor[1] = midLeftIR;
    lineSensor[2] = midIR;
    lineSensor[3] = midRightIR;
    lineSensor[4] = rightIR;
    
    toggle2 = !toggle2;
}

//algorithm to be done when we have the final version of the hardware
int WeightPID(bool toggle, int error){
    //assign the weight of the error depending on which sensor is on
    //adds a count to change the weight when two sensors are on to change 
    
    int i;
    int count = 0;
    
    //if the interrupt has occured, pursue the PID calculations
        
    for(i = 0; i < 5; i++){
        if(lineSensor[i] && !count){
            count++;
            error = 2*(i-2);
                
                    
        } else if(lineSensor[i] && count == 1){
            error++;
            count++;
                    
        }else if(lineSensor[i] && count == 2){
            //decide to turn right or left depending on the colour of the disk
            //should be defined by a boolean, with 0 equals left and 1 equals right or vice versa                
        }
    }    
    
    if(!count){
        if(error == 2 || error == -2) error = 0;
        else error<0 ? error = -5 : error = 5;
        }  
        
    return  error; 
}



float CalculatePID(int error, int *previousError){
    //as name suggests, calculates the proportions of corrections to be applied to the motors
    //error : error given by weightPID
    //*previousError : pointer to the previous error calculated by weightPID
    //returns the PID value
    
    float P = 0.0, I = 0.0, D = 0.0;
    float Kp = 1.0, Ki = 0.0, Kd = 1.0;
    
    P = error;
    I = I + error;
    D = error - *previousError;
        
    *previousError = error;
    
    return Kp*P + Ki*I + Kd*D;
    
}



prevDirect MotorControl(float PIDValue, float initSpeed, prevDirect prevDir){
    //assigns the calculated direction to the motors
    //PIDvalue : calculated PID value given by CalculatePID
    //initSpeed : speed without correction 
    //check previousSpeed to make sure the direction is the same
    
    
    float highThresh = 0.2, lowThresh = 0.2;
    float maxPID = 5.0, minPID = -5.0;
    int dirLeft, dirRight;
    bool left
    
    //assign new speed with PID value and scale it
    float leftSpeed, rightSpeed;
    
    //scale and assign speed
    PIDValue = PIDValue / 5;
    leftSpeed = initSpeed - PIDValue;
    rightSpeed = initSpeed + PIDValue;
    
    leftSpeed > 0 ? dirLeft = true : dirLeft = false;
    if(leftSpeed == prevDir.prevDirL) setDirection(...);
    
    righSpeed > 0 ? dirRight = true : dirRight = false;
    if(rightSpeed == prevDir.prevDirR) setDirection(...);
    
    SetSpeed(...)
    
    
    
    return prevDir
}

//------------------------------------------------




int main(){
    
    //pointers which point to either PWM forward or backwards of left and right
    PwmOut *pwmLeftPtr;
    PwmOut *pwmRightPtr;
    
    //Assign pwm forward to the pointers
    pwmLeftPtr = &(pwmLeft.motorFw);
    pwmRightPtr = &(pwmRight.motorFw);
    
    //give a period of 1khz to every pwm
    pwmLeft.motorBw.period_ms(1);
    pwmLeft.motorFw.period_ms(1);
    pwmRight.motorBw.period_ms(1);
    pwmRight.motorFw.period_ms(1);
    
    //duty cycle of 0
    pwmLeft.motorBw.write(0);
    pwmLeft.motorFw.write(0);
    pwmRight.motorBw.write(0);
    pwmRight.motorFw.write(0);
    
    
    float speed = 0.2;
    //float angle = 1.0;
    bool directionLeft = 0;
    bool directionRight = 0;

    //first set direction left and right to forward 
    pwmLeftPtr = SetDirection(directionLeft, 0);
    pwmRightPtr = SetDirection(directionRight, 1);
    
    
    
    
    
    short int error = 0, previousError, i;
    int *previousErrorPtr;
    float PIDValue = 0, initSpeed = 0.3, prevSpeedL, prevSpeedR;
    bool toggle = false;
    
    prevDir.prevDirL = directionLeft;
    prevDir.prevDirR = directionRight;
        
    previousErrorPtr = &previousError;
    
    //initialize sensor array with 0s
    for(i = 0; i<5; i++) lineSensor[i] = 0;
    
    //get a first reading
    Sample();
    sampling.attach(&Sample, 0.01);
    
    prevSpeedL = initSpeed;
    prevSpeedR = initSpeed;
    
    while(1){
    
    //check if interrupt has occured
        if(toggle != toggle2){
            error = WeightPID(toggle, error);
            PIDValue = CalculatePID(error, previousErrorPtr);
            MotorControl(PIDValue, initSpeed, prevSpeedLPtr, prevSpeedRPtr);
            toggle = toggle2;
        }
    
        
    }        
        
    
}