/**
  ******************************************************************************
  * @file 		ServoRunner8A
  * @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 "ServoRunner8.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/*----------------------command list------------------------------------------*/                                                                
/*----------------------------------------------------------------------------*/
ServoRunner8::ServoRunner8(uint8_t mySlaveID)
{
	SlaveID = mySlaveID;
    InitArmCommander();
}
void ServoRunner8::SetPosAndRun(uint8_t SerID, uint16_t Pos)
{
  __innoCommandBuff__[0] = 88;
  __innoCommandBuff__[2] = SerID;
  *((uint16_t *)&__innoCommandBuff__[3]) = Pos;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 7);
}
void ServoRunner8::SetPosSpdAndRun(uint8_t SerID, uint16_t Pos, uint16_t Spd)
{
  __innoCommandBuff__[0] = 89;
  __innoCommandBuff__[2] = SerID;
  *((uint16_t *)&__innoCommandBuff__[3]) = Pos;
  *((uint16_t *)&__innoCommandBuff__[5]) = Spd;	      	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 9);
}
void ServoRunner8::SetPosTimeAndRun(uint8_t SerID, uint16_t Pos, uint16_t Time)
{
  __innoCommandBuff__[0] = 90;
  __innoCommandBuff__[2] = SerID;
  *((uint16_t *)&__innoCommandBuff__[3]) = Pos;
  *((uint16_t *)&__innoCommandBuff__[5]) = Time;	      	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 9);
}
void ServoRunner8::SetPos(uint8_t SerID, uint16_t Pos)
{
  __innoCommandBuff__[0] = 91;
  __innoCommandBuff__[2] = SerID;
  *((uint16_t *)&__innoCommandBuff__[3]) = Pos; 
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 7);
}
void ServoRunner8::SetPosSpd(uint8_t SerID, uint16_t Pos, uint16_t Spd)
{
  __innoCommandBuff__[0] = 92;
  __innoCommandBuff__[2] = SerID;
  *((uint16_t *)&__innoCommandBuff__[3]) = Pos;
  *((uint16_t *)&__innoCommandBuff__[5]) = Spd;  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void ServoRunner8::SetPosTime(uint8_t SerID, uint16_t Pos, uint16_t Time)
{
  __innoCommandBuff__[0] = 93;
  __innoCommandBuff__[2] = SerID;
  *((uint16_t *)&__innoCommandBuff__[3]) = Pos;
  *((uint16_t *)&__innoCommandBuff__[5]) = Time;	 	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void ServoRunner8::Run1Servo(uint8_t S1)
{
  __innoCommandBuff__[0] = 94;
  __innoCommandBuff__[2] = S1;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void ServoRunner8::Run2Servo(uint8_t S1, uint8_t S2)
{
  __innoCommandBuff__[0] = 95;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void ServoRunner8::Run3Servo(uint8_t S1, uint8_t S2, uint8_t S3)
{
  __innoCommandBuff__[0] = 96;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 7);
}
void ServoRunner8::Run4Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4)
{
  __innoCommandBuff__[0] = 97;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;		  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);
}
void ServoRunner8::Run5Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5)
{
  __innoCommandBuff__[0] = 98;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;      	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 9);
}
void ServoRunner8::Run6Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5, uint8_t S6)
{
  __innoCommandBuff__[0] = 99;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;
  __innoCommandBuff__[7] = S6;  	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 10);
}
void ServoRunner8::Run7Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5, uint8_t S6, uint8_t S7)
{
  __innoCommandBuff__[0] = 100;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;
  __innoCommandBuff__[7] = S6; 
  __innoCommandBuff__[8] = S7;   	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 11);
}

void ServoRunner8::RunAllServo(void)
{
  __innoCommandBuff__[0] = 109;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}  

