/**
  ******************************************************************************
  * @file 		MR2X5
  * @author  	Innovati Team
  * @version  	V0.1.0
  * @date     	07/18/2011
  * @brief  	Main program body
  ******************************************************************************
  * @copy
  *
  * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
  * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
  * TIME. AS A RESULT, Innovati SHALL NOT BE HELD LIABLE FOR ANY
  * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
  * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
  * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
  *
  * <h2><center>&copy; COPYRIGHT 2011 Innovati, Inc</center></h2>
  */
/* Includes ------------------------------------------------------------------*/
#include "arm32f.h"
#include "I2CRoutines.h"
#include "MR2X5A.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/*----------------------command list------------------------------------------*/                                                                
/*----------------------------------------------------------------------------*/
MR2X5A::MR2X5A(uint8_t mySlaveID)
{
	SlaveID = mySlaveID;
    InitArmCommander();
} 
void MR2X5A::ForwardA(uint16_t DutyCycle)
{
#ifdef __CHECK__
	char *ptr = (char *)0x1f0007B5;
	if(*ptr != 'E') __innoCommandBuff__[0] = 88;
	else 	
#endif	  
  __innoCommandBuff__[0] = 88;
  __innoCommandBuff__[2] = DutyCycle;
  __innoCommandBuff__[3] = DutyCycle>>8;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::ForwardB(uint16_t DutyCycle)
{
  __innoCommandBuff__[0] = 89;
  __innoCommandBuff__[2] = DutyCycle;
  __innoCommandBuff__[3] = DutyCycle>>8;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
} 
void MR2X5A::ForwardAB(uint16_t DutyCycleA, uint16_t DutyCycleB)
{
  __innoCommandBuff__[0] = 90;
  __innoCommandBuff__[2] = DutyCycleA;
  __innoCommandBuff__[3] = DutyCycleA>>8;
  __innoCommandBuff__[4] = DutyCycleB;
  __innoCommandBuff__[5] = DutyCycleB>>8;  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);
}
void MR2X5A::BackwardA(uint16_t DutyCycle)
{
  __innoCommandBuff__[0] = 92;
  __innoCommandBuff__[2] = DutyCycle;
  __innoCommandBuff__[3] = DutyCycle>>8;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::BackwardB(uint16_t DutyCycle)
{
  __innoCommandBuff__[0] = 93;
  __innoCommandBuff__[2] = DutyCycle;
  __innoCommandBuff__[3] = DutyCycle>>8;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}	
void MR2X5A::BackwardAB(uint16_t DutyCycleA, uint16_t DutyCycleB)
{
  __innoCommandBuff__[0] = 94;
  __innoCommandBuff__[2] = DutyCycleA;
  __innoCommandBuff__[3] = DutyCycleA>>8;
  __innoCommandBuff__[4] = DutyCycleB;
  __innoCommandBuff__[5] = DutyCycleB>>8;  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);
}
void MR2X5A::BrakeA(void)
{
  __innoCommandBuff__[0] = 99;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
void MR2X5A::BrakeB(void)
{
  __innoCommandBuff__[0] = 100;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
void MR2X5A::BrakeDual(void)
{
  __innoCommandBuff__[0] = 101;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}    	
void MR2X5A::StopA(void)
{
  __innoCommandBuff__[0] = 96;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
} 
void MR2X5A::StopB(void)
{
  __innoCommandBuff__[0] = 97;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
} 
void MR2X5A::StopDual(void)
{
  __innoCommandBuff__[0] = 98;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
} 
void MR2X5A::SetDirAB(uint8_t DirA, uint8_t DirB)
{
  __innoCommandBuff__[0] = 104;
  __innoCommandBuff__[2] = DirA;
  __innoCommandBuff__[3] = DirB;	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::SetVelAB(int16_t DutyCycleA, int16_t DutyCycleB)
{
  __innoCommandBuff__[0] = 118;
  *((int16_t *)&__innoCommandBuff__[2]) = DutyCycleA;
  *((int16_t *)&__innoCommandBuff__[4]) = DutyCycleB;	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);
}
void MR2X5A::GetCurr(uint16_t &CurrA, uint16_t &CurrB) 
{
  __innoCommandBuff__[0] = 132;
  __innoNumByteToRead__ = 5;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
  	CurrA = *((uint16_t *)&__innoCommandBuff__[0]);	  	
  	CurrB = *((uint16_t *)&__innoCommandBuff__[2]);
  }		  	  
}

void MR2X5A::SetCurrMode(uint8_t Mode, uint8_t Time, uint16_t Curr)
{
  __innoCommandBuff__[0] = 133;
  __innoCommandBuff__[2] = Mode;
  __innoCommandBuff__[3] = Time;
  __innoCommandBuff__[4] = Curr;
  __innoCommandBuff__[5] = Curr>>8;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);	  	
}

