/*This work is licensed under a

Creative Commons Attribution-NonCommmercial-ShareAlike 4.0 International

This license lets others remix, tweak, and build upon this work non-commercially,
as long as they credit the AUTHOR and license their new creations under the
identical terms.
*/

/*
Ivan Rene Morales Argueta (2014)
ivan[at]fisica[dot]usac[dot]edu[dot]gt
Universidad de San Carlos de Guatemala
EE School
*/

/*
Modified by David Barrientos (2015)
for applications using Embedded C that require float data
d[dot]b[dot]gt[at]ieee[dot]org
Universidad de San Carlos de Guatemala
EE School
*/

#include <stdint.h>
#include <stdbool.h>
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/tm4c123gh6pm.h"
//******************************Librerias
Extras************************************//
#include "driverlib/sysctl.h"
#include "driverlib/timer.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"
#include "driverlib/pin_map.h"
#include "driverlib/uart.h"
#include "driverlib/adc.h"
#include "driverlib/pwm.h"
#include "inc/hw_timer.h"
/
************************************Definiciones***********************************
***********/
#define Clock 80000000
#define WheelMotorsFreq 1000
#define Frequency 50
#define CLOCK 80000000
#define Tmpmax 39999999
#define dP = 0.3141592
/
*************************************Variables*************************************
***********/
///-----------------PID/-----------------//
volatile float Kp, Ki, Kd, minInt, maxInt;
volatile float setPoint;
volatile float derivator, integrator;
volatile float error;
///-----------------PWM-----------------//
volatile uint32_t Load;
volatile uint32_t PWMClock;
volatile uint32_t DutyC1;
///-----------------Control-----------------//
volatile uint32_t SensorVariable;
volatile float RPM1;
volatile float RPM2;
volatile float contador1 =0;
volatile float contador2 =0;
volatile float dt1 = 0.00000;
volatile float dt2 = 0.00000;
/
************************************PID********************************************
*************/

//**********************Inicializaci�n*************************//
void PIDInit(float p, float i, float d, float minI, float maxI){
//-----------------Parametros PID------------------------------//
Kp = p; Ki = i; Kd = d;
//-----------------Limites del Integrador----------------------//
minInt = minI; maxInt = maxI;
//-----------------Condiciones Iniciales-----------------------//
derivator = 0; integrator = 0;
error = 0;
}

//**************************Set Point*************************//
void PIDSetPoint(float set){
setPoint = set;
derivator = 0;
integrator = 0;
}

//****************Update (Output Generation)*******************//

float PIDUpdate(float currentValue){
float pValue, iValue, dValue, PID;
error = setPoint - currentValue;
//-----------------Proportional---------------------//
pValue = Kp*error;
//-----------------Integral-------------------------//
integrator = integrator + error;
if (integrator > maxInt){integrator = maxInt;}
else if(integrator < minInt){integrator = minInt;}
iValue = integrator*Ki;
//-----------------Derivative-----------------------//
dValue = Kd*(error - derivator);
derivator = error;
//-----------------Output----------------------//
PID = pValue + iValue + dValue;

//****************Specific to Application*******************//
//------------Motor Rotation Change------------//
/*if(PID>0){GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|
GPIO_PIN_7, 0x10);}
else{GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7,
0x20);}
*/
//--------Hysteresis Control for PWM---------//
if(PID>100){PID=100;}
 else if(PID<-100){PID=-100;}
if(PID<0){PID=(-1)*PID;}

return PID;
}

/
************************************PWM********************************************
*************/
void PWM_Init(void){
PWMClock = Clock/64;
Load = (PWMClock / WheelMotorsFreq) - 1;
PWMGenConfigure(PWM0_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN);
PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, Load);
PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, true);
PWMGenEnable(PWM0_BASE, PWM_GEN_0);
}

//Sensores RPM
Encoder-------------------------------------------------------------------

void Timer1(void){
TimerConfigure(TIMER0_BASE, TIMER_CFG_ONE_SHOT_UP);
TimerLoadSet(TIMER0_BASE, TIMER_A, Tmpmax);
}

void Timer2(void){
TimerConfigure(TIMER1_BASE, TIMER_CFG_ONE_SHOT_UP);
TimerLoadSet(TIMER1_BASE, TIMER_A, Tmpmax);
}

void INTERRUPCION1(void){
GPIOIntTypeSet(GPIO_PORTD_BASE, GPIO_PIN_0, GPIO_FALLING_EDGE);
GPIOIntEnable(GPIO_PORTD_BASE, GPIO_INT_PIN_0);
IntEnable(INT_GPIOD);
IntMasterEnable();
}

void INTERRUPCION2(void){
GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_4, GPIO_FALLING_EDGE);
GPIOIntEnable(GPIO_PORTE_BASE, GPIO_INT_PIN_4);
IntEnable(INT_GPIOE);
IntMasterEnable();
}

void MEDIR_VELOCIDAD1(void){
GPIOIntClear(GPIO_PORTD_BASE, GPIO_INT_PIN_0);
contador1 = TimerValueGet(TIMER0_BASE, TIMER_A); // Se determina cuantos numeros
se han contado
HWREG(TIMER0_BASE + TIMER_O_TAV) = 0x00; //Reiniciar el contador
TimerEnable(TIMER0_BASE, TIMER_A);
}

void MEDIR_VELOCIDAD2(void){
GPIOIntClear(GPIO_PORTE_BASE, GPIO_INT_PIN_4);
contador2 = TimerValueGet(TIMER1_BASE, TIMER_A); // Se determina cuantos numeros
se han contado
HWREG(TIMER1_BASE + TIMER_O_TAV) = 0x00; //Reiniciar el contador
 TimerEnable(TIMER1_BASE, TIMER_A);
}

/
************************************Main*******************************************
**************/
void main(void){

/************************************CLOCK*****************************************
***************/
SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_XTAL_16MHZ);
/****************************PWM *************************************/
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPWMClockSet(SYSCTL_PWMDIV_64);
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);
GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_6);
GPIOPinConfigure(GPIO_PB6_M0PWM0);
GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_7);
GPIOPinConfigure(GPIO_PB7_M0PWM1);
PWM_Init();

PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, Load*DutyC1/100);

PIDInit(5, 3, 5, -2, 2);

PIDSetPoint(0);
Timer1();
Timer2();
INTERRUPCION1();
INTERRUPCION2();
while(1){
dt1 = contador1/40000000;
dt2 = contador2/40000000;
RPM1 = dt1;
RPM2 = dt2;

PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, Load*DutyC1/100);

DutyC1 = (unsigned int)PIDUpdate(RPM1 - RPM2);

}
}