Esempio n. 1
0
//Main hall effect sensor setup, called from main()
void hallSetup() {
    //Init of PID controller objects
    int i;
    for (i = 0; i < NUM_HALL_PIDS; i++) {
        hallInitPIDObjPos(&(hallPIDObjs[i]), DEFAULT_HALL_KP, DEFAULT_HALL_KI,
                DEFAULT_HALL_KD, DEFAULT_HALL_KAW, DEFAULT_HALL_FF);
        hallPIDObjs[i].minVal = 0;
        hallPIDObjs[i].satValNeg = 0;
        hallPIDObjs[i].maxVal = FULLTHROT;
        hallPIDObjs[i].satValPos = SATTHROT;
    }

    // Controller to PWM channel correspondance
    hallOutputChannels[0] = MC_CHANNEL_PWM1;
    hallOutputChannels[1] = MC_CHANNEL_PWM2;

    //Init for velocity profile objects
    hallInitPIDVelProfile();

    //System setup
    SetupTimer1();
    SetupTimer2(); // used for leg hall effect sensors
    SetupInputCapture(); // setup input capture for hall effect sensors
    int retval;
    retval = sysServiceInstallT1(hallServiceRoutine);

    // returns pointer to queue with 8 move entries
    hallMoveq = mqInit(8);
    hallIdleMove = malloc(sizeof (moveCmdStruct));
    hallIdleMove->inputL = 0;
    hallIdleMove->inputR = 0;
    hallIdleMove->duration = 0;
    hallCurrentMove = hallIdleMove;

    hallManualMove = malloc(sizeof (moveCmdStruct));
    hallManualMove->inputL = 0;
    hallManualMove->inputR = 0;
    hallManualMove->duration = 0;

    lastMoveTime = 0;
    //  initialize PID structures before starting Timer1
    hallPIDSetInput(0, 0, 0);
    hallPIDSetInput(1, 0, 0);

    for (i = 0; i < NUM_HALL_PIDS; i++) {
        hallbemfLast[i] = 0;
        hallbemfHist[i][0] = 0;
        hallbemfHist[i][1] = 0;
        hallbemfHist[i][2] = 0;
    }
}
Esempio n. 2
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
}
Esempio n. 3
0
static void cmdHallPIDSetInput(MacPacket packet) {
    Payload pld;
    unsigned int *frame;

    pld = macGetPayload(packet);
    frame = (unsigned int*) payGetData(pld);
    hallPIDSetInput(frame[0],frame[1]);
}