int main(void) {
BOARD_Init();
printf("board_inited\n");
init_MDB();
AHRS_init();
printf("ahrs inited\n");
// timer_init();
//while(1);
int i = 700;
while (i >= -700) {
while (IsReceiveEmpty());
printf("looped\n");
printf("current angle: %f\n",get_AHRS_angle());
printf("current speed: %f\n", get_AHRS_rate());
waitabit(1000);
motor_command(i);
printf("sent command %d\n\n", i);
GetChar();
i = i - 100;
}
// while (1){
// while (IsReceiveEmpty());
// printf("looped\n");
// waitabit(1000);
// printf("current speed: %d\n", motor_command(10000));
// GetChar();
// }
}
/*********************************
main entry point
*********************************/
int main ( void )
{
// Init the basic hardware
InitCnsts();
InitHardware();
// Start the main 1Khz timer and PWM timer
InitTimer1();
InitTimer2();
InitPWM();
// Initialize A2D,
InitA2D();
UART1_Init(XBEE_SPEED); // for communication and control signals
UART2_Init(LOGGING_RC_SPEED); // for spektrum RC satellite receiver
// Wait for a bit before doing rate gyro bias calibration
// TODO: test this length of wait
uint16_t i=0;for(i=0;i<60000;i++){Nop();}
// turn on the leds until the bias calibration is complete
led_on(LED_RED);
led_on(LED_GREEN);
// Initialize the AHRS
AHRS_init();
// Initialize the Controller variables
Controller_Init();
// MAIN CONTROL LOOP: Loop forever
while (1)
{
// Gyro propagation
if(loop.GyroProp){
loop.GyroProp = 0;
// Call gyro propagation
AHRS_GyroProp();
}
// Attitude control
if(loop.AttCtl){
loop.AttCtl = 0;
// Call attitude control
Controller_Update();
}
// Accelerometer correction
if( loop.ReadAccMag ){
loop.ReadAccMag = 0;
AHRS_AccMagCorrect( );
}
// Send data over modem - runs at ~20Hz
if(loop.SendSerial){
loop.SendSerial = 0;
// Send debug packet
UART1_SendAHRSpacket();
}
// Process Spektrum RC data
if(loop.ProcessSpektrum){
loop.ProcessSpektrum = 0;
UART2_ProcessSpektrumData();
}
// Read data from UART RX buffers - 500 Hz
if(loop.ReadSerial){
loop.ReadSerial = 0;
// Read serial data
//UART2_FlushRX_Spektrum();
}
// Toggle Red LED at 1Hz
if(loop.ToggleLED){
loop.ToggleLED = 0;
// Toggle LED
led_toggle(LED_RED);
}
} // End while(1)
}