/*
  This program is used to control the torisonal pendulum
  It used timer1 and interupts
*/

#include <p30f6015.h>

#define	FCY	117920000
#include <libpic30.h>
#include <delay.h>
#include <uart.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <qei.h>
#include <timer.h>
#include <ports.h>   // this one is new
#include <math.h>   // this one is new

#define MAXCNT 799  // maximum count for QEI encoders before interrupt
#define PI 3.14159265
#define TWO_PI 2*PI

#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 for the controller

#define MAX_ISUM 0.5 //0.25 works
#define MAX_DELTA_U 10.0*PI/180.0
#define MAX_U 30.0*PI/180.0

// define some other variables
#define EOL '\r'
#define CR 13	// carriage return
#define BS 8	// back space
#define MOTOR_READY 62
#define DISABLE_MOTOR 97
#define ENABLE_MOTOR 65
#define CW_ANGLE_LIMIT 88
#define READ_CW_ANGLE_LIMIT 120
#define CCW_ANGLE_LIMIT 89
#define READ_CCW_ANGLE_LIMIT 121
#define MOVE_TO_POSITION 82
#define GET_POSITION  114
#define VELOCITY 86
#define READ_VELOCITY 118

#define MAX_BUFFER 12
#define MAX_DUTY_CYCLE 0xFFFF

#define YES 1
#define NO 0

#define MAX_DELTA_OMEGA 0.1745  // 10 degrees in radians

char buffer[MAX_BUFFER];
char ch;
int bindex;
int received_data;
unsigned int GO;
int encindex, p2, r2;

/***********************************************************************/
// external input interupt handler

void __attribute__((interrupt,no_auto_psv)) _INT4Interrupt( void )
{
   int i, bob;
   char send, sbuf[6];

  // turn power off to the motor

  send = DISABLE_MOTOR;
  bob = sprintf(sbuf,"%c\r",send);
  for (i=0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
    // turn off the  LED to indicate power if off

  TRISDbits.TRISD7 = 0;
  PORTD = 0b0000000000000000;  // signal the power is off
  
  // Disable the interrupt

  DisableINT4;

  // now just wait

  while(1);
  
}
/***********************************************************************/

// timer 1 interupt handler

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

  // update the position variables

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

  // reset Timer 1 interrupt flag 

  IFS0bits.T1IF = 0;

  // we have a new value

  GO = 1;
}
/**************************************************/

// Interrupt routines

// UART2 receive ISR

void __attribute__((interrupt,auto_psv)) _U2RXInterrupt(void)
{
  while( !DataRdyUART2());	// wait for data to be ready
  ch = ReadUART2();

  buffer[bindex++] = ch;
  if (ch == CR)
  {
      received_data = YES;
      buffer[bindex] = 0;
      bindex = 0;
  }

  IFS1bits.U2RXIF = 0;	// clear interrupt flag

}

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

// 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_2 & // 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;
}

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

// Initialize external interrupt 4

void Init_INT4(void)
{
  unsigned int config;

  config = FALLING_EDGE_INT & // interrupt on a falling edge
           EXT_INT_ENABLE &        // enable the interrupts
           //EXT_INT_PRI_0 ;
           GLOBAL_INT_ENABLE;
            
  ConfigINT4( config );


   // turn on the LED to show interrupt is set

  TRISDbits.TRISD7 = 0; 
  PORTD = 0b0000000010000000;  // signal the interrupt is set

  // prepare for an input on RD11

  TRISDbits.TRISD11 = 1;

  // enable the interrupt

  DisableINT4;
  EnableINT4;

  return;
}

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

// setup UART1, the default 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 & // disable 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; // 115200 baud

OpenUART1( config1, config2, ubrg); 
}
/*****************************************************************/

// setup UART2, the motor control UART

 void uart2_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 & // disable 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; // 115200 baud

OpenUART2( config1, config2, ubrg);

config1 = UART_RX_INT_EN & // enable the uart interrupt
        UART_RX_INT_PR4 &  // the interrupt priority is 6
        UART_TX_INT_DIS &
        UART_TX_INT_PR4; // uart transmit priority is 2

