int main(void) {
unsigned char ret;
init_io();
cli();
check_wdt();
setup_wdt();
sei(); // Enables interrupts
// Enable Sleep Mode for Power Down
set_sleep_mode(SLEEP_MODE_PWR_DOWN); // Set Sleep Mode: Power Down
sleep_enable(); // Enable Sleep Mode
for(;;) { // Event Loop
if(tick_flag) {
tick_flag = 0;
sleep_disable();
set_sleep_mode(SLEEP_MODE_PWR_DOWN);
sleep_enable();
sleep_mode();
}
}
}
int main()
{
int Status;
init_axi_uart();
init_axi_gpio();
init_spi();
init_pendulum_plant();
init_fifo_queues();
init_wdt();
assert_trigger(PRODUCTION);
set_set_point(0);
Status = init_interrupt_system();
if (Status != XST_SUCCESS) {
xil_printf("Failed intr setup\r\n");
return XST_FAILURE;
}
start_ioi();
static bool startTAIGA = false;
static bool assertTrigger = false;
while(true){
if(!startTAIGA && read_btn(BTN0)){
startTAIGA = true;
start_wdt();
}
assert_trigger(startTAIGA & (check_wdt() | assertTrigger));
supervisor_update_set_point();
if(check_control_cycle()){
set_debug(DEBUG2, true);
reset_control_cycle();
if((get_alphaR() >= 0 ? get_alphaR():-get_alphaR()) < (20.*pi/180))
calculateKalmanControlSignal(get_plant_state_instance());
set_debug(DEBUG2, false);
supervisor_send_state_vector(get_plant_state_instance()->xhat);
set_debug(DEBUG2, true);
if(startTAIGA){
reset_wdt();
set_debug(DEBUG4, true);
if((get_alphaR() >= 0 ? get_alphaR():-get_alphaR()) < (20.*pi/180)){
if(trivial_trigger_mechanism(get_plant_state_instance())){
assertTrigger = true;
set_debug(DEBUG3, true);
}
else set_debug(DEBUG3, false);
}
set_debug(DEBUG4, false);
}
supervisor_send_tail(get_plant_state_instance()->u, startTAIGA, assertTrigger, check_wdt());
set_debug(DEBUG2, false);
}
}
return XST_SUCCESS;
}