// WallFollower.c
// Runs on LM4F120/TM4C123
// Use SysTick interrupts to implement a software PWM to drive
// two DC motors at variable duty cycles.
// Aquí va la descripción.
// Fusion de los proyectos para Keil DCMotor y ADCSWTrigger de la carpeta TExaSware 
// del curso de Sistemas Embebidos del Dr. Valvano

/*
    
                   
         -----------
         |         |
   < --- | Robot   | ---> 
         |         |
         -----------
                   
    
*/
// Daniel Valvano, Jonathan Valvano
// September 12, 2013

// Grupo de Arquitectura de Micros DCI
// Septiembre 18, 2015


// PA0 connected to U0Rx (VCP receive)
// PA1 connected to U0Tx (VCP transmit)
// PA5 connected to DC motor interface for left wheel
// PA6 connected to DC motor interface for right wheel

// PE1 connected to forward facing IR distance sensor
// PE2 connected to forward rear IR distance sensor
// PE4 connected to forward right IR distance sensor
// PE5 connected to forward left IR distance sensor

#include "ADCSWTrigger.h"
#include "PLL.h"
//#include "UART.h"

#define DIR_MOTOR_A 	          (*((volatile unsigned long *)0x40004060))
#define DIR_MOTOR_B							(*((volatile unsigned long *)0x40004300))

#define GPIO_PORTA_DATA_R       (*((volatile unsigned long *)0x400043FC))
#define GPIO_PORTA_DIR_R        (*((volatile unsigned long *)0x40004400))
#define GPIO_PORTA_AFSEL_R      (*((volatile unsigned long *)0x40004420))
#define GPIO_PORTA_DR8R_R       (*((volatile unsigned long *)0x40004508))
#define GPIO_PORTA_DEN_R        (*((volatile unsigned long *)0x4000451C))
#define GPIO_PORTA_AMSEL_R      (*((volatile unsigned long *)0x40004528))
#define GPIO_PORTA_PCTL_R       (*((volatile unsigned long *)0x4000452C))
#define GPIO_PORTF_DATA_R       (*((volatile unsigned long *)0x400253FC))
#define GPIO_PORTF_DIR_R        (*((volatile unsigned long *)0x40025400))
#define GPIO_PORTF_IS_R         (*((volatile unsigned long *)0x40025404))
#define GPIO_PORTF_IBE_R        (*((volatile unsigned long *)0x40025408))
#define GPIO_PORTF_IEV_R        (*((volatile unsigned long *)0x4002540C))
#define GPIO_PORTF_IM_R         (*((volatile unsigned long *)0x40025410))
#define GPIO_PORTF_RIS_R        (*((volatile unsigned long *)0x40025414))
#define GPIO_PORTF_ICR_R        (*((volatile unsigned long *)0x4002541C))
#define GPIO_PORTF_AFSEL_R      (*((volatile unsigned long *)0x40025420))
#define GPIO_PORTF_PUR_R        (*((volatile unsigned long *)0x40025510))
#define GPIO_PORTF_DEN_R        (*((volatile unsigned long *)0x4002551C))
#define GPIO_PORTF_LOCK_R       (*((volatile unsigned long *)0x40025520))
#define GPIO_LOCK_KEY           0x4C4F434B  // Unlocks the GPIO_CR register
#define GPIO_PORTF_CR_R         (*((volatile unsigned long *)0x40025524))
#define GPIO_PORTF_AMSEL_R      (*((volatile unsigned long *)0x40025528))
#define GPIO_PORTF_PCTL_R       (*((volatile unsigned long *)0x4002552C))
#define SYSCTL_RCGC2_R          (*((volatile unsigned long *)0x400FE108))
#define NVIC_ST_CTRL_R          (*((volatile unsigned long *)0xE000E010))
#define NVIC_ST_RELOAD_R        (*((volatile unsigned long *)0xE000E014))
#define NVIC_ST_CURRENT_R       (*((volatile unsigned long *)0xE000E018))
#define NVIC_EN0_R              (*((volatile unsigned long *)0xE000E100))
#define NVIC_PRI7_R             (*((volatile unsigned long *)0xE000E41C))
#define NVIC_SYS_PRI3_R         (*((volatile unsigned long *)0xE000ED20))

