//  Motor & motion control     Motors.c					// Rev 12/31/05

//Copyright (C) 2005 Alex Brown	rbirac@cox.net
//This program is free software; See license at the end of this file for details.

/*
  performs all computing of basic forward motion and steering commands and
  computes required PWM and drives the two motors.
  
  scaling:  acceleration can handle +/- 32767 mm/sec^2 (3+ g's)
  			velocity                +/- 32767 mm/sec (+/- 117 Km/h)
			distance				+/- 35 Km
*/
#include "Microcontroller.h"	  //for registers, config data & stdio
#include <stdlib.h>				  //for abs()

extern int  time;				  //from Microcontroller.c
extern long enc0;   		      //drive wheel encoder, normally left wheel
extern long enc1;   		      //drive wheel encoder, normally right wheel
extern long odomLlast,odomRlast;  //temporary values of previous odom data
extern long DistActL, DistActR;	  //actual distances wheels have moved in mm
extern int  hdgdeg;	  			  //gyro direction of robot in deg*10
extern int Compass;				  //compass direction of robot in deg*10
extern int Speed;
extern int USBtimeout;			  //time since last message from laptop

// Motor Constants with defaults. Normally set by laptop download
// default to zero so motors are stable on power up
  //Left motor constants
  int PwmBaseL  = 0;	   //motor start threshold
  int KvL       = 0;	   //velocity gain (feedforward)
  int KaccL     = 0;	   //acceleration gain (feedforward)
  int KdistL    = 0;	   //distance error gain (proportional)
  int RCoffsetL = 0;	   //center offset for RC motor output
  
  //Right motor constants
  int PwmBaseR  = 0;	   //motor start threshold
  int KvR       = 0;	   //velocity gain
  int KaccR     = 0;	   //acceleration gain
  int KdistR    = 0;	   //distance error gain
  int RCoffsetR = 0;	   //center offset for RC motor output
  
  //Common constants
  int PWMmax    = 0;	   //max duty cycle value
  
// Steering constants.  Normally set by laptop download  
  int PivotConv = 3000;    // converts degrees of rotation to wheel distance

// Motor test
  int testvalueL; 		   // these values used for test in BITE.c
  int testvalueR;
  int PWMtestOn = 0;
  
//MotorCmd globals
    int   AccMax;		    	     //accel/decel max value. 
	int   Vmax;		    		     //velocity max speed 
	long  DistTarget;  		         //+ to move forward, - for reverse
	int	  Vtarget;					 //current velocity target
	int   AccCmd,AccCmdL,AccCmdR;    //accel/decel cmd  + for forward (mm/sec^2)
	int	  Vcmd,VcmdL,VcmdR;		     //velocity cmd + for forward  (mm/sec)   
	long  DistCmd,DistCmdL,DistCmdR; //distance cmd + for forward  (mm)  
	int   DistErrL, DistErrR;	     //distance error each wheel (DistCmd-DistAct)
	long  DistToGo;					 //distance from current position to target
	int   DecelFlag;		         //1 = decel to distance target
	int   StopFlag;					 //1 = stopped at end of forward command
	long  V1 = 0;			    	 //long of Vcmd for intermediate calcs.
	long  D1 = 0;			    	 //long of DistCmd for intermediate calcs
	int   MotorMode; 		         // see MotorExec()
	int   FwdModeDone;  			 //Motion status word  0 = in progress,
				        			 //	 1 = done  2... = status message
  //steering globals	
	int			TAccMax;			 //turn accel/decel max value (mm/sec^2)
    int         TurnRateMax;		 //turn velocity max value    (mm/sec)
	long		TurnTarget;			 //turn distance target       (mm)
	int			TRtgt;				 //current turn velocity target
	int			TAccCmd;			 //turn accel/decel command
	int			TurnRateCmd;		 //turn velocity command
	long		TurnCmd;			 //turn distance command
	long		TurnToGo;  			 //distance to end of turn
	int			TurnDecelFlag;		 //1 = deceleration in progress
	int			TurnStopFlag;		 //1 = stopped at end of turn 
	long		TV1;				 //long version of TurnRateCmd for calcs.
	long		TD1;				 //long version of TurnCmd for calcs.
    int			TurnType;			 //type of turn in progress
	int         TurnModeDone;	   	 //0: in progress, 1: done, 2.. messages
	long 		SteeringCmd;		 //turn distance command passed to motorexec
	int			SteerAccCmd;		 //turn accel command passed to motorexec
	int			SteerVcmd;			 //turn velocity command passed to motorexec
	
	long 		HdgTarget;
	long		TurnIC;
	long		TurnTargetOld;
	int			HdgErr;
	
  //motor globals
	int	   		pwmL, pwmR;		   //range +/- 100 %
	int 		MotorType;	   	   //hbridge mode = 0, RC mode = 1 else -1
	
