void cmdSetPIDGains(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
int Kp, Ki, Kd, Kaw, ff;
int idx = 0;
Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
pidSetGains(0,Kp,Ki,Kd,Kaw, ff);
Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
pidSetGains(1,Kp,Ki,Kd,Kaw, ff);
//Send confirmation packet
radioConfirmationPacket(RADIO_DEST_ADDR, CMD_SET_PID_GAINS, status, 20, frame);
return; //success
}
unsigned char cmdSetPIDGains(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
int Kp, Ki, Kd, Kaw, ff;
int idx = 0;
Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
pidSetGains(0,Kp,Ki,Kd,Kaw, ff);
Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
pidSetGains(1,Kp,Ki,Kd,Kaw, ff);
radioSendData(RADIO_DST_ADDR, status, CMD_SET_PID_GAINS, 20, frame, 0); //TODO: Robot should respond to source of query, not hardcoded address
//Send confirmation packet
// WARNING: Will fail at high data throughput
//radioConfirmationPacket(RADIO_DEST_ADDR, CMD_SET_PID_GAINS, status, 20, frame);
return 1; //success
}
void steeringSetGains(int Kp, int Ki, int Kd, int Kaw, int ff) {
pidSetGains(&steeringPID, Kp, Ki, Kd, Kaw, ff);
}
void legCtrlSetGains(unsigned int num, int Kp, int Ki, int Kd, int Kaw, int ff){
pidSetGains(&(motor_pidObjs[num]), Kp, Ki, Kd, Kaw, ff);
}
