//*********************************************************************
//   Filename: BackEmf.c
//       Date: 8/20/04
//    Version: 1.0
//     Author: David Hygh
//*********************************************************************
//   Processor frequency: 20 MHz
//   Notes:
//      Demonstrates PID control of a motor, using Back EMF as the 
//      control feedback. This code implements a positional PID
//      controller, i.e. the target is a position or distance.
//
//      The code sequences from a trapezoid position trajectory to either
//      a constant velocity or to holding.
//
//      Implemented in a PIC16F88 using the CCS C compiler
//
//*********************************************************************
#include <16F88.h>
#device ADC=8
#fuses HS,NOWDT,NOPUT,MCLR,NOBROWNOUT,NOLVP,NOCPD,NOWRT,CCPB0,NOPROTECT,NOFCMEN,NOIESO
#use delay (clock=20000000)
#use rs232(baud=19200, xmit=PIN_B5, rcv=PIN_B2, parity=n)
#use fast_io(A)
#use fast_io(B)

#define END_POSITION 40000L

// Function Prototypes
void EMFfb(void);
void PIDfb(void);
void SetPWM(signed long);
void Tick(void);
void Move(int, long, long, int32);
void CalcTraj(void);

enum run_types {
  R_OFF,
  R_TRAPEZOID,
  R_HOLD,
  R_CONST };
    
// Global vars
const long pGain = 3L;      // proportional gain
const long dGain = 10L;     // derivative gain
const long iGain = 0L;      // integral gain

int32 tEndPos;              // target end position
long tVel, tAcc;            // target velocity, target acceleration
signed long currPos = 0;    // current position based on EMF feedback
int32 genPos = 0;           // generated postion from trapeziod function
signed long genVel = 0;     // generated velocity from trapeziod function
int32 tDecPos;              // calculated deceleration position

int runState = R_OFF;       // current run state
signed long eInteg = 0L;    // integral accumulator

void main()
{
        
  setup_comparator(NC_NC_NC_NC);
  setup_vref(0);
  set_tris_a(0b11110011);                         // A2-3 outputs
  set_tris_b(0b11010100);                         // B0-1,B3,B5 outputs
  setup_adc_ports(sAN0 | sAN1 | VSS_VDD);         // setup A/D inputs
  setup_adc( ADC_CLOCK_INTERNAL );                // setup A/D clock
  set_adc_channel(0);                             // init A/D channel
  
  setup_ccp1(CCP_PWM);
  setup_timer_2(T2_DIV_BY_4, 255, 1);       // 4.88 kHz PWM
  set_pwm1_duty(0);                // initialize PWM off
        
  output_low(PIN_A2);        // motor side A low
  output_low(PIN_A3);        // motor side B low
        
  delay_ms(1000);
  // printf("Starting\n\r"); // debug, only if UART connected

  /* Trapeziodal Speed  */
  Move(R_TRAPEZOID, 120, 1, END_POSITION);
  while (runState != R_OFF)
  {
    delay_ms(5);
    Tick();
  }
        
  /* Hold or Constant Speed */
  Move(R_HOLD, 0, 0, END_POSITION);         // hold position 
//  Move(R_CONST, 30, 0, 0);            // constant speed
  while (TRUE)
  {
    delay_ms(5);
    Tick();
  }
}

/* copies run state, target position, velocity and acceleration to globals */
void Move(int run, long vel, long acc, int32 endPos)
{
  tEndPos = endPos;
  tVel = vel;
  tAcc = acc;
  // the deceleration position is approximated by calculating the ramp up
  // position and subtracting it from the end position.
  // the ramp up position is vel^2/acc*2 + vel/2
  // this routine skips '+ vel/2' in order to handle non-evenly divisible
  // vel/acc/endPos values
  tDecPos = endPos - ((vel * vel)/(acc*2));
  runState = run;
  //printf("endpos=%lu decpos=%lu\n\r", tEndPos, tDecPos);
}

void Tick(void)
{
    CalcTraj();     // calculate the trajectory
    EMFfb();        // get motor position using back EMF
    PIDfb();        // calculate PID control
}
 
/* calculate trajectory based on runState */ 
void CalcTraj(void)
{
  
  switch (runState)
  {
    case R_OFF:
      break;
    case R_HOLD:
      genPos = tEndPos;
      break;
    case R_CONST:
      genPos += tVel;
      break;
    case R_TRAPEZOID:
      if (genPos >= tDecPos)
        genVel -= tAcc;       // decelerate
      else if (genVel < tVel)
      {
        genVel += tAcc;       // accelerate
        if (genVel > tVel) genVel = tVel;
      }
      // position is at/beyond target endpoint, or velocity was negative
      if ((genPos + tAcc) >= tEndPos || genVel < 0)
      {
        genPos = tEndPos;
        genVel = 0;
        runState = R_OFF;
      }
      genPos += genVel;
  }
     
//      printf("gtp=%lu gtv=%ld\n\r", genPos, genVel);
}

/* implements EMF feedback */
void EMFfb(void)
{
  long adcMa = 0;       // holds ADC value from motor side A
  long adcMb = 0;       // holds ADC value from motor side B
  
  setup_ccp1(CCP_OFF);        // shut off PWM

  output_low(PIN_B3);         // sync signal for scope
  delay_us(500);
  set_adc_channel( 0 );
  delay_us(10);
  adcMa = read_adc();         // get ADC value from motor side A
  set_adc_channel( 1 );
  delay_us(10);
  adcMb = read_adc();         // get ADC value from motor side B
  output_high(PIN_B3);        // sync signal for scope
  
  setup_ccp1(CCP_PWM);        // activate PWM

  currPos += adcMa - adcMb;   // calculate current position 
}

/* implements PID control */
void PIDfb(void)
{
  signed long pwmSet;
  static signed long ePrev;
  signed long error;
  
  if (runState != R_OFF)      // only calculate PWM values if moving or holding
  {
    error = genPos - currPos;         // error is desired-current position
    pwmSet = pGain * error  +         // standard PID equation 
             iGain * eInteg +  
             dGain * (error - ePrev); // derivative is current-previous
    eInteg += error;                  // integral is simply a summation over time
    ePrev = error;                    // save previous for derivative
  }
  else
    pwmSet = 0; 
  
  SetPWM(pwmSet);           // set PWM value to motor

//  printf("e%ld cp%ld 1V%ld 2V%ld\n\r", error, currPos, adcV1, adcV2);
}

/* sets hardware PWM based on sign and value of pwm */
void SetPWM(signed long pwm)
{    
  long pset;
 
  if (pwm < 0)
  {
    output_low(PIN_A2);          // motor side A low
    output_high(PIN_A3);         // motor side B high
  } else {
    output_low(PIN_A3);          // motor side B low
    output_high(PIN_A2);         // motor side A high
  }
    
  pset = (abs(pwm));            // take absolute value of pwm
  // clip PWM signal to hardware max of 1023
  if (pset > 1023L)
  {
    pset = 1023;
    //printf("ps%ld pwm%ld\n\r", pset, pwm);
  }
  set_pwm1_duty(pset);          // CCS function to set hardware PWM
}