/*
 * SCSProtocol.h
 * SCS 串行舵机协议程序 [serial servo protocol programs - tlfong01's translation from Chinese v0.1  2020sep29hkt1459]
 * 日期: 2016.8.25
 * 作者: 谭雄乐 [Tam Hung Lok]
 */

#ifndef _SCSPROTOCOL_H
#define _SCSPROTOCOL_H

typedef		char			s8;
typedef		unsigned char	u8;
typedef		unsigned short	u16;
typedef		short			s16;
typedef		unsigned long	u32;
typedef		long			s32;

class SCSProtocol{
public:
	SCSProtocol();
	int genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//普通写指令 [general write instruction

/*
 * SCSProtocol.cpp
 * SCS串行舵机协议程序 [scs serial servo portocol program]
 * 日期: 2016.8.25
 * 作者: 谭雄乐 [Tam Hung Lok]
 */

#include <stddef.h>
#include "SCSProtocol.h"

SCSProtocol::SCSProtocol()
{
	Level = 1;//除广播指令所有指令返回应答 [except broadcast instruction, all instructions return reply]
	End = 1;//舵机处理器与控制板处理器端结构不一致 [servo processor is different from control boardside structure not identical]
}

//1个16位数拆分为2个8位数 [1 16-bit integer split into 2 8-bit integer]
//DataL为低位，DataH为高位 [DataL is low, DataH is High]

#include <SCServo.h>

SCServo SERVO;

int LEDpin = 13;
void setup()
{
  pinMode(LEDpin,OUTPUT);
  digitalWrite(LEDpin, HIGH);
	Serial1.begin(1000000);
  Serial.begin(115200);
  SERVO.pSerial = &Serial1;
  delay(500);
}

void loop()
{
  int vol = SERVO.ReadVoltage(1);
	if(vol!=-1)
  {
    digitalWrite(LEDpin, LOW);
    Serial.println(vol, DEC);
    delay(10);
   }
   else
   {
    digitalWrite(LEDpin, HIGH);
    delay(2000);
   }
}

//读电压，超时返回-1 [read voltage in, timeout return -1]
int SCSProtocol::ReadVoltage(u8 ID)
{
	return readByte(ID, P_PRESENT_VOLTAGE);
}

// Read voltage in, timeout return -1
int SCSProtocol::ReadVoltage(u8 ID)
{
	return readByte(ID, P_PRESENT_VOLTAGE);
}

// Read 1 byte, timeout return -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;
	}
}

// Read instruction
// [servo ID, MemAddr, return nData, data nLen]
int SCSProtocol::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen)
{
	flushSCS();
	writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
	u8 bBuf[5];