/*
  This simple program reads the input from a variable resistor
  and the then uses pwm to change the speed of the wheel 
  It used timer1 and interupts
*/

#include <p30f6015.h>

#define	FCY	117920000
#include <libpic30.h>
#include <delay.h>
#include <adc10.h>
#include <pwm.h>
#include <uart.h>
#include <string.h>
#include <stdio.h>
#include <qei.h>
#include <timer.h>

#define PERIOD  7300 // for 1000 Hz pwm frequency
#define MAX_DUTY 2*7300 // MAX_DUTY = 2*PERIOD since pwm prescaler is 2
#define MAX_INCREMENT_DUTY MAX_DUTY //the is the largest increment in duty cycle we can have
#define MAXCNT 719  // maximum count for QEI encoders before interrupt, 720 counts per revolution
#define MAX_U (double) MAX_DUTY
#define MAX_ISUM 10000.0

#define max(A ,B) ((A) > (B) ? (A) : (B))
#define min(A, B) ((A) < (B) ? (A) : (B))

// Configuration Bits
_FOSC(CSW_FSCM_OFF & FRC_PLL16); //FRC 7.37MHz * 16 = 117.92MHz/29.48MIPS Tcy=33.9nS
_FWDT(WDT_OFF);  // watchdog timer off
_FBORPOR(PBOR_OFF & BORV27 & PWRT_16 & MCLR_EN);  //brown out parameters
_FGS(GWRP_OFF);

// define some variables

unsigned int AD_value;
unsigned int GO;
int encindex, p1, p2, r1, r2, Delta;

/***********************************************************************/
// timer 1 interupt handler

void __attribute__((interrupt,auto_psv)) _T1Interrupt( void )
{ 
  unsigned int ReadQEI( void );
  extern unsigned int AD_value;
  extern unsigned int GO;
  extern int p2, r2, encindex;

 // read from the A/D channel

  ADCON1bits.SAMP = 1; // start the sampling
  while(!ADCON1bits.DONE);
  AD_value = ReadADC10(0);

  // update the position variables

  p2 = (int) ReadQEI();
  r2 = encindex;

  // reset Timer 1 interrupt flag 

  IFS0bits.T1IF = 0;

  // we have a new value

  GO = 1;
}

/***********************************************************************/
// QEI interupt handler

void __attribute__((interrupt,no_auto_psv)) _QEIInterrupt( void )
{ 
  extern int encindex;

 // update the encoder count every time the counter POSCNT gets to MAXCNT

  if (QEICONbits.UPDN )
  {
     encindex++;
  } 
  else
  {
     encindex--;
  }

  IFS2bits.QEIIF = 0;  // 
}    

/****************************************************************************************************/ 
// Initialize timer 1

void Init_Timer1( unsigned int period )
{
  unsigned int config;

  config = T1_INT_PRIOR_4 & // set interrupt priority to 2
           T1_INT_ON;       // enable the interrupts
            
  ConfigIntTimer1( config );

  config =  T1_ON &  // turn on the timer
            T1_IDLE_CON & // operate during sleep
            T1_GATE_OFF & // timer gate accumulation is disabled
            T1_PS_1_256 &   // timer prescale is 256
            T1_SYNC_EXT_OFF & // don't synch with external clock
            T1_SOURCE_INT; // use the internal clock

  OpenTimer1( config, period );

  return;
}

/***********************************************************************/

