/*-----------------------------------------------------------------------------
* User function
-----------------------------------------------------------------------------*/
static void cmdSetThrustOpenLoop(unsigned char status, unsigned char length, unsigned char *frame) {
//Unpack unsigned char* frame into structured values
//_args_cmdSetThrustOpenLoop* argsPtr = (_args_cmdSetThrustOpenLoop*) (frame);
PKT_UNPACK(_args_cmdSetThrustOpenLoop, argsPtr, frame);
//set motor duty cycles
//PDC1 = argsPtr->dc1;
//PDC2 = argsPtr->dc1;
#define ROBOT2
#ifdef ROBOT2
if (argsPtr->dc1 < 0) {
mcSetDutyCycle(MC_CHANNEL_PWM1, (-1.0-(argsPtr->dc1))/4.0);
#else
if(argsPtr->dc1 >= 0) {
mcSetDutyCycle(MC_CHANNEL_PWM1, (argsPtr->dc1)/4.0);
#endif
mcSetDutyCycle(MC_CHANNEL_PWM4, (argsPtr->dc2)/4.0);
}
}
static void cmdSetThrustClosedLoop(unsigned char status, unsigned char length, unsigned char *frame) {
#ifdef HALL_SENSORS
PKT_UNPACK(_args_cmdSetThrustClosedLoop, argsPtr, frame);
hallPIDSetInput(0 , argsPtr->chan1, argsPtr->runtime1);
hallPIDOn(0);
hallPIDSetInput(1 , argsPtr->chan1, argsPtr->runtime2);
hallPIDOn(1);
#else
//_args_cmdSetThrustClosedLoop* argsPtr =
// (_args_cmdSetThrustClosedLoop*) (frame);
PKT_UNPACK(_args_cmdSetThrustClosedLoop, argsPtr, frame);
legCtrlSetInput(LEG_CTRL_LEFT, argsPtr->chan1);
legCtrlOnOff(LEG_CTRL_LEFT, PID_ON); //Motor PID #1 -> ON
legCtrlSetInput(LEG_CTRL_RIGHT, argsPtr->chan2);
legCtrlOnOff(LEG_CTRL_RIGHT, PID_ON); //Motor PID #2 -> ON
//This is now obsolete
//unsigned char temp[2];
//*(unsigned int*)temp = 1000;
//if (argsPtr->telem_samples > 0) {
// cmdGetPIDTelemetry(0, 2, (unsigned char*) (&telem_samples));
//}
#endif
}
static void cmdHallPIDOn(MacPacket packet) {
hallPIDOn();
}
