int main(void)
{
lb_init(&lb);
event_init();
motor_init();
uart_init(); // init USART
enc_init();
i2c_init();
adc_init();
kalman_init();
sei(); // enable interrupts
// Wait a second at startup
_delay_ms(1000);
// send initial string
printf_P(PSTR("Hello world!\n"));
imu_init();
for (;/*ever*/;)
{
// ADCSRA |= (1<<ADSC); // Set start conversion bit and wait for conversion to finish
// while(ADCSRA&(1<<ADSC));
// OCR1AL = ADCH; // Set ADC reading to timer 0 compare
if(event_pending())
{
event_action();
}
else // No pending operation, do low priority tasks
{
// dequeue receive buffer if any bytes waiting
while (uart_avail())
{
char c = uart_getc();
if (lb_append(&lb, c) == LB_BUFFER_FULL)
{
lb_init(&lb); // Clear line
printf_P(PSTR("\nMax line length exceeded\n"));
}
// Process command if line buffer is ready ...
if (lb_line_ready(&lb))
{
strcpy(cmd_string,lb_gets(&lb));
do_cmd(cmd_string);
lb_init(&lb);
}
}
}
// Process command if line buffer is terminated by a line feed or carriage return
}
return 0;
}
/** Main loop for MIDI. **/
void midi_link_main(void) {
if (d12_device_is_configured()) {
/** Receive data on UART if usb is configured. **/
while (uart_avail()) {
uint8_t c = uart_getc();
usb_midi_handle_rx_byte(c);
}
/* try to send outgoing data as soon as possible */
usb_midi_send_buf();
}
}
/** Main routine for midi-link. **/
int main(void) {
/** Disable watchdog. **/
wdt_disable();
/** setbits for the LEDs. **/
// DDRC |= _BV(5);
DDRC |= _BV(4);
sr165_init();
lcd_init();
lcd_line1();
lcd_puts("HELO");
for (;;) {
uint16_t sr = sr165_read16();
lcd_line1();
lcd_putnumber16(sr);
}
/** Initialize UART. **/
uart_init();
usb_midi_init();
/** Initialize D12 pins and stack. **/
d12_pins_init();
d12_init();
/** Enable interrupts. **/
sei();
uint8_t uart_configured = 0;
for (;;) {
if (d12_device_is_configured() && (uart_configured == 0)) {
/* discard data in uart buffer */
while (uart_avail()) {
uart_getc();
}
uart_configured = 1;
}
if (uart_configured < 100) {
_delay_ms(10);
uart_configured++;
} else {
/** Handle midi from UART and from USB. **/
midi_link_main();
}
/** Handle usb status. **/
d12_main();
/* check ep2 again because of d12 bug */
uint8_t status;
do {
d12_read_cmd(D12_CMD_SELECT_EP + D12_MIDI_EP_OUT, &status, 1);
if ((status & 1) && d12_device_is_configured() && (uart_configured > 100)) {
handle_midi_ep_out();
}
} while (status & 1);
}
}
void ui_loop(void) {
_ui_init_lcd();
/*alarm_set(1000);*/
char obd_buf[64];
uint8_t obd_idx = 0;
bool do_cmd = true;
for ( ; ; ) {
int c;
/*bool do_cmd = false;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
if (timer0_count >= 1000) {
do_cmd = true;
timer0_count -= 1000;
}
}*/
if (do_cmd) {
// get throttle position
fputs_P(PSTR("0111\n"), stn1110);
obd_idx = 0;
do_cmd = false;
}
/* repeatedly receive and transmit data until neither are available */
bool got_data;
do {
got_data = false;
/* tx */
/*if (uart_avail(UART_PC) && (c = fgetc(stdin)) != EOF) {
fputc(c, stn1110);
if (!tx) {
tx = true;
rx = false;
lcd_clear();
lcd_goto_xy(0, 0);
fputs_P(PSTR(">>\r\n>>\r\n<<\r\n<<"), lcd);
lcd_goto_xy(3, 0);
}
if (c != '\n') {
fputc(c, lcd);
}
got_data = true;
}*/
/* rx */
if (uart_avail(UART_STN1110) && (c = fgetc(stn1110)) != EOF) {
fputc(c, stdout);
if (c != '\n') {
obd_buf[obd_idx++] = c;
} else {
obd_buf[obd_idx] = '\0';
/*// REMOVE ME
strcpy_P(obd_buf, PSTR("10 41 e8"));*/
lcd_clear();
// TODO: just overwrite
/*static uint8_t counter = 0;*/
uint8_t discard, accel;
if (sscanf_P(obd_buf, PSTR("%hhx %hhx %hhx"),
&discard, &discard, &accel)== 3) {
/*accel = counter++;*/
int16_t accel_i = (int16_t)accel - 40;
accel_i *= 100;
accel_i /= (205 - 40);
int16_t accel_b = (accel_i * 4) / 5;
if (accel_b < 0) accel_b = 0;
if (accel_b > 100) accel_b = 100;
fprintf_P(lcd, PSTR("%3d"), accel_i);
fputc(' ', lcd);
while (accel_b >= 5) {
lcd_write(0xff);
accel_b -= 5;
}
if (accel_b > 0) {
lcd_write(accel_b - 1);
}
} else {
fprintf_P(lcd, PSTR("error:\r\n%s"), obd_buf);
}
do_cmd = true;
/*_delay_ms(100);*/
}
got_data = true;
}
} while (got_data);
// TODO: go to sleep instead
_delay_us(10);
}
}
int main(void)
{
// INITIALISATIONS
float x = 0.0;
float y = 0.0;
float theta = 0.0;
float vel = 0.0;
float velref = 0.0;
float Mvel = 0.0;
char* word_array[MAX_CMDS];
int no_of_words = 0;
int error = 1;
DDRC |= 1 << 5; // PortC.5 as output
lb_init(&lb); // init line buffer lb
uart_init(); // init USART
enc_init(); // init Encoder
ctrl_init(); // init Controller
motor_init(); // init Motor
sei(); // enable interrupts
printf_P(PSTR("Sup Bitches\nThis is Command\n"));
for (;/*ever*/;)
{
while (uart_avail())
{
char c = uart_getc(); //gets character from circular buffer
if (lb_append(&lb, c) == LB_BUFFER_FULL) // Add character "c" to line buffer, report status(putc) and handle special characters(append)
{
lb_init(&lb); // Clear line buffer, discards input
printf_P(PSTR("\nMax line length exceeded\n"));
}
}
error = 1;
// Process command if line buffer is terminated by a line feed or carriage return
if (lb_line_ready(&lb)) //if not empty and has null terminator
{
for (int j = 0; j < NUM_CMDS; j++) //re-setting word_array to zero
{
word_array[j] = 0;
}
no_of_words = string_parser( lb_gets(&lb), word_array); // gets serial, puts into word_array
for (int i=0; cmd_table[i].cmd != NULL; ++i) //
{
if( !strcmp(word_array[0], cmd_table[i].cmd))
{
error = 0;
cmd_table[i].func(no_of_words, word_array);
}
}
lb_init(&lb);
// Error checking
if(!no_of_words)
{
printf_P(PSTR("No Command Entered\n"));
}
if(error)
{
printf_P(PSTR("Invalid Command\n"));
}
/* // Note: The following is a terrible way to process strings from the user
// See recommendations section of the lab guide for a better way to
// handle commands with arguments, which scales well to a large
// number of commands.
if (!strncmp_P(lb_gets(&lb), PSTR("help"), 4))
{
printf_P(PSTR(
"MCHA3000 RS232 lab help.\n"
"Replace these lines with your own help instructions.\n"));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("x="), 2)) // takes 'x' co-ordinate
{
x = atof(lb_gets_at(&lb, 2));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("x?"), 2)) // prints 'x' co-ordinate to serial
{
printf_P(PSTR("x is %f\n"), x);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("y="), 2)) // takes 'y' co-ordinate
{
y = atof(lb_gets_at(&lb, 2));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("xy?"), 3)) // prints 'x'*'y' to serial
{
printf_P(PSTR("%f\n"), x*y);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("theta="), 6)) // HIL: takes 'theta'
{
theta = atof(lb_gets_at(&lb, 6));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("vel="), 4)) // HIL: takes 'vel'
{
vel = atof(lb_gets_at(&lb, 4));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("velref="), 7)) // HIL: takes 'velref'
{
velref = atof(lb_gets_at(&lb, 7));
}
else if (!strncmp_P(lb_gets(&lb), PSTR("ctrl?"), 5)) // HIL: initialises feedback loop for cascade controller
{ // and prints control action to serial
float outer_loop = velocity_controller(velref - vel);
float inner_loop = angle_controller(outer_loop - theta);
printf_P(PSTR("%g\n"), inner_loop);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("ecount?"), 7)) // prints enc_count
{
printf_P(PSTR("Encoder1 Count = %d\n"), enc_read1());
printf_P(PSTR("Encoder2 Count = %d\n"), enc_read2());
}
else if (!strncmp_P(lb_gets(&lb), PSTR("ereset"), 6)) // Resets enc_count then prints count
{
enc_reset();
printf_P(PSTR("Encoder1 Count = %d\n"), enc_read1());
printf_P(PSTR("Encoder2 Count = %d\n"), enc_read2());
}
else if (!strncmp_P(lb_gets(&lb), PSTR("mvel="), 5)) // Motor On/Off
{
Mvel=atof(lb_gets_at(&lb, 5));
motor_vel(Mvel);
printf_P(PSTR("Motor Velocity = %f\n"), Mvel);
}
else if (!strncmp_P(lb_gets(&lb), PSTR("I"), 1)) // Motor Current
{
printf_P(PSTR("Motor Current = %f\n"), motor_current());
}
else // WARNING: Unknown command
{
printf_P(PSTR("Unknown command: \"%s\"\n"), lb_gets(&lb));
}
lb_init(&lb); // Reset line buffer
*/
}
}
return 0;
}