// basic functions defined at end of startup.s
void DisableInterrupts(void); // Disable interrupts
void EnableInterrupts(void);  // Enable interrupts
long StartCritical (void);    // previous I bit, disable interrupts
void EndCritical(long sr);    // restore I bit to previous value
void WaitForInterrupt(void);  // low power mode

// Properties of the controller
long Error;                     // difference between right and left distances to the walls
#define GAIN              2     // translates the difference between right and left ADC readings into a PWM duty update
#define CRASH             IR10CM// if there is less than this distance ahead of the robot...
#define IR10CM            3100  // according to datasheet ~2.3 V with object at 10 cm
#define IR16CM            1704  // according to experiment ~1.25 V with object at 16 cm
#define IR20CM            1417  // according to experiment ~1.038 V with object at 20 cm
#define IR80CM            546   // according to datasheet ~0.4 V with object at 80 cm
                               
// Initialize motor pins and configure SysTick interrupts

unsigned long p1,p2;            // p1 y p2 toman valores entre 1 y 9, por un factor de 10 para el duty cycle en cada motor
int cp1,cp2, b,f;                    // cp1 y cp2 llevan el control del PWM en cada motor 

void Motor_Init(void){
  SYSCTL_RCGC2_R |= 0x00000001; // activate clock for port A
  GPIO_PORTA_AMSEL_R &= ~0xFC;      // disable analog functionality on PA5
  GPIO_PORTA_PCTL_R &= ~0xFFFFFF00; // configure PA7-PA2 as GPIO
  GPIO_PORTA_DIR_R |= 0xFC;     // make PA7-PA2 out
  GPIO_PORTA_DR8R_R |= 0xFC;    // enable 8 mA drive on PA7-PA2
  GPIO_PORTA_AFSEL_R &= ~0xFC;  // disable alt funct on PA7-PA2
  GPIO_PORTA_DEN_R |= 0xFC;     // enable digital I/O on PA7-PA2
  GPIO_PORTA_DATA_R &= ~0xFC;   // make PA7-PA2 low
  NVIC_ST_CTRL_R = 0;           // disable SysTick during setup
  NVIC_ST_RELOAD_R = 8000;      // reload value for 100us
  NVIC_ST_CURRENT_R = 0;        // any write to current clears it
  NVIC_SYS_PRI3_R = (NVIC_SYS_PRI3_R&0x00FFFFFF)|0x40000000; // priority 2
  NVIC_ST_CTRL_R = 0x00000007;  // enable with core clock and interrupts
}
void SysTick_Handler(void){
	
	cp1--;												// se reiniciara al valor de p1 cada 1ms
	cp2--;												// se reiniciara al valor de p2 cada 1ms
	
	if(cp1>0)
		GPIO_PORTA_DATA_R |= 0x20;  // make PA5 high
	else 
		GPIO_PORTA_DATA_R &= ~0x20; // make PA5 low
	
	if(cp2>0)
		GPIO_PORTA_DATA_R |= 0x04;  // make PA2 high
	else 
		GPIO_PORTA_DATA_R &= ~0x04; // make PA2 low
	
	if(-cp1+p1+1 == 10)cp1 = p1;
	if(-cp2+p2+1 == 10)cp2 = p2;
	
    NVIC_ST_RELOAD_R = 8000;     // reload value for high phase
}


