unsigned char cmdPIDStartMotors(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
pidObjs[0].timeFlag = 0;
pidObjs[1].timeFlag = 0;
pidSetInput(0, 0);
pidObjs[0].p_input = pidObjs[0].p_state;
pidOn(0);
pidSetInput(1, 0);
pidObjs[1].p_input = pidObjs[1].p_state;
pidOn(1);
return 1;
}
void cmdSetThrustClosedLoop(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame)
{ int thrust1 = frame[0] + (frame[1] << 8);
unsigned int run_time_ms1 = frame[2] + (frame[3] << 8);
int thrust2 = frame[4] + (frame[5] << 8);
unsigned int run_time_ms2 = frame[6] + (frame[7] << 8);
//currentMove = manualMove;
pidSetInput(0 ,thrust1, run_time_ms1);
pidOn(0);
pidSetInput(1 ,thrust2, run_time_ms2);
pidOn(1);
}
// ==== Flash/Experiment Commands ==============================================================================
// =============================================================================================================
unsigned char cmdStartTimedRun(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
unsigned int run_time = frame[0] + (frame[1] << 8);
int i;
for (i = 0; i < NUM_PIDS; i++){
pidObjs[i].timeFlag = 1;
pidSetInput(i, 0);
checkSwapBuff(i);
pidOn(i);
}
pidObjs[0].mode = 0;
pidStartTimedTrial(run_time);
return 1;
}
