Feetech Write PWM (Mode 2) functions

SMS_STS.cpp

int SMS_STS::RegWritePwm(u8 ID, s16 Pwm)
{
    if(Pwm<0){
        Pwm = -Pwm;
        Pwm |= (1<<10);
    }
    u8 bBuf[2];
    Host2SCS(bBuf+0, bBuf+1, Pwm);
    
    return regWrite(ID, SMS_STS_GOAL_TIME_L, bBuf, 2);
}

void SMS_STS::SyncWritePwm(u8 ID[], u8 IDN, s16 Pwm[])
{
    u8 offbuf[IDN][2];
    for(u8 i = 0; i<IDN; i++){
		if(Pwm[i]<0){
			Pwm[i] = -Pwm[i];
			Pwm[i] |= (1<<10);
		}
        u8 bBuf[2];
        Host2SCS(bBuf+0, bBuf+1, Pwm[i]);
        memcpy(offbuf[i], bBuf, 2);
    }
    syncWrite(ID, IDN, SMS_STS_GOAL_TIME_L, (u8*)offbuf, 2);
}