// setup ADC10

  void adc_init(void){

  unsigned int config1, config2, config3, configport, configscan;

  ADCON1bits.ADON = 0 ; // turn off ADC

  SetChanADC10(
       ADC_CH0_NEG_SAMPLEA_VREFN & // negative reference for channel 0 is VREF negative
       ADC_CH0_POS_SAMPLEA_AN3    // 
      // ADC_CHX_NEG_SAMPLEA_VREFN &  // negative reference for channel 1 is VREF negative
      // ADC_CHX_POS_SAMPLEA_AN3AN4AN5
                 );

  ConfigIntADC10( ADC_INT_DISABLE ); // disable the interrupts

  config1 = 
       ADC_MODULE_ON &  //turn on ADC module
       ADC_IDLE_CONTINUE & // let it idle if not in use
       ADC_FORMAT_INTG &   // unsigned integer format
       ADC_CLK_AUTO  &  // manual trigger source
       ADC_AUTO_SAMPLING_OFF  & // do not continue sampling
       ADC_SAMPLE_SIMULTANEOUS & // sample both channels at the same time
       ADC_SAMP_ON; // enable sampling

  config2 = 
       ADC_VREF_AVDD_AVSS & // voltage reference
       ADC_SCAN_OFF & // don't scan
       ADC_CONVERT_CH0 & // convert channel 0
       ADC_SAMPLES_PER_INT_1 & // 1 samples per interupt
       ADC_ALT_BUF_OFF & // don't use the alternate buffer
       ADC_ALT_INPUT_OFF; // don't use an alternate input

  config3 = 
       ADC_SAMPLE_TIME_2 & // auto sample time bits
       ADC_CONV_CLK_SYSTEM & // use the system clock
       ADC_CONV_CLK_13Tcy;  // conversion clock speed (coeff of TCY is ADCS)
       
  configport = 
        ENABLE_AN3_ANA;   // parameters to be configured in the to ADPCFG 

  configscan = 
       SCAN_NONE; // scan select parameter for the ADCSSL register
  
  OpenADC10( config1, config2, config3, configport, configscan );
}

/*****************************************************************/

// setup pwm

  void pwm_init(void){

  unsigned int config1, config2, config3;
  unsigned int sptime;

  config1 = PWM_INT_DIS &     // disable the interrupt
            PWM_FLTA_DIS_INT; // disable the interrupt on fault

  ConfigIntMCPWM( config1 );

  config1 = PWM_EN & //  enable the PWM module
            PWM_IPCLK_SCALE4 & // input prescaler set to 2
            PWM_OP_SCALE1 & // post scalar set to 1
            PWM_MOD_UPDN; // free running mode

  config2 = PWM_MOD1_IND & // pwm modules run independently
            PWM_MOD2_IND & 
            PWM_MOD3_IND & 
            PWM_MOD4_IND & 
            PWM_PDIS1H & // disable 1 high
            PWM_PDIS2H & // disabale 2 high
            PWM_PEN3H & // enable 3 high
			PWM_PDIS4H  & // disable 4 high
            PWM_PDIS1L & // disable 1 low
            PWM_PDIS2L & // disable 2 low
            PWM_PDIS3L &  // disable 3 low
            PWM_PDIS4L ;  //disable 4 low

  config3 = PWM_UEN; // enable updates

  sptime = 0x0;

  OpenMCPWM( PERIOD, sptime, config1, config2, config3 );

}

/*****************************************************************/

// setup the UART

 void uart1_init(void) {
 
unsigned int config1, config2, ubrg;

config1 = UART_EN & // enable the UART
          UART_IDLE_CON & // set idle mode
          UART_DIS_WAKE & // disable wake-up on start
          UART_DIS_LOOPBACK & // disable loopback
          UART_DIS_ABAUD & // diable autobaud rate detect
          UART_NO_PAR_8BIT & // no parity, 8 bits
          UART_1STOPBIT;     // one stop bit

config2 = UART_INT_TX_BUF_EMPTY & // interrupt anytime a buffer is empty
          UART_TX_PIN_NORMAL & // set transmit break pin
          UART_TX_ENABLE & // enable UART transmission
          UART_INT_RX_CHAR & // recieve interrupt mode selected
          UART_ADR_DETECT_DIS & // disable address detect
          UART_RX_OVERRUN_CLEAR; // overrun bit clear

ubrg = 15; // 11520 baud

OpenUART1( config1, config2, ubrg); 
}

/*****************************************************************/

// setup the QEI encoder