// function prototypes
    void MotorCmd(void);	//compute forward motion commands
	void Steering(void);	//compute steering commands
	void MotorHB(void);		//drive motor H bridge output
	void MotorRC(void);		//drive motor RC output

//-----------------------------------------------------------------------------	
void MotorExec()
  {
//printf("mo   %d  %d\n",time,MotorMode);
    switch (MotorMode)
	 {
	   case 0: 		  //stop			 //yes, these two cases are identical, but
	     Steering();  					 //they didn't used to be, & might not in
	     MotorCmd();					 //the future.
 		 AccCmdL = AccCmd + SteerAccCmd;
		 AccCmdR = AccCmd - SteerAccCmd;
		 VcmdL = Vcmd + SteerVcmd;
		 VcmdR = Vcmd - SteerVcmd;
		 DistCmdL =  DistCmd + TurnIC + SteeringCmd;
		 DistCmdR =  DistCmd - TurnIC - SteeringCmd;
	     break;

	   case 1: 		  //forward motion
	     Steering();
	     MotorCmd();	
 		 AccCmdL = AccCmd + SteerAccCmd;
		 AccCmdR = AccCmd - SteerAccCmd;
		 VcmdL = Vcmd + SteerVcmd;
		 VcmdR = Vcmd - SteerVcmd;
		 DistCmdL =  DistCmd + TurnIC + SteeringCmd;
		 DistCmdR =  DistCmd - TurnIC - SteeringCmd;
	     break;
	 }  	
    //calculate errors between commanded and actual distances  
    DistErrL = DistCmdL - DistActL;			       
    DistErrR = DistCmdR - DistActR;
	if(MotorType == 1)
	  MotorRC();
	else if(MotorType == 0)
	  MotorHB();  
  }
  	
 /*---------------------------------------------------------------------------
 Fractional gain multiplier
  returns a signed integer which is the product of the first and second 
  parameters divided by 1000.  Effectively, this gives the ability to 
  muliply a number by +/- 32.000.  Overflows are limited to max integer.
        e.g.  gain (500,1500)  gives 750
*/
int gain (int inp,int k)
  {	long tmp;
    tmp = (long)inp * k/1000;
	if (tmp> 32767) tmp =  32767;
	if (tmp<-32767) tmp = -32767;
	return (tmp);
  }	
  
 /*---------------------------------------------------------------------------
*/
void Steering(void)
{
	long		TAccTemp;  //used to calc turn decel rate
	long 		TAccTemp2; //used to calc turn accel rate
	
  	 switch(TurnType)
	 {
	   case 1:	 //Turn specified degrees at specified rate
	   		if(TurnTarget != TurnTargetOld) //reset decel/stop if target changed
	 		{
	 		  TurnDecelFlag = 0;
	 		  TurnStopFlag = 0;
	   		  TurnTargetOld = TurnTarget;
	 		}
	 
	 		//Set turn rate polarity based on direction to TurnTarget
	 		if(TurnTarget >= TurnCmd) TRtgt =  TurnRateMax;
	 		else                      TRtgt = -TurnRateMax;
	  		// set turnrate to zero if no laptop messages being received
	  		if(USBtimeout == USBtimeoutLimit) TRtgt = 0;
	 
	 		//Calculate deceleration necessary to stop by TurnTarget
	 		TurnToGo = TurnTarget - TurnCmd;
	 		if(TurnToGo == 0) TurnToGo = 1;  //avoid divide by zero
	 		TAccTemp = -(long) TurnRateCmd * TurnRateCmd /TurnToGo /2;
	 
	 		//Set turn decel flag if calc decel exceeds TAccMax
	 		if(abs(TAccTemp) > TAccMax) TurnDecelFlag = 1;
	 
	 		//if in decel, use calculated value of accel, else ramp to TurnRateMax
	 		if(TurnDecelFlag) TAccCmd = TAccTemp;
	 		else
	 		{  TAccTemp2 = (long)TRtgt * 61 - TV1;
	    	   if(TAccTemp2 > 32676) TAccTemp2 = 32767;  //fix overflow
			   if(TAccTemp2 <-32676) TAccTemp2 =-32767;
			   TAccCmd = (int)TAccTemp2;
			   if(TAccCmd > TAccMax) TAccCmd =  TAccMax;
			   if(TAccCmd <-TAccMax) TAccCmd = -TAccMax;
	 		}
	 	
			//or is it time to stop?
			if(abs(TurnToGo)<2)
			{  TurnStopFlag = 1;
	   		   TurnModeDone = 1;
			}	
			if (TurnStopFlag)
			{  TAccCmd = 0;
	   		   TurnRateCmd = 0;
	   		   TV1 = 0;
			}
		
	 		TV1 = TV1 + TAccCmd;
	 		TurnRateCmd = TV1 / 61;
	 		TD1 = TD1 + TurnRateCmd;
	 		TurnCmd = TD1 / 61;
//	printf("Mot  %d  %d  %ld\n",TAccCmd,TurnRateCmd,TurnCmd);
			break;

	   case 5:	 // Stop at specified acceleration
	 
	 
			// is it time to stop?
			if(abs(TurnRateCmd) < 10)
			{  TurnStopFlag = 1;
	   		   TurnModeDone = 1;
			}	
			
	 		//Set decel polarity based on existing turn rate
	 		if(TurnRateCmd > 0) TAccCmd =  -TAccMax;
	 		else                TAccCmd =  +TAccMax;
			if(abs(TV1) < abs(TAccCmd)) TAccCmd = -TV1;
		
			if (TurnStopFlag)
			{  TAccCmd = 0;
	   		   TurnRateCmd = 0;
	   		   TV1 = 0;
			}
		
	 		TV1 = TV1 + TAccCmd;
	 		TurnRateCmd = TV1 / 61;
	 		TD1 = TD1 + TurnRateCmd;
	 		TurnCmd = TD1 / 61;
//	printf("Mot  %d  %d  %ld\n",TAccCmd,TurnRateCmd,TurnCmd);
			break;

	case 10:
		   	HdgErr = HdgTarget - hdgdeg;
			if(HdgErr >  1800) HdgErr -= 3600;
			if(HdgErr < -1800) HdgErr += 3600;
			
			TAccCmd = 0;
			TurnRateCmd = 1 * HdgErr;
			if(TurnRateCmd >  TurnRateMax) TurnRateCmd =  TurnRateMax;
			if(TurnRateCmd < -TurnRateMax) TurnRateCmd = -TurnRateMax;
			TD1 = TD1 + TurnRateCmd;
			TurnCmd = TD1 / 61;
			break;


	 }
	 
		 
	 SteerAccCmd = (int)((long)TAccCmd * PivotConv / 11460);
	 SteerVcmd   = (int)((long)TurnRateCmd * PivotConv / 11460);
	 SteeringCmd = TurnCmd * PivotConv / 11460;
	 
//printf("Mo steer %d  %d %ld\n",SteerAccCmd,SteerVcmd,SteeringCmd);	 		

}  