uint8_t MR2X5A::GetBrakeButStatus(void)
{
  __innoCommandBuff__[0] = 123;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
    return __innoCommandBuff__[0]; 	
  return 0;
}     
void MR2X5A::ClrBrakeButStatus(void)
{
  __innoCommandBuff__[0] = 124;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);	  	
}
uint8_t MR2X5A::GetFaultStatus(void)
{
  __innoCommandBuff__[0] = 127;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
    return __innoCommandBuff__[0]; 	
  return 0;
}    
void MR2X5A::EnFaultStop(void)
{
  __innoCommandBuff__[0] = 125;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
void MR2X5A::DisFaulStop(void)
{
  __innoCommandBuff__[0] = 126;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
void MR2X5A::ClearFaultStatus(void)
{
  __innoCommandBuff__[0] = 128;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
void MR2X5A::RestoreStatus(void)
{
  __innoCommandBuff__[0] = 131;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
	
void MR2X5A::ForwardDual(uint16_t Dutycycle)
{        
  __innoCommandBuff__[0] = 91;
  *((uint16_t*)&__innoCommandBuff__[2]) = Dutycycle;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::BackwardDual(uint16_t Dutycycle)
{       
  __innoCommandBuff__[0] = 95;
  *((uint16_t*)&__innoCommandBuff__[2]) = Dutycycle;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::SetDirA(uint8_t Dir)
{  	        
  __innoCommandBuff__[0] = 102;
  __innoCommandBuff__[2] = Dir;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void MR2X5A::SetDirB(uint8_t Dir)
{        
  __innoCommandBuff__[0] = 103;
  __innoCommandBuff__[2] = Dir;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void MR2X5A::SetDirDual(uint8_t Dir)
{        
  __innoCommandBuff__[0] = 105;
  __innoCommandBuff__[2] = Dir;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void MR2X5A::SetDCA(uint16_t Spd)
{          
  __innoCommandBuff__[0] = 106;
  *((uint16_t *)&__innoCommandBuff__[2]) = Spd;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::SetDCB(uint16_t Spd)
{      
  __innoCommandBuff__[0] = 107;
  *((uint16_t *)&__innoCommandBuff__[2]) = Spd;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::SetDCAB(uint16_t SpdA, uint16_t SpdB)
{     
  __innoCommandBuff__[0] = 108;
  *((uint16_t *)&__innoCommandBuff__[2]) = SpdA;
  *((uint16_t *)&__innoCommandBuff__[4]) = SpdB;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);
}
void MR2X5A::SetDCDual(uint16_t Spd)
{        
  __innoCommandBuff__[0] = 109;
  *((uint16_t *)&__innoCommandBuff__[2]) = Spd;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void MR2X5A::GetDCA(uint16_t& Spd)
{  
  __innoCommandBuff__[0] = 110;
  __innoNumByteToRead__ = 3;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Spd = *((uint16_t *)&__innoCommandBuff__[0]);
  }	        
}
void MR2X5A::GetDCB(uint16_t& Spd)
{  
  __innoCommandBuff__[0] = 111;
  __innoNumByteToRead__ = 3;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Spd = *((uint16_t *)&__innoCommandBuff__[0]);
  }	        
}
void MR2X5A::GetDirA(uint8_t& Dir)
{  
  __innoCommandBuff__[0] = 112;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Dir = __innoCommandBuff__[0];
  }	        
}
void MR2X5A::GetDirB(uint8_t& Dir)
{  
  __innoCommandBuff__[0] = 113;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Dir = __innoCommandBuff__[0];
  }	        
}
void MR2X5A::GetDCAB(uint16_t& SpdA, uint16_t& SpdB)
{  
  __innoCommandBuff__[0] = 114;
  __innoNumByteToRead__ = 5;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  SpdA = *((uint16_t*)&__innoCommandBuff__[0]);
	  SpdB = *((uint16_t*)&__innoCommandBuff__[2]);
  }	        

}
void MR2X5A::GetDirAB(uint8_t& DirA, uint8_t& DirB)
{  
  __innoCommandBuff__[0] = 115;
  __innoNumByteToRead__ = 3;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  DirA = __innoCommandBuff__[0];
	  DirB = __innoCommandBuff__[1];
  }	        

}
void MR2X5A::SetVelA(int16_t Vel)
{  
  __innoCommandBuff__[0] = 116;
  *((int16_t *)&__innoCommandBuff__[2]) = Vel;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);        
}
void MR2X5A::SetVelB(int16_t Vel)
{  
  __innoCommandBuff__[0] = 117;
  *((int16_t *)&__innoCommandBuff__[2]) = Vel;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);       
}
void MR2X5A::SetVelDual(int16_t Vel)
{  
  __innoCommandBuff__[0] = 119;
  *((int16_t *)&__innoCommandBuff__[2]) = Vel;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);        
}
void MR2X5A::GetVelA(int16_t& Vel)
{  
  __innoCommandBuff__[0] = 120;
  __innoNumByteToRead__ = 3;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Vel = *((int16_t *)&__innoCommandBuff__);   
  }	        
}
void MR2X5A::GetVelB(int16_t& Vel)
{  
  __innoCommandBuff__[0] = 121;
  __innoNumByteToRead__ = 3;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Vel = *((int16_t *)&__innoCommandBuff__);   
  }
}
void MR2X5A::GetVelAB(int16_t& VelA, int16_t& VelB)
{  
  __innoCommandBuff__[0] = 122;
  __innoNumByteToRead__ = 5;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  VelA = *((int16_t *)&__innoCommandBuff__[0]);   
	  VelB = *((int16_t *)&__innoCommandBuff__[2]); 
  }	        
}
void MR2X5A::GetCurrMode(uint8_t& Mode, uint8_t& Time, uint16_t& Curr)
{  
  __innoCommandBuff__[0] = 134;
  __innoNumByteToRead__ = 5;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Mode = __innoCommandBuff__[0];   
	  Time = __innoCommandBuff__[1];
      Curr = *((uint16_t *)&__innoCommandBuff__[2]);  
  }	        
}   