void encoder_init(void) {

unsigned int config1, config2;

config1 =  QEI_DIR_SEL_QEB &
           QEI_INT_CLK & 
           QEI_INDEX_RESET_DISABLE & // QEI index pulse resets postion counter 
           QEI_CLK_PRESCALE_1 &
           QEI_GATED_ACC_DISABLE &
           QEI_NORMAL_IO &  
           QEI_INPUTS_NOSWAP & 
		   QEI_MODE_x2_MATCH &  // reset on match
           QEI_DOWN_COUNT & // count up
           QEI_IDLE_CON;  // continue on idle
          
config2 = POS_CNT_ERR_INT_DISABLE & // diable error interrupts
          QEI_QE_CLK_DIVIDE_1_1 &   //1_256
          QEI_QE_OUT_ENABLE &  // enable digital filter
          MATCH_INDEX_INPUT_PHASEA &
          MATCH_INDEX_INPUT_LOW;

OpenQEI( config1, config2 );
         
config1 = QEI_INT_ENABLE & // enable the interups
         QEI_INT_PRI_2 ;  // set the priority to two

WriteQEI( (unsigned int )MAXCNT );

ConfigIntQEI( config1 );
}

/*****************************************************************/

int main ( void )
{
  extern unsigned int AD_value;
  extern unsigned int GO;
  extern int r1, r2, p1, p2;
  unsigned int dutycyclereg, dutycycle, period;
  char updatedisable;
  double scale, time;
  double u, error, speed_scale, speed, AD_scale, dt;
  double R;

  // enable the input for A/D

  TRISBbits.TRISB3 = 1;

  // enable the input for QEI

  TRISBbits.TRISB4 = 1;
  TRISBbits.TRISB5 = 1;

  //  set up the A/D parameters

  adc_init();

  // set up the pwm

  pwm_init();

  // set up the uart

  uart1_init();

  // disable updates

  updatedisable = 0;
 
  // get some scaling out of the way, the maximum AD value is 1024 which should be mapped to the 
  // maximum duty cycle

  scale = ((double) MAX_DUTY)/1024.0;
  r1 = 0;
  r2 = 0;
  p1 = 0;
  p2 = 0;
  encindex = 0;

  // initialize timer1

  dt = 0.025;  // the sampling inverval in seconds

  // dt = N*256/29,480,000;  assuming a 256 prescalar.
  // so N = dt* 115156

  period = (unsigned int) (dt*115156.0);
 
  period = min(period, 32768);  // don't let the period get bigger than the counter

  Init_Timer1( period );

  // set up the encoder

  encoder_init();

  // enable the QEI interupt

  EnableIntQEI;

  // set up the parameter for the power amplifier

  dutycycle = (unsigned int)0;
  dutycyclereg = 3;
  SetDCMCPWM( dutycyclereg, dutycycle, updatedisable );  // duty cycle set to low

  // enable the output to control the motor

  TRISEbits.TRISE6 = 0;   // IN1 
  TRISEbits.TRISE7 = 0;   // IN2

  PORTE = 0b01000000;   // IN1 = 0, IN2 = 1;

  // set the scalinig from AD values to rad/sec

  AD_scale = 1.0;  // estimated from multiple step responses, convert from A/D to rad/sec

  // set up the scaling to convert to radians per second

  speed_scale = (2*3.14159/720.0)/dt; // convert to radians/sec = 2*pi/720 counts per revolution

  // now just loop

  AD_value = 0;
  time = 0;

  while(1){

    while(!GO );
    GO = 0;
    time = time + dt;
   
    // get the speed in radians/sec

    speed = ((double)((r2-r1)*720+(p2-p1)))*speed_scale;

    R = (double) AD_value*AD_scale;  // this is the reference input

    error = R;
    u = error;

    u = u*scale/AD_scale;  // convert back to a pwm signal

    u = min(u,MAX_U);  // don't let u get too large
    u = max(u,0.0);    // don't let u get negative, since the motor cannot switch direction

    dutycycle = (unsigned int) u;
 
    dutycyclereg = 3;
    SetDCMCPWM( dutycyclereg, dutycycle, updatedisable);

    // use the following for determining the scaling

    printf("%8.4f %8.4f %6d %10.4f\n", time, R ,(int) dutycycle, speed );
   
    // save the current positions

	r1 = r2;
    p1 = p2;

  } 
}