void ServoRunner8::Pause1Servo(uint8_t S1)
{
  __innoCommandBuff__[0] = 197;
  __innoCommandBuff__[2] = S1;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void ServoRunner8::Pause2Servo(uint8_t S1, uint8_t S2)
{
  __innoCommandBuff__[0] = 198;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void ServoRunner8::Pause3Servo(uint8_t S1, uint8_t S2, uint8_t S3)
{
  __innoCommandBuff__[0] = 199;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 7);
}
void ServoRunner8::Pause4Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4)
{
  __innoCommandBuff__[0] = 200;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;		  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);
}
void ServoRunner8::Pause5Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5)
{
  __innoCommandBuff__[0] = 201;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;      	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 9);
}
void ServoRunner8::Pause6Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5, uint8_t S6)
{
  __innoCommandBuff__[0] = 202;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;
  __innoCommandBuff__[7] = S6;  	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 10);
}
void ServoRunner8::Pause7Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5, uint8_t S6, uint8_t S7)
{
  __innoCommandBuff__[0] = 203;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;
  __innoCommandBuff__[7] = S6; 
  __innoCommandBuff__[8] = S7;   	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 11);
}

void ServoRunner8::PauseAllServo(void)
{
  __innoCommandBuff__[0] = 212;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
void ServoRunner8::Stop1Servo(uint8_t S1)
{
  __innoCommandBuff__[0] = 213;
  __innoCommandBuff__[2] = S1;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 5);
}
void ServoRunner8::Stop2Servo(uint8_t S1, uint8_t S2)
{
  __innoCommandBuff__[0] = 214;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}
void ServoRunner8::Stop3Servo(uint8_t S1, uint8_t S2, uint8_t S3)
{
  __innoCommandBuff__[0] = 215;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 7);
}
void ServoRunner8::Stop4Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4)
{
  __innoCommandBuff__[0] = 216;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;		  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 8);
}
void ServoRunner8::Stop5Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5)
{
  __innoCommandBuff__[0] = 217;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;      	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 9);
}
void ServoRunner8::Stop6Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5, uint8_t S6)
{
  __innoCommandBuff__[0] = 218;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;
  __innoCommandBuff__[7] = S6;  	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 10);
}
void ServoRunner8::Stop7Servo(uint8_t S1, uint8_t S2, uint8_t S3, uint8_t S4, uint8_t S5, uint8_t S6, uint8_t S7)
{
  __innoCommandBuff__[0] = 219;
  __innoCommandBuff__[2] = S1;
  __innoCommandBuff__[3] = S2;
  __innoCommandBuff__[4] = S3;	  
  __innoCommandBuff__[5] = S4;	
  __innoCommandBuff__[6] = S5;
  __innoCommandBuff__[7] = S6; 
  __innoCommandBuff__[8] = S7;   	  
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 11);
}

void ServoRunner8::StopAllServo(void)
{
  __innoCommandBuff__[0] = 228;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 3);
}
   
void ServoRunner8::SetPosOffset(uint8_t SerID, int8_t Offset) 
{
  __innoCommandBuff__[0] = 193;
  __innoCommandBuff__[2] = SerID;
  __innoCommandBuff__[3] = Offset;
  __i2cWriteBuffer(__innoCommandBuff__, SlaveID, 6);
}   
                                                              	

