Beispiel #1
0
/*-----------------------------------------------------------------------------
 *         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
}
Beispiel #2
0
static void cmdHallPIDOn(MacPacket packet) {
    hallPIDOn();
}