File content as of revision 3:5b5d5af46804:

#include "mbed.h"
#include "TFC.h"
DigitalOut myled(LED1);

int  motor_setup(); 
void TurnOn();
void RunMotor();
void DefaultMode();


//Speed control
void Acc(float& motorA, float& motorB);
void Decc(float& motorA, float& motorB);
void StartLine();

//Corner Control

void PWM_cornerCntrl(bool a,float pwmRatio);
void cornerLeft(float speed);
void cornerRight(float speed);


int cornerPwmControl;// this sets the cornering percentage ratio. value between 1 and 0.
                     // 100 means both motors run at the same speed. 0 means that one motor turns off.



// uncomment for testing motor functions.


 int   motor_setup()
    {
        
        //TurnOn();
        
        return 0;
    }
    
    
 
    

 void   RunMotor() /// putting it into this mode for some reason makes a bit of a whinning noise (this may simply just be the motor running)
    {
        
        TFC_HBRIDGE_ENABLE;
        // TFC_SetMotorPWM(0.4,0.7);
        
        while(1)
        {
            if(TFC_ReadPushButton(0)>0)
            {
                TFC_SetMotorPWM(0.0,0.0);
                TFC_HBRIDGE_DISABLE;
                DefaultMode();
                
            }
        }
        return;
    }
    
void DefaultMode()
    {
        while(1)
        {
            if(TFC_ReadPushButton(1)>0)
               {
                    RunMotor();
                }
            
            else if(TFC_ReadPushButton(0)>0)
               {
                    //this will be a debug mode
                }
            
               
        }
    return;
    }
    
    
void Acc(float& motorA, float& motorB)// set up so as to control both motors during acc. Potential use during corners
    {
        motorA = motorA + 0.1;
        motorB = motorB + 0.1;
        TFC_SetMotorPWM(motorA,motorB);
        
                
    return ;
    
    }
    
void Decc(float &motorA, float &motorB)
    {
        // a good thing to do would be to have a margin for adjustment so that the car cornering can control the acc+dcc much better. 
        
        motorA = motorA - 0.1;
        motorB = motorB - 0.1;
        
        TFC_SetMotorPWM(motorA,motorB);
        
    return ;
    
    }
    
    
void StartLine()
    {
        TFC_HBRIDGE_ENABLE;
        float a=0;
        float b=0;
          
        int x=0 ;
        while(x<5)
        {
            Acc(a,b);
            wait(0.5);
            x++  ;
        } 
        
     return ;
    }
    
void finishLine(float pwmA)
    {
        float pwmB= pwmA;
        while(pwmA>0)
        {
            
          
           Decc(pwmA,pwmB);
           
        }
        return;
    }

void PWM_cornerCntrl(bool a,float pwmRatio)
    {
        //A is the Right motor B is the left motor.
        // may need the value of speed to decelerate.
        if(a==1)//turn left
        {
            cornerLeft(pwmRatio);
        }
        
        else()//turn right
        {
               cornerRight(pwmRatio)
        }
     return;   
    }
    
    
    
    
void cornerLeft(float speed)
    {// when cornering left the left motor slows down more than the right hand side
    // may just replace with ACC and DECC
    
    // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
    w1 = speed + diff;
    w2 = speed-diff;
    TFC_SetMotorPWM(w2,w1); //temperary values
        
    
    
    
        
     
     
     return;   
    }
    
void cornerRight(float speed)
    {   // may need to put deceleration control within here.
        // may just replace with ACC and DECC
        float diff = speed*cornerPwmControl;
        w1 = speed + diff;
        w2 = speed-diff;
        TFC_SetMotorPWM(w1,w2); //temperary values
        
     return;   
    }
    
    
void deltaCornerLeft(float speed,float deltaTheta)
    {// when cornering left the left motor slows down more than the right hand side
    // may just replace with ACC and DECC
    float r;
    float d;
    float l;
    // it may be worth doing this off the change in theta. so that it is insesnsitive to the calibration.
    diff= ((d*tan(deltatheta)/(2*l)));
    
    w1 = (speed/r)*(1+diff);
    w2 = (speed/r)*(1-diff);
    
    // when there is a speed sensor tghe conversion will be much more simplistic. this is basically just guessing.
    // need to convert w1 to the duty cycle
    TFC_SetMotorPWM(w2,w1); //temperary values
        
        
     return;   
    }