// -------------------------------------------
// called from main()
void pidSetup()
{
int i;
for(i = 0; i < NUM_PIDS; i++){
initPIDObjPos( &(pidObjs[i]), DEFAULT_KP, DEFAULT_KI, DEFAULT_KD, DEFAULT_KAW, DEFAULT_FF);
}
initPIDVelProfile();
SetupTimer1(); // main interrupt used for leg motor PID
// returns pointer to queue with 8 move entries
moveq = mqInit(8);
idleMove = malloc(sizeof(moveCmdStruct));
idleMove->inputL = 0;
idleMove->inputR = 0;
idleMove->duration = 0;
currentMove = idleMove;
manualMove = malloc(sizeof(moveCmdStruct));
manualMove->inputL = 0;
manualMove->inputR = 0;
manualMove->duration = 0;
lastMoveTime = 0;
// initialize PID structures before starting Timer1
pidSetInput(0,0,0);
pidSetInput(1,0,0);
EnableIntT1; // turn on pid interrupts
calibBatteryOffset(100);
}
int main(int argc, char** argv)
{
SetupTimer1();
INTEnableInterrupts();
while (1)
{
}
return (EXIT_SUCCESS);
}
void setup()
{
pinMode(throttle_pin, INPUT);
pinMode(leftright_pin, INPUT);
pinMode(forwardback_pin, INPUT);
pinMode(rudder_pin, INPUT);
pinMode(acceldata_pin, INPUT);
pinMode(accelpower_pin, OUTPUT);
pinMode(led_pin, OUTPUT);
pinMode(motor1_pin, OUTPUT);
pinMode(motor2_pin, OUTPUT);
digitalWrite(led_pin, HIGH); //turn on signal LED before timers so it comes on immediately
digitalWrite(accelpower_pin, HIGH); //turn on power for accel (if ( accel is connected to chip for power)
//setup interrupts for RX pins
PCattachinterrupt(throttle_pin, throttle_change);
PCattachinterrupt(leftright_pin, leftright_change);
PCattachinterrupt(forwardback_pin, forwardback_change);
PCattachinterrupt(rudder_pin, rudder_change);
SetupTimer1(); //fire up timer1 (2 bytes) - accessed via TCNT1 variable
configmode = 0; //we're not in configmode to start
motors_off(); //make sure those motors are off...
throttle = 0; //make sure throttle is off at boot
//flash LED on boot
for (x = 1; x <= 20; x++)
{
digitalWrite(led_pin, !digitalRead(led_pin));
delay(20);
}
/*
if ( Eprom_check = 555 )
{ //load config data from eprom if ( eprom_check was set to 555
tracking_comp = tracking_comp_store;
if ( load_heading == 1 ) led_adjust = led_adjust_store;
if ( load_heading == 1 ) led_adjust_backup = led_adjust_store;
base_accel = base_accel_store;
}
*/
}
//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;
}
}
int main ( void )
{
fun_queue = queueInit(FUN_Q_LEN);
rx_pay_queue = pqInit(12); //replace 12 with a #define const later
test_function tf;
/* Initialization */
SetupClock();
SwitchClocks();
SetupPorts();
SetupInterrupts();
SetupI2C();
SetupADC();
SetupTimer1();
SetupPWM();
SetupTimer2();
gyroSetup();
xlSetup();
dfmemSetup();
WordVal pan_id = {RADIO_PAN_ID};
WordVal src_addr = {RADIO_SRC_ADDR};
WordVal dest_addr = {RADIO_DEST_ADDR};
radioInit(src_addr, pan_id, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetDestAddr(dest_addr);
radioSetChannel(RADIO_MY_CHAN);
char j;
for(j=0; j<3; j++){
LED_2 = ON;
delay_ms(500);
LED_2 = OFF;
delay_ms(500);
}
LED_2 = ON;
EnableIntT2;
while(1){
while(!queueIsEmpty(fun_queue))
{
rx_payload = pqPop(rx_pay_queue);
tf = (test_function)queuePop(fun_queue);
(*tf)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload));
payDelete(rx_payload);
}
}
return 0;
}
//--------------------------------------------------------------------------------
//
// Main program loop.
//
void main()
{
__disable_interrupt();
_pulseDataAddress = (char *) EEPROM_PULSE_DATA;
_numberOfPulses = *_pulseDataAddress++;
SetupPorts();
SetupUART();
SetupTimer2();
SetupTimer1();
__enable_interrupt();
while (1)
{
__wait_for_interrupt();
}
}
//sets up all the pins and interrupts
void setup(void)
{
adc_init(); //init the ADC...
set_throttle_pin_as_input();
set_leftright_pin_as_input();
set_forwardback_pin_as_input();
set_accel_data_pin_as_input();
set_accelpower_pin_as_output();
set_accelpower_pin_on(); //turn on power for accel (accel is connected to chip for power)
set_led_pin_as_output();
set_motor1_pin_as_output();
set_motor2_pin_as_output();
set_led_on(); //turn on signal LED before timers so it comes on immediately
//enable pin change interrupt - any changes on PORTB trigger interrupt
PCMSK0 = 0xFF;
PCICR = 1<<PCIE0;
SetupTimer1(); //fire up timer1 (2 bytes) - accessed via TCNT1 variable
motors_off(); //make sure those motors are off...
//flash LED on boot
for (x = 1; x <= 10; x++)
{
toggle_led();
_delay_ms(200);
}
throttle = 0; //make sure throttle is off at boot
}
// -------------------------------------------
// called from main()
void pidSetup()
{
int i;
for(i = 0; i < NUM_PIDS; i++){
initPIDObjPos( &(pidObjs[i]), DEFAULT_KP, DEFAULT_KI, DEFAULT_KD, DEFAULT_KAW, DEFAULT_FF);
}
initPIDVelProfile();
SetupTimer1(); // main interrupt used for leg motor PID
lastMoveTime = 0;
// initialize PID structures before starting Timer1
pidSetInput(0,0);
pidSetInput(1,0);
EnableIntT1; // turn on pid interrupts
calibBatteryOffset(100); //???This is broken for 2.5
}
void legCtrlSetup() {
int i;
//Setup for PID controllers
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
#ifdef PID_HARDWARE
//THe user is REQUIRED to set up these pointers before initializing
//the object, because the arrays are local to this module.
motor_pidObjs[i].dspPID.abcCoefficients =
motor_abcCoeffs[i];
motor_pidObjs[i].dspPID.controlHistory =
motor_controlHists[i];
#endif
pidInitPIDObj(&(motor_pidObjs[i]), LEG_DEFAULT_KP, LEG_DEFAULT_KI,
LEG_DEFAULT_KD, LEG_DEFAULT_KAW, LEG_DEFAULT_KFF);
//Set up max's and saturation values
motor_pidObjs[i].satValPos = SATTHROT;
motor_pidObjs[i].satValNeg = 0;
motor_pidObjs[i].maxVal = FULLTHROT;
motor_pidObjs[i].minVal = 0;
}
//Set which PWM output each PID Object will correspond to
legCtrlOutputChannels[0] = MC_CHANNEL_PWM1;
legCtrlOutputChannels[1] = MC_CHANNEL_PWM2;
SetupTimer1(); // Timer 1 @ 1 Khz
int retval;
retval = sysServiceInstallT1(legCtrlServiceRoutine);
//ADC_OffsetL = 1; //prevent divide by zero errors
//ADC_OffsetR = 1;
//Move Queue setup and initialization
moveq = mqInit(32);
idleMove = malloc(sizeof (moveCmdStruct));
idleMove->inputL = 0;
idleMove->inputR = 0;
idleMove->duration = 0;
idleMove->type = MOVE_SEG_IDLE;
idleMove->params[0] = 0;
idleMove->params[1] = 0;
idleMove->params[2] = 0;
currentMove = idleMove;
currentMoveStart = 0;
moveExpire = 0;
blinkCtr = 0;
inMotion = 0;
//Ensure controllers are reset to zero and turned off
//External function used here since it will zero out the state
pidSetInput(&(motor_pidObjs[0]), 0);
pidSetInput(&(motor_pidObjs[1]), 0);
motor_pidObjs[0].onoff = PID_OFF;
motor_pidObjs[1].onoff = PID_OFF;
//Set up filters and histories
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
bemfLast[i] = 0;
bemfHist[i][0] = 0;
bemfHist[i][1] = 0;
bemfHist[i][2] = 0;
}
}
