File content as of revision 9:22b119eef1de:

#include "mbed.h"
#include "TFC.h"
#include <math.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 corner(float &w1,float &w2,float deltaTheta,int maxspeed)

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 speedSetting(int w1, int w2 ,int w1M, int w2M)
{
    // need to compare the measured frequency 
    
    float deltaW1 = w1 - w1M;
    float deltaW2 = w2 - w2M;
    
    if(delatW1 <0)
    {
        changespeed(0,w1);   
    }
    
    else if(delatW1 >0)
    {
        changespeed(1,w1);
    } 
    return;
}
    
//need to fill out function for calculating the change in speed.
void changespeed(bool a, float w)
{
    float change;
    
    if(a == 1)
    {
        w+= change;
    }
    
    else if (a ==0)
    {
        
        w-= change;    
    }
 return;   
}
    
 //----------------------------------------------------------------------------------------------------------------------------   
 //------------------------------------------------Cornering Control-----------------------------------------------------------   
 //----------------------------------------------------------------------------------------------------------------------------   
    
        
void corner(float &w1,float &w2,float deltaTheta,int maxspeed)
    {// 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 = (maxspeed/r)*(1+diff);
    float w2 = (maxspeed/r)*(1-diff);
    
    
        
        
     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)
{   // this function is for simply calculating the motor speeds. this reduces the amount of calculations needed 
    //and can be triggered when the car steers.
    
    
    float deltaTheta =  center- theta;
    
    // need to convert to degrees/radians. that can do the above as well.
    if(deltaTheta <0)
    {
        // going left?
        corner(w1,w2,deltaTheta,maxspeed);
    }
    
    else()
    {
        //going right?
        corner(w2,w1,deltaTheta);
        
    }
    
 return;   
}