if (AccTemp2 > 32767) AccTemp2 =  32767; //fix overflow when convert
		 if (AccTemp2 <-32767) AccTemp2 = -32767; //  to integer next line
		 AccCmd = (int)AccTemp2;
		 if(AccCmd > AccMax) AccCmd = AccMax;
		 if(AccCmd <-AccMax) AccCmd =-AccMax;
	  }

   //Keep motors from splitting if pwm limit reached
   		//if both motors below pwm limit, accelerate normally  
		if(pwmL < PWMmax && pwmR < PWMmax)
		  {V1 = V1 + AccCmd;
		  }
		else  //else don't incr Vcmd & back off DistCmd to wheel farthest behind  
		  { if (Vcmd > 0)  //going forward
		    {  if(DistErrL>0 || DistErrR>0)
		       { if(pwmL >= PWMmax && pwmR >= PWMmax) 	 //if both on limit
		         { if (DistErrL > DistErrR)  
				        D1 = D1 - (long)DistErrL * 61 * 1000 / KdistL;
			       else D1 = D1 - (long)DistErrR * 61 * 1000 / KdistR;
			     }
		         else	   	 	  			  	  	  //else just one on limit
			     { if (pwmL >= PWMmax)
				        D1 = D1 - (long)DistErrL * 61 * 1000 / KdistL;
			       if (pwmR >= PWMmax) 
				        D1 = D1 - (long)DistErrR * 61 * 1000 / KdistR;
			     }
			   }	 
		    }	   	      
			else               //going in reverse
			{  if(DistErrL<0 || DistErrR<0)
		       { if(pwmL >= PWMmax && pwmR >= PWMmax) 	 //if both on limit
		         { if (DistErrL < DistErrR) 
				        D1 = D1 - (long)DistErrL * 61 * 1000 / KdistL;
			       else D1 = D1 - (long)DistErrR * 61 * 1000 / KdistR;
			     }
		         else	   	 	  			  	  	   	 //else just one on limit
			     { if (pwmL >= PWMmax) 
				        D1 = D1 - (long)DistErrL * 61 * 1000 / KdistL;
			       if (pwmR >= PWMmax)
				        D1 = D1 - (long)DistErrR * 61 * 1000 / KdistR;
			     }
		       }	
			}
		  }	
		
	    Vcmd = V1/61;
	    D1 += Vcmd;
        DistCmd = D1/61;

   //or is it time to stop?
	    if((MotorMode == 1) && (abs(DistToGo) < 2))  //in forward mode
		{ StopFlag = 1;
		  FwdModeDone = 1;
		}
		
//	    if ((MotorMode == 0) && (abs(Vcmd) < 5) )	 //in stop mode
	    if ((MotorMode == 0) && (abs(Speed) < 10) )	 //in stop mode
		 { DistTarget = DistCmd;
		   StopFlag = 1;
		   FwdModeDone = 1;
		 }  

		if (StopFlag)
		 {  AccCmd = 0;
			Vcmd = 0;
			V1 = 0;
			DistCmd = DistTarget;
			D1 = (long) DistCmd * 61;
		 }
}   //end of function MotorCmd()

  
//---------------------------------------------------------------------------
/*  MOTOR SPEED CONTROL

    Calculates the necessary pwm duty cycle for each wheel.  Sets direction bit
	output, limits to maximum pwm level (100%) and sets PWM duty cycle register.
	Brake output to h-bridge is not controlled (always turned off).
*/ 

