// ------------------------------------------- // 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; } }