void Switch_Init(void){  unsigned long volatile delay;
  SYSCTL_RCGC2_R |= 0x00000020; // (a) activate clock for port F
  delay = SYSCTL_RCGC2_R;
  GPIO_PORTF_LOCK_R = 0x4C4F434B; // unlock GPIO Port F
  GPIO_PORTF_CR_R = 0x11;         // allow changes to PF4,0
  GPIO_PORTF_DIR_R &= ~0x11;    // (c) make PF4,0 in (built-in button)
  GPIO_PORTF_AFSEL_R &= ~0x11;  //     disable alt funct on PF4,0
  GPIO_PORTF_DEN_R |= 0x11;     //     enable digital I/O on PF4,0
  GPIO_PORTF_PCTL_R &= ~0x000F000F; //  configure PF4,0 as GPIO
  GPIO_PORTF_AMSEL_R &= ~0x11;  //     disable analog functionality on PF4,0
  GPIO_PORTF_PUR_R |= 0x11;     //     enable weak pull-up on PF4,0
  GPIO_PORTF_IS_R &= ~0x11;     // (d) PF4,PF0 is edge-sensitive
  GPIO_PORTF_IBE_R &= ~0x11;    //     PF4,PF0 is not both edges
  GPIO_PORTF_IEV_R &= ~0x11;    //     PF4,PF0 falling edge event
  GPIO_PORTF_ICR_R = 0x11;      // (e) clear flags 4,0
  GPIO_PORTF_IM_R |= 0x11;      // (f) arm interrupt on PF4,PF0
  NVIC_PRI7_R = (NVIC_PRI7_R&0xFF00FFFF)|0x00400000; // (g) priority 2
  NVIC_EN0_R = 0x40000000;      // (h) enable interrupt 30 in NVIC
}

void Delay100ms(unsigned long time){
  unsigned long i;
  while(time > 0){
    i = 1333333;  // this number means 100ms
    while(i > 0){
      i = i - 1;
    }
    time = time - 1; // decrements every 100 ms
  }
}


void forward(unsigned int v){
GPIO_PORTF_DATA_R |= 0x08;
DIR_MOTOR_A = 0x10; //PA4 = 1 y PA3 = 0, adelante
DIR_MOTOR_B = 0x80; //PA7 = 1 y PA6 = 0, adelante
p1=cp1=p2=cp2=v;
}

void backward(unsigned int v){
GPIO_PORTF_DATA_R |= 0x02;
DIR_MOTOR_A = 0x08; //PA4 = 0 y PA3 = 1, reversa
DIR_MOTOR_B = 0x40; //PA7 = 0 y PA6 = 1, reversa
p1=cp1=p2=cp2=v;
}

void stop(void){
GPIO_PORTF_DATA_R &= ~0x0E;
	
DIR_MOTOR_A = 0x00; //PA4 = 0 y PA3 = 1, reversa
DIR_MOTOR_B = 0x00; //PA7 = 0 y PA6 = 1, reversa	
	
p1=cp1=p2=cp2=1;	
}

void leftf(unsigned int w){ //w 1-5 Izquierda adelante
GPIO_PORTF_DATA_R |= 0x0A;
DIR_MOTOR_A = 0x10; //PA4 = 1 y PA3 = 0, adelante
DIR_MOTOR_B = 0x80; //PA7 = 1 y PA6 = 0, adelante
p1=cp1=5+w;
p2=cp2=w;
}

void leftb(unsigned int w){ //w 1-5 Izquierda reversa
GPIO_PORTF_DATA_R |= 0x0C;
DIR_MOTOR_A = 0x08; //PA4 = 0 y PA3 = 1, reversa
DIR_MOTOR_B = 0x40; //PA7 = 0 y PA6 = 1, reversa
p1=cp1=5+w;
p2=cp2=w;
}

void rightf(unsigned int w){ //w 1-5 Derecha adelante
GPIO_PORTF_DATA_R |= 0x06;
DIR_MOTOR_A = 0x10; //PA4 = 1 y PA3 = 0, adelante
DIR_MOTOR_B = 0x80; //PA7 = 1 y PA6 = 0, adelante
p1=cp1=w;
p2=cp2=5+w;
}

void rightb(unsigned int w){ //w 1-5 Derecha reversa
GPIO_PORTF_DATA_R |= 0x0E;
DIR_MOTOR_A = 0x08; //PA4 = 0 y PA3 = 1, reversa
DIR_MOTOR_B = 0x40; //PA7 = 0 y PA6 = 1, reversa
p1=cp1=w;
p2=cp2=5+w;
}