void MotorHB(void) 
  { 
	// Left motor calculations
    pwmL = gain(VcmdL,KvL) + gain(AccCmdL,KaccL) + gain(DistErrL,KdistL);
    if(PWMtestOn) pwmL = testvalueL;       // BITE test pwm override 

	if (pwmL > 0) PTH &= ~0x04;		       //set direction bit to forward
    else          PTH |=  0x04;		       //set direction bit to reverse

	pwmL = abs(pwmL) + PwmBaseL;  	 	   //add motor breakout pwm
	if(pwmL > PWMmax) pwmL = PWMmax;
	PWMDTY2 = pwmL;
	
	PTH &= ~0x08;  	  	   	       		   //set brake bit off   

	 // Right motor calculations
    pwmR = gain(VcmdR,KvR) + gain(AccCmdR,KaccR) + gain(DistErrR,KdistR);
    if(PWMtestOn) pwmR = testvalueR;       // BITE test pwm override 

	if (pwmR > 0) PTH |=  0x10; 	       //set direction bit to forward
    else          PTH &= ~0x10;		       //set direction bit to reverse

	pwmR = abs(pwmR) + PwmBaseR;  	 	   //add motor breakout pwm
	if(pwmR > PWMmax) pwmR = PWMmax;
	PWMDTY1 = pwmR;
	
	PTH &= ~0x20;  	  	   	       		   //set brake bit off   
	
  }  		  //end of function 
  
void MotorRC(void) 
  { 
	int	temp;
	
	// Left motor calculations
    pwmL = gain(VcmdL,KvL) + gain(AccCmdL,KaccL) + gain(DistErrL,KdistL);
    if(PWMtestOn) pwmL = testvalueL;       // BITE test pwm override 
//printf("xx %d %d %d %d %d %d %d",VcmdL,KvL,AccCmdL,KaccL,DistErrL,KdistL,pwmL);
	if(pwmL > 0) pwmL += PwmBaseL;
	if(pwmL < 0) pwmL -= PwmBaseL;
	if(pwmL >  PWMmax) pwmL =  PWMmax;
	if(pwmL < -PWMmax) pwmL = -PWMmax;
	temp = 1500 + RCoffsetL + 5 * pwmL;
//printf("   %d\n",pwmL);
	PWMDTY2 = temp / 256;
	PWMDTY3 = temp - PWMDTY2 * 256;
	PWMCNT3 = 0;   	 		   	   //to restart PWM period
	
	 // Right motor calculations
    pwmR = gain(VcmdR,KvR) + gain(AccCmdR,KaccR) + gain(DistErrR,KdistR);
    if(PWMtestOn) pwmR = testvalueR;       // BITE test pwm override 

	if(pwmR > 0) pwmR += PwmBaseR;
	if(pwmR < 0) pwmR -= PwmBaseR;
	if(pwmR >  PWMmax) pwmR =  PWMmax;
	if(pwmR < -PWMmax) pwmR = -PWMmax;
	temp = 1500 + RCoffsetR + 5 * pwmR;
	
	PWMDTY0 = temp / 256;
	PWMDTY1 = temp - PWMDTY0 * 256;
	PWMCNT1 = 0;     		   	   	 	   //to restart PWM period
//printf("%d %d %d %d %d\n",pwmR, pwmL,VcmdL,AccCmdL,DistErrL);
  }  		  //end of function 
  
//---------------------------------------------------------------------------
//sets up stop forward motion mode 
void MotorStop(int Acc) 
  {  
   MotorMode = 0;
    if(Acc != 0) AccMax = abs(Acc);
	else         AccMax = 300;  //default decel
	StopFlag = 0;
	DecelFlag = 1;
	FwdModeDone = 0;
//printf("Mo Stop AccMax = %d \n",AccCmd);
  }	
	
//---------------------------------------------------------------------------
//sets up forward/reverse mode and starts odom at zero.
void MotorFwd(int Acc, int Vmx, long int DistTarg, int ResetOdom) 
  { 
    if(Acc != 0) AccMax = abs(Acc);
	else         AccMax = 300;  //default accel
	
	Vmax   = abs(Vmx);
   //reset decel  & stop flags if target changes
	  if(DistTarget != DistTarg)
	  {  DecelFlag = 0;
	     StopFlag = 0;
	  }
	DistTarget = DistTarg;
	if(MotorMode != 1)             //if starting a new forward/reverse motion
	 {MotorMode = 1;
	  AccCmd = 0;
	  Vcmd = 0;
      StopFlag = 0;
	  DecelFlag = 0;
	 }
	if(ResetOdom == 1) 
	 {D1 = 0; DistCmd = DistCmdL = DistCmdR =0;
	  V1 = 0;
	  StopFlag = 0;
	  DecelFlag = 0;
	  enc0 = enc1 = 0;  	       //reset encoders to zero
	  odomLlast = 0;
	  odomRlast = 0;
	  
     TurnCmd = 0; 				   //reset turn data
	 TurnIC = 0;
	 TurnTarget = 0;
	 TV1 = TD1 = 0;
	 }
	FwdModeDone = 0;
//printf("//mo Fwd   AMx %d  VMx %d DT %ld\n",AccMax,Vmax,DistTarget);	
  }	
	

