File content as of revision 7:9aaa4f73bb32:

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


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);

void steering(float center, float theta, int maxspeed);

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.



//need a function for calcu;lating the angle
//need a function for converting w1 to delta. or do i?
 
    

 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_SetMotorPWM(0.4,0.7);
        
        while(1)
        {
            if(TFC_ReadPushButton(0)>0)
            {
                TFC_SetMotorPWM(0.0,0.0);
                
                DefaultMode();
                
            }
        }
        return;
    }
    
void DefaultMode()
    {
        TFC_Init();
        while(1)
        {
            TFC_HBRIDGE_ENABLE;
            if(TFC_ReadPushButton(1)>0)
               {
                    runMotor();
                }
            
            else if(TFC_ReadPushButton(0)>0)
               {
                    //this will be a debug mode
                }
            
               
        }
        TFC_HBRIDGE_DISABLE;
    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.
    float diff = speed*cornerPwmControl;
    float w1 = speed + diff;
    float 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;
        float w1 = speed + diff;
        float 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.
    float diff= ((d*tan(deltaTheta)/(2*l)));
    
    float w1 = (speed/r)*(1+diff);
    float 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;   
    }
    

    
    
float centerWheels(float min, float max)
{
    float center= (min +max)/2;

    return center;
}

void steering(float center, float theta, int maxspeed,float w1, float w2)
{
    float deltaTheta =  center- theta;
    if(deltaTheta <0)
    {
        // going left?
        corner(w1,w2,deltaTheta);
    }
    
    else()
    {
        //going right?
        corner(w2,w1,deltaTheta)
        
    }
    
 return;   
}