void ServoRunner8::Get1ServoReadyStatus(uint8_t ID0, uint8_t& Status)
{  
  __innoCommandBuff__[0] = 174;
  __innoCommandBuff__[2] = ID0;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 5, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::Get2ServoReadyStatus(uint8_t ID0, uint8_t ID1, uint8_t& Status)
{  
  __innoCommandBuff__[0] = 175;
  __innoCommandBuff__[2] = ID0;
  __innoCommandBuff__[3] = ID1;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 6, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::Get3ServoReadyStatus(uint8_t ID0, uint8_t ID1, uint8_t ID2, uint8_t& Status)
{  
  __innoCommandBuff__[0] = 176;
  __innoCommandBuff__[2] = ID0;
  __innoCommandBuff__[3] = ID1;
  __innoCommandBuff__[4] = ID2;      
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 7, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::Get4ServoReadyStatus(uint8_t ID0, uint8_t ID1, uint8_t ID2, uint8_t ID3, uint8_t& Status)
{  
  __innoCommandBuff__[0] = 177;
  __innoCommandBuff__[2] = ID0;
  __innoCommandBuff__[3] = ID1;
  __innoCommandBuff__[4] = ID2;
  __innoCommandBuff__[5] = ID3;  
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 8, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::Get5ServoReadyStatus(uint8_t ID0, uint8_t ID1, uint8_t ID2, uint8_t ID3, 
                                        uint8_t ID4, uint8_t& Status)
{  
  __innoCommandBuff__[0] = 178;
  __innoCommandBuff__[2] = ID0;
  __innoCommandBuff__[3] = ID1;
  __innoCommandBuff__[4] = ID2;
  __innoCommandBuff__[5] = ID3;    
  __innoCommandBuff__[6] = ID4; 
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 9, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::Get6ServoReadyStatus(uint8_t ID0, uint8_t ID1, uint8_t ID2, uint8_t ID3, 
                                        uint8_t ID4, uint8_t ID5, uint8_t& Status)
{  
  __innoCommandBuff__[0] = 179;
  __innoCommandBuff__[2] = ID0;
  __innoCommandBuff__[3] = ID1;
  __innoCommandBuff__[4] = ID2;
  __innoCommandBuff__[5] = ID3;    
  __innoCommandBuff__[6] = ID4;    
  __innoCommandBuff__[7] = ID5;      
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 10, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::Get7ServoReadyStatus(uint8_t ID0, uint8_t ID1, uint8_t ID2, uint8_t ID3, 
                                        uint8_t ID4, uint8_t ID5, uint8_t ID6, uint8_t& Status)
{  
  __innoCommandBuff__[0] = 180;
  __innoCommandBuff__[2] = ID0;
  __innoCommandBuff__[3] = ID1;
  __innoCommandBuff__[4] = ID2;
  __innoCommandBuff__[5] = ID3;    
  __innoCommandBuff__[6] = ID4;  
  __innoCommandBuff__[7] = ID5;    
  __innoCommandBuff__[8] = ID6;      
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 11, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::GetAllServoReadyStatus(uint8_t& Status)
{  
  __innoCommandBuff__[0] = 189;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Status = __innoCommandBuff__[0];
  }	        
}


void ServoRunner8::GetNowPos(uint8_t ID, uint16_t& Pos)
{  
  __innoCommandBuff__[0] = 190;
  __innoCommandBuff__[2] = ID;
  __innoNumByteToRead__ = 4;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 5, &__innoNumByteToRead__))
  {
	  Pos = *((uint16_t *)&__innoCommandBuff__);   
  }	        

}


void ServoRunner8::GetPos(uint8_t ID, uint16_t& Pos)
{  
  __innoCommandBuff__[0] = 191;
  __innoCommandBuff__[2] = ID;
  __innoNumByteToRead__ = 3;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 5, &__innoNumByteToRead__))
  {
	  Pos = *((uint16_t *)&__innoCommandBuff__);   
  }	        

}


void ServoRunner8::GetSpdAndTime(uint8_t ID, uint8_t& Spd, uint16_t& Time)
{  
  __innoCommandBuff__[0] = 192;
  __innoNumByteToRead__ = 4;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  { 
	  Spd = __innoCommandBuff__[0];
	  Time = *((uint16_t *)&__innoCommandBuff__[1]);        
  }	        
}


void ServoRunner8::GetPosOffset(uint8_t ID, uint8_t& Offset)
{  
  __innoCommandBuff__[0] = 194;
  __innoNumByteToRead__ = 2;
  if(!__i2cReadBuffer(__innoCommandBuff__, SlaveID, 3, &__innoNumByteToRead__))
  {
	  Offset = __innoCommandBuff__[0];
  }	        
}




