#include "include\MSP430x14x.h"
#include "include\trapservo.h"
#include "include\main.h"
#include "include\plddriver.h"
#define abs(a) (((a)>=0)?(a):(-a))

#define HALF_POS    0x007FFFFF
#define MAX_POS     0x00FFFFFF



void trapServoLoop(MotorChannel* mc)
{
long int error=0;
long int dist_remain=0;
long int dist_stop=0;
long int current_velocity=0;
long int PID=0;
long int accel=2;
long int v_limit=40;


P2OUT |= 0x20;

	dist_remain=mc->goal_position-mc->last_position;
    current_velocity = mc->intermediate-mc->last_position;
  	dist_stop=0.5*current_velocity*current_velocity/accel;

   //and save for next time
  mc->last_position = mc->intermediate;
 	
  if((mc->cmd_vel*dist_remain)>=0)
  {
  	// is it time to begin stopping?
  	if((abs(dist_remain)>abs(dist_stop))&(mc->stop_flag==0))
  	{	
  		//keep going, not time to stop
  		if(dist_remain>0)
  		{
  		mc->cmd_vel+=accel;
  		if (mc->cmd_vel > VEL_LIMIT)mc->cmd_vel = VEL_LIMIT;
  		}
  		
  		else
  		{
  		mc->cmd_vel-=accel;
  		if (-mc->cmd_vel > VEL_LIMIT)mc->cmd_vel = -VEL_LIMIT;
  		}	
  	}
  	else
	{ 
		//time to decel
		mc->stop_flag=1;
		if (mc->cmd_vel >0) mc->cmd_vel -= accel;
		else mc->cmd_vel+=accel;
 	}
  }
 else
  {
  	if(mc->cmd_vel>0)
  	mc->cmd_vel-=accel;
  	else mc->cmd_vel+=accel;
  }	
  
  
  //we are very close, just servo to zero error
 if(abs(dist_remain)<20)
  {	
  	mc->cmd_vel=0;
  //	stop_flag=2;
  	mc->intermediate=mc->goal_position;
  
  }
  else
  {
  
   //adjust commanded position
   mc->intermediate += mc->cmd_vel;
  }

 
  error = mc->intermediate - mc->position;
 //calc proportional term 
  PID = (mc->Kp * error);
  
  //add in error to form intergral
  mc->sum = mc->sum + error;
  //check it's limits
  if (mc->sum > INT_LIMIT) mc->sum = INT_LIMIT;
  else if ( mc->sum < -INT_LIMIT) mc->sum = -INT_LIMIT;
  //calc the integral portion
  PID += mc->sum / mc->Ki;
  
   PID += mc->Kd * (error - mc->last_error);
  //update errors
 	mc->last_error = error;
 
  //now output the result
  //if negative then negate result but set dir
  if (PID < 0 )
  {
  P5OUT |= 0x01;
  P5OUT &= 0xfd;
  PID = PID * -1;   //flip the PID value to positive
  }
  else {
  P5OUT &= 0xfc;//set for forward
  }
  
  
 // encoders can't handle much faster than 700 for old demo board
 if (PID > 475) PID = 475;  //must leave some off time for the
                              //the driver to recharge
                              
	//maintain small dead band to eliminate unnecessary vibration                              
 if(((abs((long)(mc->position-mc->goal_position))<8)&(PID<60))) 
 {	
 	PID=0;	
 }
 
 //update the PWM                           
  TBCCR1 = PID;   
  
 	P2OUT &= 0xDf; 

}
void updatePosition24(MotorChannel* mc)
{
    long old_pos = (MAX_POS & mc->position);
    long new_pos = mc->encoderPosition;
    long delta_pos = new_pos - old_pos;

     if ( abs(delta_pos) > HALF_POS)
    {
        if ( delta_pos < 0 )
            delta_pos += MAX_POS;
        else
            delta_pos -= MAX_POS;
    }

    mc->position += delta_pos;

}
void getMotorPos(MotorChannel* mc)
{
	unsigned char *pos_bytes;
	pos_bytes = (unsigned char *)&mc->encoderPosition;
		
		pos_bytes[0] = readreg(Encode_1_LB);
		pos_bytes[1] = readreg(Encode_CB);
		pos_bytes[2] = readreg(Encode_MB);
		pos_bytes[3] = 0;
		
		//code to spit position out to the serial port
		while ((IFG2 & UTXIFG1) == 0); 
		sendbyte(pos_bytes[2]);
		sendbyte(pos_bytes[1]);
		sendbyte(pos_bytes[0]);
		while ((IFG2 & UTXIFG1) == 0);        
       	TXBUF1 = 0x0a;                
		while ((IFG2 & UTXIFG1) == 0);        
       	TXBUF1 = 0x0d;  
		
		updatePosition24(mc);
		
		//return position;

}
void initMotorChannel(MotorChannel* mc)
{
	getMotorPos(mc);
	mc->last_position=mc->position;
	mc->goal_position=mc->position;
	mc->intermediate=mc->position;
	mc->stop_flag=0;
	mc->cmd_vel=0;
	mc->Kp=2;
	mc->Kd=1;
	mc->Ki=500;
	mc->sum=0;
	mc->last_error=0;
	mc->accel=2;
	
}