text: /*
   * SCSProtocol.cpp
   * SCS���ж��Э�����
   * ����: 2016.8.25
   * ����: ̷����
   */
  
  #include <stddef.h>
  #include "SCSProtocol.h"
  
  SCSProtocol::SCSProtocol()
  {
  	Level = 1;//���㲥ָ������ָ���Ӧ��
  	End = 1;//�������������ư崦�����˽ṹ��һ��
  }
  
  //1��16λ�����Ϊ2��8λ��
  //DataLΪ��λ��DataHΪ��λ
  void SCSProtocol::Host2SCS(u8 *DataL, u8* DataH, int Data)
  {
  	if(End){
  		*DataL = (Data>>8);
  		*DataH = (Data&0xff);
  	}else{
  		*DataH = (Data>>8);
  		*DataL = (Data&0xff);
  	}
  }
  
  //2��8λ�����Ϊ1��16λ��
  //DataLΪ��λ��DataHΪ��λ
  int SCSProtocol::SCS2Host(u8 DataL, u8 DataH)
  {
  	int Data;
  	if(End){
  		Data = DataL;
  		Data<<=8;
  		Data |= DataH;
  	}else{
  		Data = DataH;
  		Data<<=8;
  		Data |= DataL;
  	}
  	return Data;
  }
  
  void SCSProtocol::writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun)
  {
  	u8 msgLen = 2;
  	u8 bBuf[6];
  	u8 CheckSum = 0;
  	bBuf[0] = 0xff;
  	bBuf[1] = 0xff;
  	bBuf[2] = ID;
  	bBuf[4] = Fun;
  	if(nDat){
  		msgLen += nLen + 1;
  		bBuf[3] = msgLen;
  		bBuf[5] = MemAddr;
  		writeSCS(bBuf, 6);
  		
  	}else{
  		bBuf[3] = msgLen;
  		writeSCS(bBuf, 5);
  	}
  	CheckSum = ID + msgLen + Fun + MemAddr;
  	u8 i = 0;
  	if(nDat){
  		for(i=0; i<nLen; i++){
  			CheckSum += nDat[i];
  		}
  	}
  	writeSCS(nDat, nLen);
  	writeSCS(~CheckSum);
  }
  
  //��ͨдָ��
  //���ID��MemAddr�ڴ���ַ��д�����ݣ�д�볤��
  int SCSProtocol::genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
  {
  	flushSCS();
  	writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
  	return Ack(ID);
  }
  
  //�첽дָ��
  //���ID��MemAddr�ڴ���ַ��д�����ݣ�д�볤��
  int SCSProtocol::regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
  {
  	flushSCS();
  	writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE);
  	return Ack(ID);
  }
  
  //ͬ��дָ��
  //���ID[]���飬IDN���鳤�ȣ�MemAddr�ڴ���ַ��д�����ݣ�д�볤��
  void SCSProtocol::snycWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen)
  {
  	u8 mesLen = ((nLen+1)*IDN+4);
  	u8 Sum = 0;
  	u8 bBuf[7];
  	bBuf[0] = 0xff;
  	bBuf[1] = 0xff;
  	bBuf[2] = 0xfe;
  	bBuf[3] = mesLen;
  	bBuf[4] = INST_SYNC_WRITE;
  	bBuf[5] = MemAddr;
  	bBuf[6] = nLen;
  	writeSCS(bBuf, 7);
  
  	Sum = 0xfe + mesLen + INST_SYNC_WRITE + MemAddr + nLen;
  	u8 i, j;
  	for(i=0; i<IDN; i++){
  		writeSCS(ID[i]);
  		writeSCS(nDat, nLen);
  		Sum += ID[i];
  		for(j=0; j<nLen; j++){
  			Sum += nDat[j];
  		}
  	}
  	writeSCS(~Sum);
  }
  
  int SCSProtocol::writeByte(u8 ID, u8 MemAddr, u8 bDat)
  {
  	flushSCS();
  	writeBuf(ID, MemAddr, &bDat, 1, INST_WRITE);
  	return Ack(ID);
  }
  
  int SCSProtocol::writeWord(u8 ID, u8 MemAddr, u16 wDat)
  {
  	flushSCS();
  	u8 buf[2];
  	Host2SCS(buf+0, buf+1, wDat);
  	writeBuf(ID, MemAddr, buf, 2, INST_WRITE);
  	return Ack(ID);
  }
  
  int SCSProtocol::EnableTorque(u8 ID, u8 Enable)
  {
  	return writeByte(ID, P_TORQUE_ENABLE, Enable);
  }
  
  int SCSProtocol::writePos(u8 ID, u16 Position, u16 Time, u16 Speed, u8 Fun)
  {
  	flushSCS();
  	u8 buf[6];
  	Host2SCS(buf+0, buf+1, Position);
  	Host2SCS(buf+2, buf+3, Time);
  	Host2SCS(buf+4, buf+5, Speed);
  	writeBuf(ID, P_GOAL_POSITION_L, buf, 6, Fun);
  	return Ack(ID);
  }
  
  //дλ��ָ��
  //���ID��Positionλ�ã�ִ��ʱ��Time��ִ���ٶ�Speed
  int SCSProtocol::WritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
  {
  	return writePos(ID, Position, Time, Speed, INST_WRITE);
  }
  
  //�첽дλ��ָ��
  //���ID��Positionλ�ã�ִ��ʱ��Time��ִ���ٶ�Speed
  int SCSProtocol::RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
  {
  	return writePos(ID, Position, Time, Speed, INST_REG_WRITE);
  }
  
  void SCSProtocol::RegWriteAction()
  {
  	writeBuf(0xfe, 0, NULL, 0, INST_ACTION);
  }
  
  //дλ��ָ��
  //���ID[]���飬IDN���鳤�ȣ�Positionλ�ã�ִ��ʱ��Time��ִ���ٶ�Speed
  void SCSProtocol::SyncWritePos(u8 ID[], u8 IDN, u16 Position, u16 Time, u16 Speed)
  {
  	u8 buf[6];
  	Host2SCS(buf+0, buf+1, Position);
  	Host2SCS(buf+2, buf+3, Time);
  	Host2SCS(buf+4, buf+5, Speed);
  	snycWrite(ID, IDN, P_GOAL_POSITION_L, buf, 6);
  }
  
  //��ָ��
  //���ID��MemAddr�ڴ���ַ����������nData�����ݳ���nLen
  int SCSProtocol::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen)
  {
  	flushSCS();
  	writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
  	u8 bBuf[5];
  	if(readSCS(bBuf, 5)!=5){
  		return 0;
  	}
  	int Size = readSCS(nData, nLen);
  	if(readSCS(bBuf, 1)){
  		return Size;
  	}
  	return 0;
  }
  
  //��1�ֽڣ���ʱ����-1
  int SCSProtocol::readByte(u8 ID, u8 MemAddr)
  {
  	u8 bDat;
  	int Size = Read(ID, MemAddr, &bDat, 1);
  	if(Size!=1){
  		return -1;
  	}else{
  		return bDat;
  	}
  }
  
  //��2�ֽڣ���ʱ����-1
  int SCSProtocol::readWord(u8 ID, u8 MemAddr)
  {	
  	u8 nDat[2];
  	int Size;
  	u16 wDat;
  	Size = Read(ID, MemAddr, nDat, 2);
  	if(Size!=2)
  		return -1;
  	wDat = SCS2Host(nDat[0], nDat[1]);
  	return wDat;
  }
  
  //��λ�ã���ʱ����-1
  int SCSProtocol::ReadPos(u8 ID)
  {
  	return readWord(ID, P_PRESENT_POSITION_L);
  }
  
  //��Ȧ����ָ��
  int SCSProtocol::WriteSpe(u8 ID, s16 Speed)
  {
  	if(Speed<0){
  		Speed = -Speed;
  		Speed |= (1<<10);
  	}
  	return writeWord(ID, P_GOAL_TIME_L, Speed);
  }
  
  //����ѹ����ʱ����-1
  int SCSProtocol::ReadVoltage(u8 ID)
  {	
  	return readByte(ID, P_PRESENT_VOLTAGE);
  }
  
  //���¶ȣ���ʱ����-1
  int SCSProtocol::ReadTemper(u8 ID)
  {	
  	return readByte(ID, P_PRESENT_TEMPERATURE);
  }
  
  //Pingָ����ض��ID����ʱ����-1
  int SCSProtocol::Ping(u8 ID)
  {
  	flushSCS();
  	u8 bBuf[6];
  	writeBuf(ID, 0, NULL, 0, INST_PING);
  	int Size = readSCS(bBuf, 6);
  	if(Size==6){
  		return bBuf[2];
  	}else{
  		return -1;
  	}
  }
  
  int SCSProtocol::wheelMode(u8 ID)
  {
  	u8 bBuf[4];
  	bBuf[0] = 0;
  	bBuf[1] = 0;
  	bBuf[2] = 0;
  	bBuf[3] = 0;
  	return genWrite(ID, P_MIN_ANGLE_LIMIT_L, bBuf, 4);
  }
  
  int SCSProtocol::joinMode(u8 ID, u16 minAngle, u16 maxAngle)
  {
  	u8 bBuf[4];
  	Host2SCS(bBuf, bBuf+1, minAngle);
  	Host2SCS(bBuf+2, bBuf+3, maxAngle);
  	return genWrite(ID, P_MIN_ANGLE_LIMIT_L, bBuf, 4);
  }
  
  //��λ�������ΪĬ��ֵ
  int SCSProtocol::Reset(u8 ID)
  {
  	flushSCS();
  	writeBuf(ID, 0, NULL, 0, INST_RESET);
  	return Ack(ID);
  }
  
  int	SCSProtocol::Ack(u8 ID)
  {
  	if(ID != 0xfe && Level){
  		u8 buf[6];
  		u8 Size = readSCS(buf, 6);
  		if(Size!=6){
  			return 0;
  		}
  	}
  	return 1;
  }
  