Feetech Write Speed (mode 1) functions

SMS_STS.cpp

int SMS_STS::RegWriteSpe(u8 ID, s16 Speed, u8 ACC)
{
	if(Speed<0){
		Speed = -Speed;
		Speed |= (1<<15);
	}
	u8 bBuf[2];
	bBuf[0] = ACC;
	regWrite(ID, SMS_STS_ACC, bBuf, 1);
	Host2SCS(bBuf+0, bBuf+1, Speed);
	
	return regWrite(ID, SMS_STS_GOAL_SPEED_L, bBuf, 2);
}

void SMS_STS::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[])
{
    u8 offbuf[IDN][2];
    for(u8 i = 0; i<IDN; i++){
		if(Speed[i]<0){
			Speed[i] = -Speed[i];
			Speed[i] |= (1<<15);
		}
        u8 bBuf[2];
		if(ACC[i]){
			bBuf[0] = ACC[i];
		}else{
			bBuf[0] = 0;
		}
        genWrite(ID[i], SMS_STS_ACC, bBuf, 1);
        Host2SCS(bBuf+0, bBuf+1, Speed[i]);
        memcpy(offbuf[i], bBuf, 2);
    }
    syncWrite(ID, IDN, SMS_STS_GOAL_SPEED_L, (u8*)offbuf, 2);
}