File content as of revision 11:4a6f97cc1f1e:

#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 dutyCycleCorner( float speed, float angle);
void corner(float &w1,float &w2,float deltaTheta,int maxspeed);

void steering(float center, float theta, int maxspeed);
void centerWheels();
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?
 
//----------------------------------------------------------------------------------------------------------------------------------
//----------------------------------------------This is for Motor Set up------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------------------  

 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;
    }
//-----------------------------------------------------------------------------------------------------    
//------------------------ this is for speed control---------------------------------------------------
//-----------------------------------------------------------------------------------------------------


    
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(deltaW1 <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-----------------------------------------------------------   
 //----------------------------------------------------------------------------------------------------------------------------   
    
    
    
// this is a function whihc works off of the duty cycle. NO SENSOR REQUIREMENT
void dutyCycleCorner( float speed, float angle)
{
        
        //float width =0.15;
        //float radius= center(middlePoint);
        
        bool leftOrRight;
        float theta = angle/0.02222f;
        if(theta<0)
        {
         leftOrRight=true;
         theta=theta*-1;   
            
        }
//       float diff = (tan(theta))/ 2; 
       
       
        
        //float radius = 0.6;
        float x = tan(theta* (3.14f / 180.0f));
        float deltaW = ((0.2f*x)/0.2f)*speed;
        float w1=speed+ deltaW;
        float w2= speed- deltaW;
        
        TFC_SetMotorPWM(w2,w1);
        /*if(leftOrRight){
        TFC_SetMotorPWM(w1,w2);
        }
        else{
            TFC_SetMotorPWM(w2,w1);
        }*/
        
        
        
 return;       
}
        
        
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)));
    
     w1 = (maxspeed/r)*(1+diff);
     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,maxspeed);
        
    }
    
 return;   
}