/*********************************************************************
Main Program Loop
**********************************************************************/
int main()
{
/* Initializations */
debug_init(); /* This should be first. */
timer_init();
/* This should be before any GPIO activities. */
uint32 ret_val = bcm2835_init();
if ( ret_val == 0 )
{
DEBUG_MSG_ERROR("bcm2835_init() failed.");
}
pwm_init();
pump_init();
therm_init();
pid_init();
pump_start();
/* Take temperature as input from console. */
float setpoint;
printf("Set your desired temperature: ");
scanf("%f", &setpoint);
pid_update_temp_setpoint(setpoint);
pid_gain_params pid_gain;
pid_gain.k_p = 1;
pid_gain.k_d = 1;
pid_gain.k_i = 1;
pid_gain.k_windup = 1;
pid_set_gain(&pid_gain);
/* Main Program Loop */
while (1)
{
pwm_run();
therm_capture();
pid_loop();
}
pump_stop();
/* De-initializations */
pump_deinit();
pid_deinit();
pwm_deinit();
/* This should be after all GPIO activities. */
ret_val = bcm2835_close();
if ( ret_val == 0 )
{
DEBUG_MSG_ERROR("bcm2835_close() failed.");
}
timer_deinit();
debug_deinit(); /* This should be last. */
return 0;
}
/**@brief Function for initializing services that will be used by the application.
*/
static void services_init(void)
{
dis_init();
bas_init();
therm_init();
// Configure the Temp. Sensor Service
{
srv_TempSensor_init_t tss_init;
CLEAR(tss_init);
tss_init.support_notification= true;
// Here the sec level for the Battery Service can be changed/increased.
BLE_GAP_CONN_SEC_MODE_SET_OPEN(&tss_init.battery_level_char_attr_md.cccd_write_perm);
BLE_GAP_CONN_SEC_MODE_SET_OPEN(&tss_init.battery_level_char_attr_md.read_perm);
BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(&tss_init.battery_level_char_attr_md.write_perm);
srv_TempSensor_init(&m_tss, &tss_init);
}
}
/******************************* Main Program Code *************************/
int main(void)
{
// configure the microprocessor pins for the data lines
lcd_D7_ddr |= (1<<lcd_D7_bit); // 4 data lines - output
lcd_D6_ddr |= (1<<lcd_D6_bit);
lcd_D5_ddr |= (1<<lcd_D5_bit);
lcd_D4_ddr |= (1<<lcd_D4_bit);
// configure the microprocessor pins for the control lines
lcd_E_ddr |= (1<<lcd_E_bit); // E line - output
lcd_RS_ddr |= (1<<lcd_RS_bit); // RS line - output
// configure the microprocessor pins for the pushbutton
pushbutton_ddr &= (1<<pushbutton_bit);
pushbutton_port |= (1<<pushbutton_bit);
// initialize adc
ADMUX = ((1<<REFS0)|(1<<MUX2)|(1<<MUX0)); // Aref = Vcc, select ADC5
ADCSRA = ((1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<ADPS0)); // Prescaler div factor = 128
// initialize the LCD controller as determined by the defines (LCD instructions)
lcd_init_4d(); // initialize the LCD display for a 4-bit interface
// initialize TWI
i2c_init();
// initialize thermometer
therm_init();
// display the first line of information
lcd_write_string_4d(disp_time);
// set cursor to start of second line
lcd_write_instruction_4d(lcd_setCursor | lcd_lineTwo);
_delay_us(40); // 40 uS delay (min)
// Code for interfacing with the serial connection
char str[25];
int yy,mm,dd;
sei(); // Enable global interrupts
uart_init(); // Initialize the USART using baud rate 9600
uart_printstr(sdata); // Print a string from SRAM
uart_printstr(fdata); // Print a string from FLASH
getDate(&yy,&mm,&dd); // Get date from user
sprintf(str,"Date: %d/%d/%d\n",yy,mm,dd);
uart_printstr(str);
// endless loop
while(1)
{
uart_printstr(sdata); // Print a string from SRAM
uart_printstr(fdata);
// replace with check for button press function
if(bit_is_clear(pushbutton_pin,pushbutton_bit))
{
_delay_ms(100);
if(bit_is_clear(pushbutton_pin,pushbutton_bit))
mode_new = (mode + 1) % 3;
}
if(mode_new != mode)
{
// clear lcd
lcd_write_instruction_4d(lcd_clear);
// display the first line of information
// set cursor to start of first line
lcd_write_instruction_4d(lcd_setCursor | lcd_lineOne);
_delay_us(40); // 40 uS delay (min
lcd_write_string_4d(disp_time);
// set cursor to start of second line
lcd_write_instruction_4d(lcd_setCursor | lcd_lineTwo);
_delay_us(40); // 40 uS delay (min)
if (mode_new == 0)
{
voltage = read_ADC();
lcd_write_string_4d(disp_volt);
}
else if (mode_new == 1)
{
frequency = 10 * freq_cntr_get_frequency();
lcd_write_string_4d(disp_freq);
}
else
{
temp_calcTemp(); // test thermometer
lcd_write_string_4d(disp_temp);
}
}
mode = mode_new;
}
return 0;
}
int main()
{
led_init();
led_on();
console_init();
printf("===== APP ENTRY =====\r\n");
systime_init();
enc_init();
usb_init();
halls_init();
therm_init();
//enc_print_regs();
printf("entering blink loop...\r\n");
__enable_irq();
usb_tx(1, g_tx_buf, sizeof(g_tx_buf));
uint16_t raw_angle = 0, prev_raw_angle = 0;
float raw_vel = 0;
float filt_vel[3] = {0};
float filt_angle[3] = {0};
//float raw_vel = 0, filt_vel = 0, filt_angle = 0;
bool filter_init = false;
float unwrapped_raw = 0, prev_unwrapped_raw = 0;
uint32_t t = 0, t_last_led_blink = 0;
const float pos_gain[3] = { 0.9f, 0.99f, 0.999f };
const float vel_gain[3] = { 0.99f, 0.999f, 0.9999f };
int wraps = 0;
uint32_t t_last_therm_reading = 0;
g_therm_celsius = therm_celsius();
while (1)
{
if (SYSTIME - t_last_therm_reading > 1000)
{
g_therm_celsius = therm_celsius();
t_last_therm_reading = SYSTIME;
}
if (SYSTIME - t_last_led_blink > 100000)
{
t_last_led_blink = SYSTIME;
led_toggle();
/*
printf("\n\n");
printf("gintsts = 0x%08x\r\n", (unsigned)USB_OTG_FS->GINTSTS);
printf("dctl = 0x%08x\r\n", (unsigned)g_usbd_dbg->DCTL);
printf("dsts = 0x%08x\r\n", (unsigned)g_usbd_dbg->DSTS);
printf("dtxfsts1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DTXFSTS);
printf("diepctl1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPCTL);
printf("diepint1 = 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPINT);
printf("dieptsiz1= 0x%08x\r\n", (unsigned)USB_INEP(1)->DIEPTSIZ);
*/
}
raw_angle = enc_poll_angle();
t = SYSTIME;
if (filter_init)
{
int diff = raw_angle - prev_raw_angle;
if (diff > 8000)
wraps--;
else if (diff < -8000)
wraps++;
unwrapped_raw = (float)raw_angle + wraps * 16384;
// calculate raw_vel in ticks/usec for numerical stability
// TODO: use a better timebase, since we're polling @ 100 khz so there
// is extreme quantization on the microsecond clock
float dt_usecs = (float)(t - g_t_angle) * 1000000.0f;
if (dt_usecs < 1.0f)
dt_usecs = 1.0f;
// todo: this leads to bad numerical stability after lots of wraps
// need to re-work this crap
raw_vel = (unwrapped_raw - prev_unwrapped_raw) / dt_usecs;
for (int i = 0; i < 3; i++)
{
filt_angle[i] = pos_gain[i] * filt_angle[i] +
(1.0f - pos_gain[i]) * unwrapped_raw;
filt_vel[i] = vel_gain[i] * filt_vel[i] +
(1.0f - vel_gain[i]) * raw_vel * 1000000.0f;
}
}
else
{
filter_init = true;
for (int i = 0; i < 3; i++)
{
filt_angle[i] = raw_angle;
filt_vel[i] = 0;
}
}
prev_raw_angle = raw_angle;
prev_unwrapped_raw = unwrapped_raw;
__disable_irq();
g_t_angle = t;
g_raw_angle = raw_angle;
for (int i = 0; i < 3; i++)
{
g_angle[i] = filt_angle[i];
g_vel[i] = filt_vel[i]; // * 0.000001f; // convert to ticks / sec
}
g_num_samp++;
__enable_irq();
}
return 0;
}