/*
void forward(unsigned int v){
GPIO_PORTF_DATA_R |= 0x08;
DIR_MOTOR_A = 0x10; //PA4 = 1 y PA3 = 0, adelante
DIR_MOTOR_B = 0x80; //PA7 = 1 y PA6 = 0, adelante
p1=cp1=p2=cp2=v;
}

void backward(unsigned int v){
GPIO_PORTF_DATA_R |= 0x02;
DIR_MOTOR_A = 0x08; //PA4 = 0 y PA3 = 1, reversa
DIR_MOTOR_B = 0x40; //PA7 = 0 y PA6 = 1, reversa
p1=cp1=p2=cp2=v;
}

void stop(void){
GPIO_PORTF_DATA_R &= ~0x0E;
p1=cp1=p2=cp2=1;
}

void leftf(unsigned int w){ //w 1-5 Izquierda adelante
GPIO_PORTF_DATA_R |= 0x0A;
DIR_MOTOR_A = 0x10; //PA4 = 1 y PA3 = 0, adelante
DIR_MOTOR_B = 0x80; //PA7 = 1 y PA6 = 0, adelante
p1=cp1=5+w;
p2=cp2=w;
}

void leftb(unsigned int w){ //w 1-5 Izquierda reversa
GPIO_PORTF_DATA_R |= 0x0C;
DIR_MOTOR_A = 0x08; //PA4 = 0 y PA3 = 1, reversa
DIR_MOTOR_B = 0x40; //PA7 = 0 y PA6 = 1, reversa
p1=cp1=5+w;
p2=cp2=w;
}

void rightf(unsigned int w){ //w 1-5 Derecha adelante
GPIO_PORTF_DATA_R |= 0x06;
DIR_MOTOR_A = 0x10; //PA4 = 1 y PA3 = 0, adelante
DIR_MOTOR_B = 0x80; //PA7 = 1 y PA6 = 0, adelante
p1=cp1=w;
p2=cp2=5+w;
}

void rightb(unsigned int w){ //w 1-5 Derecha reversa
GPIO_PORTF_DATA_R |= 0x0E;
DIR_MOTOR_A = 0x08; //PA4 = 0 y PA3 = 1, reversa
DIR_MOTOR_B = 0x40; //PA7 = 0 y PA6 = 1, reversa
p1=cp1=w;
p2=cp2=5+w;
}
*/


// power:   10%    20%   30%   40%   50%   60%   70%   80%   90%
void GPIOPortF_Handler(void){ // called on touch of either SW1 or SW2
  if(GPIO_PORTF_RIS_R&0x01){  // SW2 touch
    GPIO_PORTF_ICR_R = 0x01;  // acknowledge flag0
		
    b=1;
		
		//if(p1<10)p1++;            //incrementa en 10% el duty cycle
		//cp1 = p1;
  }
  if(GPIO_PORTF_RIS_R&0x10){  // SW1 touch
    GPIO_PORTF_ICR_R = 0x10;  // acknowledge flag4
		
		f=1;
		//if(p2<10)p2++;						//incrementa en 10% el duty cycle
		//cp2 = p2;
  }
	
}