//---------------------------------------------------------------------------
//sets up stop turn mode 
void MotorTurnStop(int TAcc) 
  {  
     if(TAcc != 0) TAccMax = abs(TAcc);
	 else          TAccMax = 150;  //default turn accel
	 TurnType = 5;
	 TurnStopFlag = 0;
	 TurnDecelFlag = 1;
	 TurnModeDone = 0;
//printf("//Mo TurnStop TAccMax = %d \n",TAccMax);
  }	
	
//---------------------------------------------------------------------------
//Turns from current direction a specified number of degrees
void   MotorTurnDeg(int TAcc, int temp1, long tempL1)
   {  
     if(TAcc != 0) TAccMax = abs(TAcc);
	 else          TAccMax = 150;  //default turn accel
	 
	 TurnRateMax = abs(temp1);
	 TurnIC += TurnCmd*PivotConv/11460;
	 TD1 = 0;
	 TurnCmd = 0;
     TurnTarget = tempL1;
	 TurnType = 1;
	 TurnDecelFlag = 0;
	 TurnStopFlag = 0;
	 TurnModeDone = 0;
//printf("//mo MotorTurnDeg  %d  %d  %ld \n",TAccMax,TurnRateMax,TurnTarget);
   }   

//---------------------------------------------------------------------------
//Modifies ongoing MotorTurnDeg command
void   MotorTurnDegMod(int TAcc, int temp1, long tempL1)
   {  
     if(TAcc != 0) TAccMax = abs(TAcc);
	 else          TAccMax = 150;  //default turn accel
	 
	 TurnRateMax = abs(temp1);
   //reset decel  & stop flags if target changes Note: duplicated in Steering(),
   //  maybe this one can be deleted, I think Steering() version more general.
	  if(TurnTarget != temp1)
	  {  TurnDecelFlag = 0;
	     TurnStopFlag = 0;
	  }
     TurnTarget = tempL1;
	 TurnModeDone = 0;
//printf("mo MotorTurn  %d  %d  %ld \n",TAccMax,TurnRateMax,TurnTarget);
  }   

//---------------------------------------------------------------------------
//Turns to a specified heading  
void   MotorHdgSel(int TAcc, int temp1, long tempL1)
   { 
     if(TAcc != 0) TAccMax = abs(TAcc);
	 else         AccMax = 150;  //default turn accel
	 
     TurnRateMax = abs(temp1);
     HdgTarget = tempL1;
	 TurnType = 10;
	 TurnIC = TurnCmd * PivotConv/11460;
	 TurnTarget = 0;
	 TD1 = 0;
	 TurnModeDone = 0;
//printf("mo MotorHdgSel  %d  %d  %ld \n",TAccMax,TurnRateMax,HdgTarget);
   }   
//---------------------------------------------------------------------------
//Controls to maintain current heading +/- tempL1
void   MotorHdgHold(int TAcc, int temp1, long tempL1)
   { 
     if(TAcc != 0) TAccMax = abs(TAcc);
	 else         AccMax = 150;  //default turn accel
	 
     TurnRateMax = abs(temp1*10);
     HdgTarget = Compass + tempL1;
	 TurnType = 10;
	 TurnIC = TurnCmd * PivotConv/11460;
	 TurnTarget = 0;
	 TD1 = 0;
	 TurnModeDone = 0;
//printf("mo MotorTurn  %d  %d  %ld \n",TAccMax,TurnRateMax,TurnTarget);
   }   

//  OPEN SOURCE SOFTWARE LICENSE
/* Permission is hereby granted, free of charge, to any person obtaining a copy 
of this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to use, 
copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
Software, and to permit persons to whom the Software is furnished to do so, 
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/