//--------------RC car Traction control system -------------------------//
/*
By ABHILASH
Contact: abhilashpatel121@gmail.com
Documentation & details:
Update(22/11/21): Basic PI control for RC car traction control (for Tiva C series)
*/


#include <stdint.h>
#include <stdbool.h>
#include "inc/tm4c123gh6pm.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "driverlib/sysctl.h"
#include "driverlib/interrupt.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/pwm.h"
#include "driverlib/qei.h"
#include "driverlib/pin_map.h"
#include "inc/hw_ints.h"
#include "inc/hw_gpio.h"
#include "driverlib/debug.h"
#include "stdio.h"

void sys_setup(void);
void PWM_setup(void);
//void QEI_setup(void);


//---------------variable declaration and tuning---------------------------//
float throttle,  vel_f_avg, vel_r_avg, vel_r_req, vel_r_error, slip, pwm, vel_r_error_int, T_req, vel_f[10], vel_r[10];
int temp_f[6], temp_r[6];
int count,tf1,tf2,tr1,tr2,tf,tr,temp1,temp2,temp3;
uint32_t count1;
float time;        //seconds

//test_timings
int start_time=5;    //seconds
int end_time=7;      //seconds
float dt=0.02;    //inturrupt trigger or time step (in s)

//slip ratio target
float slip_target=0.17; //slip value

//PI tuning factors
float Kp=0.1;
float Ki=0.0008;


int i;
//-------------------------------------------------------------//

int main(void)
{
sys_setup();
PWM_setup();
//pwm=270;
//QEI_setup();
vel_r_error_int=0;
count1=0;


count=0;
tf=0; tr=0;
i=0;
for(i=0;i<9;i++)
{
    vel_f[i]=0;
    vel_r[i]=0;
}
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, 1);

while(1)
{  SysCtlDelay(500);

   temp_f[1]=temp_f[2]; temp_f[2]=temp_f[3]; temp_f[3]=temp_f[4]; temp_f[4]=temp_f[5];
   temp_r[1]=temp_r[2]; temp_r[2]=temp_r[3]; temp_r[3]=temp_r[4]; temp_r[4]=temp_r[5];

   count1=count1+1;   //each count equivalent to 25 microseconds
   temp_f[5]= GPIOPinRead(GPIO_PORTC_BASE,GPIO_PIN_5);
   if(temp_f[5]>1 && temp_f[4]==0)
   {
       tf1=tf2; tf2=count1;   tf=tf2-tf1;
   }

   temp_r[5]= GPIOPinRead(GPIO_PORTD_BASE,GPIO_PIN_6);
   if(temp_r[5]>1 && temp_r[4]==0)
   {
       tr1=tr2; tr2=count1;   tr=tr2-tr1;
   }


}

}


void Timer0IntHandler(void)
{   temp2=temp1;
    temp1=count1;
    temp3=temp1-temp2;
    count=count+1;
    time=count*dt;   // time stored in seconds

// sensor data history update

    //history update
    i=0;
    for(i=0;i<9;i++)
    {
        vel_f[i]=vel_f[i+1];
        vel_r[i]=vel_r[i+1];
    }


if(tf>20 && tf<4000){
                    vel_f[9]=(6.28*0.0215)/(tf*0.000078*12);
                    if(vel_f[9]<0.5)
                        {vel_f[9]=0.5;}
                    }
if(tr>20 && tr<4000){
                     vel_r[9]=(6.28*0.0225)/(tr*0.000078*12); //time for speed tracking function
                    }
   //vel_f[20] = QEIVelocityGet(QEI1_BASE);
   //vel_f[20]=20*(vel_f[20]/24)*3.14*.045;
   //vel_r[20] = QEIVelocityGet(QEI0_BASE);
   //vel_r[20]=20*(vel_r[20]/24)*3.14*.045;

   i=0;
   vel_f_avg=0;
   vel_r_avg=0;

   i=0;

       vel_r_avg=0.3*vel_r[9]+0.25*vel_r[8]+0.2*vel_r[7]+0.15*vel_r[6]+0.1*vel_r[5];
       vel_f_avg=0.3*vel_f[9]+0.25*vel_f[8]+0.2*vel_f[7]+0.15*vel_f[6]+0.1*vel_f[5];

//   vel_r_avg=vel_r_avg/5;
//   vel_f_avg=vel_f_avg/5;



   if(vel_f_avg<.5)  {vel_f_avg=.5;}
   slip=(vel_r_avg-vel_f_avg)/vel_f_avg;
   vel_r_req=(1+slip_target)*vel_f_avg;
   //vel_r_req=2*(time-5);

   if(vel_r_req>15){vel_r_req=15;}
  // vel_r_error=vel_r_req-vel_r;

   vel_r_error=vel_r_req-vel_r_avg;

      T_req=vel_r_error*Kp+vel_r_error_int*Ki;


      //torque to voltage conversion
   //throttle=(420*T_req+0*vel_r_avg+0.4)*1000/12; //interm of duty cycle*1000
   throttle=(376*T_req+0.7*vel_r_avg+0.4)*1000/12;//interm of duty cycle*1000

   if(throttle<850 && time>start_time && time<end_time){
       vel_r_error_int=vel_r_error_int+vel_r_error*dt;
   }
   //error integration with integral anti windup



    if(throttle<1){throttle=1;}
    if(throttle>900){throttle=900;}   //voltage limit 9.6V


    //light for visual confirmation for test start and end
    // also throttle fail safe for all condition except test condition
    if(time>start_time && time<end_time)
    {GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, 8);   PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, throttle);}
    else{GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, 2);    PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, 1);}


TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT); // Clear the timer interrupt
}














//-------------------------Functions-------------------------------//

//--------------------system and inturrept enable--------------------//

void sys_setup(void)
{
    uint32_t ui32Period;
    SysCtlClockSet(SYSCTL_SYSDIV_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN); //clock speed= 40MHz
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);

    GPIOPinTypeGPIOInput(GPIO_PORTD_BASE, GPIO_PIN_6);
    GPIOPinTypeGPIOInput(GPIO_PORTC_BASE, GPIO_PIN_5);

    //-----------------interrupt setup---------------------
    SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
    TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC);
    ui32Period = (SysCtlClockGet() *dt);  //inturrupt triggrs after every 20ms
    TimerLoadSet(TIMER0_BASE, TIMER_A, ui32Period -1);
    IntEnable(INT_TIMER0A);
    TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
    IntMasterEnable();
    TimerEnable(TIMER0_BASE, TIMER_A);
}



//--------------------PWM Setup--------------------//
void PWM_setup(void)
{
    //--------------LED setup------------------------
    GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3);
    GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_0);


    //--------------PWM setup------------------
       SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1);    //for pf0: M1pwm4                                      // PF0 can work with Control module 1 PWM5
       SysCtlPWMClockSet(SYSCTL_PWMDIV_16);                                                 // User defined
       GPIOPinTypePWM(GPIO_PORTD_BASE, GPIO_PIN_0);
       GPIOPinConfigure(GPIO_PD0_M1PWM0);
       //Pin name for pf1 tab 20.1 data sheet

       PWMGenConfigure(PWM1_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN);
       //PF1 connected with generator 2, operated on count down

       PWMGenPeriodSet(PWM1_BASE, PWM_GEN_0, 1000);
       //period set
       PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, 0); // pulse width
       PWMOutputState(PWM1_BASE, PWM_OUT_0_BIT, true);
       //enabled 5 output
       PWMGenEnable(PWM1_BASE, PWM_GEN_0);
       //Starting PWM
}


//------------------QEI setup---------------

void QEI_setup(void)
{
    // two single phase input

       SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI1);

       //Configure the QEI Pins
       GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_5 | GPIO_PIN_6);
       GPIOPinConfigure(GPIO_PC5_PHA1);
       GPIOPinConfigure(GPIO_PC6_PHB1);

       //DISable peripheral and int before configuration
       QEIDisable(QEI1_BASE);
       QEIIntDisable(QEI1_BASE,QEI_INTERROR | QEI_INTDIR | QEI_INTTIMER | QEI_INTINDEX);

       // Configure quadrature encoder
       QEIConfigure(QEI1_BASE,QEI_CONFIG_CAPTURE_A|QEI_CONFIG_NO_RESET|QEI_CONFIG_QUADRATURE |QEI_CONFIG_NO_SWAP,0);
       QEIVelocityConfigure(QEI1_BASE,QEI_VELDIV_1,SysCtlClockGet()/20);
      // QEIFilterConfigure(QEI1_BASE, QEI_FILTCNT_2);
       QEIFilterEnable(QEI1_BASE);
       QEIVelocityEnable(QEI1_BASE);
       QEIEnable(QEI1_BASE);


       //second
       SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI0);

       //Configure the QEI Pins
       GPIOPinTypeQEI(GPIO_PORTD_BASE, GPIO_PIN_6 | GPIO_PIN_7);
       GPIOPinConfigure(GPIO_PD6_PHA0);
       GPIOPinConfigure(GPIO_PD7_PHB0);

       //DISable peripheral and int before configuration
       QEIDisable(QEI0_BASE);
       QEIIntDisable(QEI0_BASE,QEI_INTERROR | QEI_INTDIR | QEI_INTTIMER | QEI_INTINDEX);

       // Configure quadrature encoder
       QEIConfigure(QEI0_BASE,QEI_CONFIG_CAPTURE_A|QEI_CONFIG_NO_RESET|QEI_CONFIG_QUADRATURE |QEI_CONFIG_NO_SWAP,0);
       QEIVelocityConfigure(QEI0_BASE,QEI_VELDIV_1,SysCtlClockGet()/20);
       //QEIFilterConfigure(QEI0_BASE, QEI_FILTCNT_2);
       QEIFilterEnable(QEI0_BASE);
       QEIVelocityEnable(QEI0_BASE);
       QEIEnable(QEI0_BASE);
}