//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;
}
}
/*-----------------------------------------------------------------------------
* 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 cmdHallPIDSetInput(MacPacket packet) {
Payload pld;
unsigned int *frame;
pld = macGetPayload(packet);
frame = (unsigned int*) payGetData(pld);
hallPIDSetInput(frame[0],frame[1]);
}
