File content as of revision 47:bafec50148b6:

#include "mbed.h"
#include "TFC.h"
#include <math.h> 
#include "motor.h"
DigitalOut myled(LED1);





//Variables
int cornerPwmControl;



// 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() // This simply sets the PWM and turns the motors off when the button is pushed
    {
        
        
        TFC_SetMotorPWM(0.4,0.4);
        
        while(1)
        {
            if(TFC_ReadPushButton(0)>0)
            {
                TFC_SetMotorPWM(0.0,0.0);
                
                DefaultMode();
                
            }
        }
        return;
    }
// This works for starting the car using the buttons on the bridge 
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---------------------------------------------------
//-----------------------------------------------------------------------------------------------------
// This simply sets the duty cycle.
// its a rudementary motor control.
float setDutyCycle(float dutyC, int w,int targetW)
{
    
    
    if(true)
    {
        if(w<targetW)   
        {
            dutyC+=0.1;        
        
        }  
        
        else
        {
            dutyC-=0.1;   
        }
        
    }
    
    
    else if(false)
    {
        
    }
    
 
    return dutyC;
}
    

 //----------------------------------------------------------------------------------------------------------------------------   
 //------------------------------------------------Cornering Control-----------------------------------------------------------   
 //----------------------------------------------------------------------------------------------------------------------------   
    
    
    
// This is a function which works off of the duty cycle. NO SENSOR REQUIREMENT

// this function works off the actual value rather than the change in angle. therefore need to have a variabe which stores where the value is.
void dutyCycleCorner( float speed, float theta,float tuner)
{
        bool leftOrRight;
        // sets if the car is going left or right
        if(theta<0)
        {
            leftOrRight=true;
            theta=theta*-1;   
            
        }
    //calculates the difference in 
        float deltaW = (((0.1f*tan((theta/0.02222f)* (3.14f / 180.0f)))/0.2f)*speed)*tuner;
        
        //TFC_SetMotorPWM(w2,w1);
        // this sets the speeds of each motor (outer wheel goes faster than inner wheel)
        if(leftOrRight){
        TFC_SetMotorPWM(speed+ deltaW,speed- deltaW);
        }
        else{
            TFC_SetMotorPWM(speed- deltaW,speed+ deltaW);
        }
        
        
        
 return;       
}
        
        
void sensorCorner(float &w1,float &w2,float theta,float speed, float tune)
{   // when cornering left the left motor slows down more than the right hand side
   // this is the ED for when the car is running off of the sensors rather than a set PWM
   
    bool leftOrRight = false;

    // makes theta positive and sets if the car is turning left or right
    if(theta<0)
        {
            leftOrRight=true;
            theta = theta*-1;   
            
        }
        
    //this limits the ED to stop it spinning out at high speeds.


    if(theta>0.35)
    {

     theta = 0.35;   
     //speed = speed- cornerlimiter;
        
    }
    
    
    
    
    //These equations set the angular speeds of the motors 
    //there are two equations for testing purposes
    if(true){
    // calculates the difference in the speed
    float angle= tan((theta/0.02222f)* (3.14f / 180.0f));
    float deltaW = (0.1f/(2*0.2f*angle))*speed;
    // 
    if(leftOrRight){
          w1= speed + (deltaW * tune);
          w2= speed -(deltaW * tune);
         
          
          }
    else{
            w1= speed- (deltaW*tune);
            w2= speed+ (deltaW*tune);
            
        }
    }
    
   /* else{
        //calulates the speed in eack wheel
        //speed of inner wheel
        float vin = speed*(1 - 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
        //speed of outer wheel.
        float vout = speed*(1 + 0.1/tan((theta/0.022222)*(3.14f / 180.0f)));
        
        // this sets the speeds of the inner and outer wheels
        if(leftOrRight){
          
          w1=vout;
          
          w2=vin;
          
          }
        else{
               
                w1=vin;
                w2=vout;
            }
        
      }*/  
    return;   
}