// Initialize RGB LED pins
void LED_Init(void){  unsigned long volatile delay;
  SYSCTL_RCGC2_R |= 0x00000020; // (a) activate clock for port F
  delay = SYSCTL_RCGC2_R;
  GPIO_PORTF_AMSEL_R &= ~0x0E;      // disable analog functionality on PF3-1
  GPIO_PORTF_PCTL_R &= ~0x0000FFF0; // configure PF3-1 as GPIO
  GPIO_PORTF_DIR_R |= 0x0E;     // make PF3-1 out (built-in LEDs)
  GPIO_PORTF_AFSEL_R &= ~0x0E;  // disable alt funct on PF3-1
  GPIO_PORTF_DEN_R |= 0x0E;     // enable digital I/O on PF3-1
  GPIO_PORTF_DATA_R &= ~0x0E;   // make PF3-1 low
}
// This function samples AIN2 (PE1), AIN9 (PE4), AIN8 (PE5) and
// returns the results in the corresponding variables.  Some
// kind of filtering is required because the IR distance sensors
// output occasional erroneous spikes.  This is an FIR filter:
// y(n) = (x(n) + x(n-1))/2
// Assumes: ADC initialized by previously calling ADC_Init2981()
void ReadADCFIRFilter(unsigned long *ain2, unsigned long *ain9, unsigned long *ain8, unsigned long *ain1){
  //                   x(n-1)
  static unsigned long ain2previous=0;
  static unsigned long ain9previous=0;
  static unsigned long ain8previous=0;
	static unsigned long ain1previous=0;
  // save some memory; these do not need to be 'static'
  //            x(n)
  unsigned long ain2newest;
  unsigned long ain9newest;
  unsigned long ain8newest;
	unsigned long ain1newest;
	
  ADC_In2981(&ain2newest, &ain9newest, &ain8newest, &ain1newest); // sample AIN2(PE1), AIN9 (PE4), AIN8 (PE5), AIN1 (PE2)
  *ain2 = (ain2newest + ain2previous)/2;
  *ain9 = (ain9newest + ain9previous)/2;
  *ain8 = (ain8newest + ain8previous)/2;
	*ain1 = (ain1newest + ain1previous)/2;
  ain2previous = ain2newest; ain9previous = ain9newest; ain8previous = ain8newest; ain1previous = ain1newest;
}
// This function samples AIN2 (PE1), AIN9 (PE4), AIN8 (PE5) and
// returns the results in the corresponding variables.  Some
// kind of filtering is required because the IR distance sensors
// output occasional erroneous spikes.  This is an IIR filter:
// y(n) = (x(n) + y(n-1))/2
// Assumes: ADC initialized by previously calling ADC_Init298()
void ReadADCIIRFilter(unsigned long *ain2, unsigned long *ain9, unsigned long *ain8, unsigned long *ain1){
  //                   y(n-1)
  static unsigned long filter2previous=0;
  static unsigned long filter9previous=0;
  static unsigned long filter8previous=0;
	static unsigned long filter1previous=0;
  // save some memory; these do not need to be 'static'
  //            x(n)
  unsigned long ain2newest;
  unsigned long ain9newest;
  unsigned long ain8newest;
	unsigned long ain1newest;
	
  ADC_In2981(&ain2newest, &ain9newest, &ain8newest, &ain1newest); // sample AIN2(PE1), AIN9 (PE4), AIN8 (PE5), AIN1 (PE2)
  *ain2 = filter2previous = (ain2newest + filter2previous)/2;
  *ain9 = filter9previous = (ain9newest + filter9previous)/2;
  *ain8 = filter8previous = (ain8newest + filter8previous)/2;
	*ain1 = filter1previous = (ain1newest + filter1previous)/2;
}
// Median function from EE345M Lab 7 2011; Program 5.1 from Volume 3
// helper function for ReadADCMedianFilter() but works for general use
unsigned long median(unsigned long u1, unsigned long u2, unsigned long u3){
unsigned long result;
  if(u1>u2)
    if(u2>u3)   result=u2;     // u1>u2,u2>u3       u1>u2>u3
      else
        if(u1>u3) result=u3;   // u1>u2,u3>u2,u1>u3 u1>u3>u2
        else      result=u1;   // u1>u2,u3>u2,u3>u1 u3>u1>u2
  else
    if(u3>u2)   result=u2;     // u2>u1,u3>u2       u3>u2>u1
      else
        if(u1>u3) result=u1;   // u2>u1,u2>u3,u1>u3 u2>u1>u3
        else      result=u3;   // u2>u1,u2>u3,u3>u1 u2>u3>u1
  return(result);
}
// This function samples AIN2 (PE1), AIN9 (PE4), AIN8 (PE5) and
// returns the results in the corresponding variables.  Some
// kind of filtering is required because the IR distance sensors
// output occasional erroneous spikes.  This is a median filter:
// y(n) = median(x(n), x(n-1), x(n-2))
// Assumes: ADC initialized by previously calling ADC_Init298()
void ReadADCMedianFilter(unsigned long *ain2, unsigned long *ain9, unsigned long *ain8, unsigned long *ain1){
  //                   x(n-2)        x(n-1)
  static unsigned long ain2oldest=0, ain2middle=0;
  static unsigned long ain9oldest=0, ain9middle=0;
  static unsigned long ain8oldest=0, ain8middle=0;
	static unsigned long ain1oldest=0, ain1middle=0;
  // save some memory; these do not need to be 'static'
  //            x(n)
  unsigned long ain2newest;
  unsigned long ain9newest;
  unsigned long ain8newest;
	unsigned long ain1newest;
  ADC_In2981(&ain2newest, &ain9newest, &ain8newest, &ain1newest); // sample AIN2(PE1), AIN9 (PE4), AIN8 (PE5)
  *ain2 = median(ain2newest, ain2middle, ain2oldest);
  *ain9 = median(ain9newest, ain9middle, ain9oldest);
  *ain8 = median(ain8newest, ain8middle, ain8oldest);
	*ain1 = median(ain1newest, ain1middle, ain1oldest);
  ain2oldest = ain2middle; ain9oldest = ain9middle; ain8oldest = ain8middle; ain1oldest = ain1middle;
  ain2middle = ain2newest; ain9middle = ain9newest; ain8middle = ain8newest; ain1middle = ain1newest;
}
// This function returns the number of digits in the input 'n'.
// For example, if n=1234, the output is 4.
// For example, if n=88, the output is 2.
// Assumes: n <= 4095
unsigned long digits(unsigned long n){
  if(n < 10){
    return 1;
  }
  if(n < 100){
    return 2;
  }
  if(n < 1000){
    return 3;
  }
  return 4;             // more is not needed in this context
} 
#define LEDPATTERN        5171  // arbitrary number defines sequence of flashing LED

