motor_test.c

   1 #include <stdbool.h>
   2 #include <stdio.h>
   3
   4 #include "brmpuk.h"
   5 #include <avr/io.h>
   6 #include <avr/interrupt.h>
   7 #include <util/delay.h>
   8
   9 #include "lcd.h"
  10 #include "sabertooth.h"
  11 #include "serial.h"
  12
  13 int main (void)
  14 {
  15         /* Control button at PC7. */
  16         DDRC &= ~(_BV(PC7)|_BV(PC6)|_BV(PC5)|_BV(PC4)|_BV(PC3));
  17         // pull up:
  18         PORTC |= (_BV(PC7)|_BV(PC6)|_BV(PC5)|_BV(PC4)|_BV(PC3));
  19
  20         int bstate = 0;
  21
  22         motor_init();
  23         lcd_init();
  24         sei();
  25
  26         lcd_text("brm brm");
  27
  28         signed char m1 = 0, m2 = 0;
  29
  30         while (1) {
  31                 bool update = false;
  32
  33                 if (bstate != PINC) {
  34                         /* Button state changed. */
  35                         serial_send(bstate);
  36                         bstate = PINC;
  37                 }
  38
  39                 if (serial_waiting()) {
  40                         m1 = serial_recv();
  41                         m2 = serial_recv();
  42                         update = true;
  43                 }
  44
  45                 /* Buttons override serial. */
  46
  47                 if (!(PINC & _BV(PC7))) {
  48                         motor_stop();
  49
  50                 } else if (!(PINC & _BV(PC6))) {
  51                         if (m1 > -63) m1--;
  52                         update = true;
  53
  54                 } else if (!(PINC & _BV(PC5))) {
  55                         if (m1 < 63) m1++;
  56                         update = true;
  57
  58                 } else if (!(PINC & _BV(PC4))) {
  59                         if (m2 > -63) m2--;
  60                         update = true;
  61
  62                 } else if (!(PINC & _BV(PC3))) {
  63                         if (m2 < 63) m2++;
  64                         update = true;
  65                 }
  66
  67                 if (update) {
  68                         motor_send(m1, m2);
  69                         _delay_ms(10);
  70
  71                         char x[40]; snprintf(x, 40, "brm:%d-%d", m1, m2);
  72                         lcd_text(x);
  73                 }
  74         }
  75
  76         return 0;
  77 }