/******************************************************************* * MAIN() *******************************************************************/ int main(void) { long lEEPROMRetStatus; uint16_t i=0; uint8_t halted_latch = 0; // Set the clocking to run at 80 MHz from the PLL. // (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't // write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we // could slow the clock down when writing to FLASH but then we need to find out how long // it takes for the clock to stabilize. This is on at the bottom of my list of things to do // for now) SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); // Initialize the device pinout. SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ); // Enable processor interrupts. IntMasterEnable(); // Setup the UART's my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // command_handler_init overwrites the baud rate. We still need to configure the pins though my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // Enable the command handler command_handler_init(); // We set the baud in here // Start the timers my_timer0_init(); my_timer1_init(); i2c_init(); motor_init(); qei_init(); gyro_init(); accel_init(); led_init(); //rc_radio_init(); //setupBluetooth(); // Initialize the EEPROM emulation region. lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE); if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14); #if 0 // If ever we wanted to write some parameters to FLASH without the HMI // we could do it here. SoftEEPROMWriteDouble(kP_ID, 10.00); SoftEEPROMWriteDouble(kI_ID, 10.00); SoftEEPROMWriteDouble(kD_ID, 10.00); SoftEEPROMWriteDouble(ANG_ID, 0.0); SoftEEPROMWriteDouble(COMPC_ID, 0.99); #endif kP = SoftEEPROMReadDouble(kP_ID); kI = SoftEEPROMReadDouble(kI_ID); kD = SoftEEPROMReadDouble(kD_ID); commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID); COMP_C = SoftEEPROMReadDouble(COMPC_ID); pid_init(kP, kI, kD, &pid_ang); motor_controller_init(20, 100, 10, &mot_left); motor_controller_init(20, 100, 10, &mot_right); //pid_init(0.0, 0.0, 0.0, &pid_pos_left); //pid_init(0.0, 0.0, 0.0, &pid_pos_right); //UART0Send("Hello World!\n", 13); // Tell the HMI what the initial parameters are. print_params(1); while(1) { delta_t = myTimerValueGet(); myTimerZero(); sum_delta_t += delta_t; // Read our sensors accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true); gyro_get_y_cal(&gyro_y, false); // Calculate the pitch angle with the accelerometer only R = sqrt(pow(accel_x, 2) + pow(accel_z, 2)); accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang; //accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0); gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t); // Kalman filter //filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t)); filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang); // Skip the rest of the process until the angle stabilizes if(i < 250) { i++; continue; } // Tell the HMI what's going on every 100ms if(sum_delta_t >= 1000) { print_update(1); print_debug(0); //print_control_surfaces(0); led_toggle(); //print_angle(); sum_delta_t = 0; } // See if the HMI has anything to say command_handler(); //continue; // If we are leaning more than +/- FALL_ANG deg off center it's hopeless. // Turn off the motors in hopes of some damage control if( abs(filtered_ang) > FALL_ANG ) { if(halted_latch) continue; stop_motors(); halted_latch = 1; continue; } halted_latch = 0; motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang); motor_left = motor_right = motor_val; drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain); } }
void prvSetupHardware( void ){ tBoolean found; long lEEPROMRetStatus; unsigned short data,data2; unsigned long uart_speed; /* If running on Rev A2 silicon, turn the LDO voltage up to 2.75V. This is a workaround to allow the PLL to operate reliably. */ if( DEVICE_IS_REVA2 ) { SysCtlLDOSet( SYSCTL_LDO_2_75V ); } /* Set the clocking to run from the PLL at 50 MHz */ SysCtlClockSet( SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ ); /* Enable Port F for Ethernet LEDs LED0 Bit 3 Output LED1 Bit 2 Output */ SysCtlPeripheralEnable( SYSCTL_PERIPH_GPIOF ); GPIODirModeSet( GPIO_PORTF_BASE, (GPIO_PIN_2 | GPIO_PIN_3), GPIO_DIR_MODE_HW ); GPIOPadConfigSet( GPIO_PORTF_BASE, (GPIO_PIN_2 | GPIO_PIN_3 ), GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD ); // // Enable the GPIO pin for the LED (PF0). Set the direction as output, and // enable the GPIO pin for digital function. // GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_0); // // Enable processor interrupts. // IntMasterEnable(); if(SoftEEPROMInit(0x1F000, 0x20000, 0x800) != 0) { LWIPDebug("SoftEEPROM initialisation failed."); } lEEPROMRetStatus = SoftEEPROMRead(UART0_SPEED_HIGH_ID, &data, &found); if(lEEPROMRetStatus == 0 && found) { SoftEEPROMRead(UART0_SPEED_LOW_ID, &data2, &found); uart_speed = (data << 16 & 0xFFFF0000) | (data2 & 0x0000FFFF); SoftEEPROMRead(UART0_CONFIG_ID, &data, &found); } else { uart_speed=115200; data = (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE); } uart_init(0, uart_speed, data); SysCtlPeripheralEnable(SYSCTL_PERIPH_WDOG0); IntPriorityGroupingSet(4); IntPrioritySet(INT_WATCHDOG,SET_SYSCALL_INTERRUPT_PRIORITY(5)); IntEnable(INT_WATCHDOG); // Enable the watchdog interrupt. WatchdogReloadSet(WATCHDOG0_BASE, SysCtlClockGet()); WatchdogResetEnable(WATCHDOG0_BASE); WatchdogEnable(WATCHDOG0_BASE); rtc_init(); modules_init(); }