int main(void){
  unsigned long frwdleft, frwdright, ahead, rear, i, timeOutput, pattern;
  //long e;               // intermediate value of Error
	
	p1=cp1=6;
	p2=cp2=6;
	
  DisableInterrupts();  // disable interrupts while initializing
  PLL_Init();           // bus clock at 80 MHz
  Motor_Init();         // output to PA6-5, SysTick interrupts
  Switch_Init();        // arm PF4, PF0 for falling edge interrupts
  LED_Init();           // configure PF3-1 as GPIO or LEDs
  ADC_Init2981();        // initialize ADC to sample AIN2 (PE1), AIN9 (PE4), AIN8 (PE5), AIN1 (PE2)
  //UART_Init();          // initialize UART0 for 115,200 baud rate
  EnableInterrupts();   // enable after all initialization are done
  //UART_OutString("\r\nL% - R% - Ahead - Frwd R - Frwd L - Error\r\n");
  timeOutput = 0;       // very imprecise counter to output occasionally
  pattern = LEDPATTERN; // used to flash an arbitrary pattern defined above
	
	  //Solo para probar funciones
		forward(5);
		Delay100ms(10);
		backward(5);
		Delay100ms(10);
		stop();
		Delay100ms(10);
		leftf(4);
		Delay100ms(10);
		rightf(4);
		Delay100ms(10);
		leftb(4);
		Delay100ms(10);
		rightb(4);
		Delay100ms(10);
		stop();
		b=f=0;
  while(1){
		
	  
    // main program samples the ADC to check the sensors
    ReadADCMedianFilter(&ahead, &frwdright, &frwdleft, &rear);

    if((timeOutput%64) == 0){        // no hagan mucho caso a esto, es algo para crear un patron en el LED azul
     
      if(pattern&0x1){ 
        GPIO_PORTF_DATA_R ^= 0x04;   // toggle blue LED
      }
      pattern = pattern/2;
      if(pattern == 0){                  
        pattern = LEDPATTERN;
      }
    }
    timeOutput = timeOutput + 1;
		
    if(b){backward(5);
			Delay100ms(4);
			b=0;
		}
		
		if(f){forward(5);
			Delay100ms(4);
			f=0;
		}
		
		
		if(rear > 1000){                       //Si el sensor trasero tiene un valor mayor que 1000 enciende los 3 LEDs
				//GPIO_PORTF_DATA_R |= 0x0E;
			  for(i=0; i<2500; i=i+1);
			
				//GPIO_PORTF_DATA_R &= ~0x0E;
			  for(i=0; i<2500; i=i+1);
			
			  //Delay100ms(10);
	      forward(5);

			}
		else {
		stop();
		pattern = LEDPATTERN;
		}		
		
		
		if(timeOutput>0xFFFF)timeOutput=0;               //igual no hacer caso
			
  }
}