void MotorCmd(void)
{  
   long AccTemp;  //used to calc decel rate
   long AccTemp2; //used to calc accel rate 
/*
  //calculate errors between commanded and actual distances  
    DistErrL = DistCmdL - DistActL;			       
    DistErrR = DistCmdR - DistActR;
*/	
   //Set max speed polarity depending on direction to target distance
	  if(DistTarget >= DistCmd) Vtarget =  Vmax;
	  else 			   			Vtarget = -Vmax;
	  // set speed to zero if no laptop messages being received
	  if(USBtimeout == USBtimeoutLimit) Vtarget = 0;
	  
   //Calculate deceleration necessary to stop by target distance
	  DistToGo = DistTarget - DistCmd;
	  if(DistToGo == 0) DistToGo = 1;   //limit to avoid divide by zero
	  AccTemp = -(long) Vcmd * Vcmd / DistToGo / 2;

   // set deceleration flag if calculated decel exceeds AccMax
	  if(abs(AccTemp) > AccMax) DecelFlag = 1;
	  
   // if in stop mode,rather than forward, just set to max decel
	  if((MotorMode == 0) && (StopFlag == 0)) 
	  {  if(Vcmd > 0) AccTemp = -AccMax; 
	     else         AccTemp =  AccMax;
	  } 
	  
   // if in decel, use calculated value, else ramp up to Vmax
	  if(DecelFlag)  AccCmd = AccTemp;
	  else
	  {  AccTemp2 = (long)Vtarget * 61 - V1;