ConfigIntUART2 (config1 );  // configure UART2 inerrupt

}

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

// 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_x4_MATCH &  // reset on match, was x2
           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_3 ;  // set the priority to two

WriteQEI( (unsigned int )MAXCNT );

ConfigIntQEI( config1 );
}
/*****************************************************************/

void setup_motor(void)
{
  double cw_limit, ccw_limit;
  int dummy, bob;
  char sbuf[10];
  char send;
  int i;
  
  // set up the motor speed

  send = VELOCITY;
  bob = sprintf(sbuf,"%c1023\r",send); 
  printf(" The velocity is set to %s \n", sbuf);
  for (i = 0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }

  __delay_ms(500);
  received_data = NO;
  send = READ_VELOCITY;
 
  bob = sprintf(sbuf,"%c\r",send);
  bindex  = 0;
  for (i=0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
  while (received_data == NO);
  dummy = atoi(buffer);
  printf(" The velocity is acknowledged as %d \n",dummy);

  // set up the motor CW angle limits

  send = CW_ANGLE_LIMIT;
  cw_limit = 1.7453; // 100 degrees
  bob = sprintf(sbuf,"%c%5.4f\r",send,cw_limit);
  printf(" The CW limits are set to %s \n", sbuf);
  for (i = 0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }

   __delay_ms(500);
  received_data = NO;
  send = READ_CW_ANGLE_LIMIT;
  bob = sprintf(sbuf,"%c\r",send);
  bindex  = 0;
  for (i=0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
  while (received_data == NO);
  cw_limit = atof(buffer)*180.0/PI;
  printf(" The CW limits are confirmed to be %8.4f \n", cw_limit);

 // set up the motor CCW angle limits

  send = CCW_ANGLE_LIMIT;
  ccw_limit = 3.4907;  // 200 degrees
  bob = sprintf(sbuf,"%c%5.4f\r",send,ccw_limit);
  printf(" The CCW limits are set to %s \n", sbuf);
  for (i = 0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
   __delay_ms(500);
  received_data = NO;
  send = READ_CCW_ANGLE_LIMIT;
  bob = sprintf(sbuf,"%c\r",send);
  bindex  = 0;
  for (i=0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
  while (received_data == NO);
  ccw_limit = atof(buffer)*180.0/PI;
  printf(" The CCW limits are confirmed to be %8.4f \n", ccw_limit);

  // disable the motor

   __delay_ms(500);
  received_data = NO;
  send = DISABLE_MOTOR;
  bob = sprintf(sbuf,"%c\r",send);
  for (i=0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
}
/************************************************************************/

// if the requested angle is too big stop the pendulum and wait

void stop_pendulum(void)
{
  int bob, i;
  char sbuf[10];
  char send;

    // turn power off to the motor

  send = DISABLE_MOTOR;
  bob = sprintf(sbuf,"%c\r",send);
  printf(" Disabling the motor \n");
  for (i =0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
    // turn off the  LED to indicate power if off

  TRISDbits.TRISD7 = 0;
  PORTD = 0b0000000000000000;  // signal the power is off

  // now just wait

  while(1);
}
/************************************************************************/

// set the pendulum in the initial position and then stop

void position_pendulum(void)
{
  int bob, i;
  char sbuf[10];
  char send;
  double theta;

  // the default center is 150 degrees

  theta = 150.0*PI/180.0;

  send = MOVE_TO_POSITION;
  bob = sprintf(sbuf,"%c%5.4f\r",send,theta);
    for (i = 0; i<bob; i++)
    {
        putcUART2(sbuf[i]);
        while(BusyUART2());
    }
  printf(" The base has been moved to %s \n", sbuf);

   __delay_ms(500);
  received_data = NO;
  send = GET_POSITION;
  bob = sprintf(sbuf,"%c\r",send);
  bindex  = 0;
  for (i=0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
  while (received_data == NO);
  theta = atof(buffer)*180.0/PI;
  printf(" The base is confirmed to be %8.4f \n", theta);

  // now stop the pendulum

  stop_pendulum();
}

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

double get_theta(void)
{
  int bob, i;
  char sbuf[10];
  char send;

  received_data = NO;
  send = GET_POSITION;
  bob = sprintf(sbuf,"%c\r",send);
  bindex = 0;
  for( i = 0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
  while (received_data != YES);  // wait for the answer
 // printf("  %s \n",buffer);
  return(atof(buffer));
}
/*****************************************************************/

int main ( void )
{
  extern unsigned int GO;
  extern int r2, p2;
  extern int bindex;
  unsigned int period;
  double theta_ref, theta_desired, phi, angle_scale, time;
  double u, dt;
  char send, sbuf[10];
  int i, bob;
  double kp, ki, last_u, error, Isum;

  // enable the input for QEI

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

  // set up the uart... uart1 is for printing, uart2 is for motor control

  bindex = 0;
  uart1_init();
  uart2_init();

  // set up the external interrupt to shut off the system

  Init_INT4();

  // setup the motor

  received_data = NO;
  send = MOTOR_READY;
  bob = sprintf(sbuf,"%c\r",send);
  bindex = 0;
  for (i = 0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }
  while (received_data == NO);
  while ( buffer[0] != MOTOR_READY);  // wait for motor to be ready

  // setup motor parameters

  setup_motor();

  angle_scale = (TWO_PI/MAXCNT); // convert to radians = 2*pi/800 counts per revolution
  p2 = 0;
  r2 = 0;
  encindex = 0;

  // initialize timer1

  dt = 0.0175;  // 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;

  // determine the reference value of theta, the position of the base

  theta_ref = 2.618;

  // enable the motor

  send = ENABLE_MOTOR;
  bob = sprintf(sbuf,"%c\r",send);
  for (i = 0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }

  // disable the motor, used only for debugging


  send = DISABLE_MOTOR;
  bob = sprintf(sbuf,"%c\r",send);
  for (i = 0; i<bob; i++)
  {
      putcUART2(sbuf[i]);
      while(BusyUART2());
  }

  // set up the control parameters

  last_u = 0.0;
  Isum = 0.0;
  kp = 0.0;  //
  ki = 0.0;  //

  // now just loop

  time = -dt;

  /******************************Main Loop **************************/
  while(1){

    while(!GO );
    GO = 0;
    time = time + dt;

    // you need to uncomment the following to initially position the
    // pendulum to the middle, then comment again

     // position_pendulum();

    // get the pendulum angle in radians

    phi = p2*angle_scale;

    // keep phi between -pi and pi

    if (phi > PI) phi = phi - TWO_PI;

    if (fabs(phi) < 0.008) phi = 0.0;  //this is the tolerance for phi

    error = -phi;
    Isum = Isum + error;
    Isum = min(Isum,MAX_ISUM);
    Isum = max(Isum,-MAX_ISUM);

    u = kp*error + ki*Isum;

    if (fabs(u) < 0.0051) u = 0.0; // tolerance for theta

    // make sure we do not try to move too much

    u = min(u, last_u+MAX_DELTA_U);
    u = max(u, last_u-MAX_DELTA_U);

    if ( fabs(u) > MAX_U)
    {
        printf(" u = %8.4f and is too large \n", u);
        printf(" Program Will STOP \n");
        stop_pendulum();
    }
 
    last_u = u;

     // u = 10.0*PI/180.0;
    // u = 0.087*sin(TWO_PI*1.2*time);

    // add the offset of theta_ref

    theta_desired = theta_ref + u;

    send = MOVE_TO_POSITION;
    bob = sprintf(sbuf,"%c%5.4f\r",send,theta_desired);
    for (i = 0; i<bob; i++)
    {
        putcUART2(sbuf[i]);
        while(BusyUART2());
    }

     printf("%3d %8.4f %8.4f %8.4f\n", GO, time, u, phi);

  } 
}
