src/edu/berkeley/slipway/slipway_drone.c

   1 //\r
   2 // YOU MUST COMPILE THIS WITH -O3 OR THE AVR WILL NOT BE ABLE TO KEEP UP!!!!\r
   3 //\r
   4 \r
   5 //#define F_CPU 3960000\r
   6 #define F_CPU 12000000\r
   7 \r
   8 #if !defined(__AVR_AT94K__)\r
   9 #error you forgot to put -mmcu=at94k on the command line\r
  10 #endif\r
  11 \r
  12 #include <avr/wdt.h>\r
  13 #include <util/delay.h>\r
  14 #include <avr/io.h>\r
  15 #include <avr/interrupt.h>\r
  16 \r
  17 int err = 0;\r
  18 \r
  19 void initUART0(unsigned int baudRate, unsigned int doubleRate) {\r
  20   UBRRHI  = (((baudRate) >> 8) & 0x000F); \r
  21   UBRR0   = ((baudRate) & 0x00FF); \r
  22   UCSR0B |= ((1 << RXEN0) | (1 << TXEN0) | (1 << RXCIE0)); \r
  23 \r
  24   if (doubleRate)\r
  25     UCSR0A |=  (1 << U2X0);\r
  26   else\r
  27     UCSR0A &= ~(1 << U2X0);\r
  28 }\r
  29 \r
  30 #define BUFSIZE (1024)\r
  31 \r
  32 inline void portd(int bit, int on) {\r
  33   /*\r
  34   if (on) {\r
  35     PORTD &= ~(1<<bit);\r
  36   } else {\r
  37     PORTD |= (1<<bit);\r
  38   }\r
  39   */\r
  40 }\r
  41 \r
  42 long int numread = 0;\r
  43 inline void cts(int c) {\r
  44   numread++;\r
  45   if (c /*&& numread < 10000*/) {\r
  46     PORTE |= (1 << 4);\r
  47     PORTE |= (1 << 2);\r
  48     portd(0, 0);\r
  49   } else {\r
  50     PORTE &= ~(1 << 4);\r
  51     PORTE &= ~(1 << 2);\r
  52     portd(0, 1);\r
  53   }\r
  54 }\r
  55 \r
  56 \r
  57 static volatile int sending = 0;\r
  58 static volatile int32_t interrupt_count = 0;\r
  59 \r
  60 // RECV //////////////////////////////////////////////////////////////////////////////\r
  61 \r
  62 char read_buf[BUFSIZE];\r
  63 volatile int read_buf_head;\r
  64 volatile int read_buf_tail;\r
  65 char write_buf[BUFSIZE];\r
  66 volatile int write_buf_head;\r
  67 volatile int write_buf_tail;\r
  68 \r
  69 inline int inc(int x) { x++; if (x>=BUFSIZE) x=0; return x; }\r
  70 inline int read_full() { return inc(read_buf_tail)==read_buf_head; }\r
  71 inline int abs(int x) { return x<0 ? -x : x; }\r
  72 inline int read_size() { return read_buf_tail<read_buf_head ? (read_buf_head-read_buf_tail) : (read_buf_tail-read_buf_head); }\r
  73 inline int read_empty() { return read_buf_head==read_buf_tail; }\r
  74 inline int read_nearlyFull() {\r
  75   if (read_buf_tail==read_buf_head) return 0;\r
  76   if (read_buf_tail < read_buf_head) return (read_buf_head-read_buf_tail) < (BUFSIZE/2);\r
  77   return (read_buf_tail-read_buf_head) > (BUFSIZE/2);\r
  78 }\r
  79 \r
  80 inline int write_full() { return inc(write_buf_tail)==write_buf_head; }\r
  81 inline int write_empty() { return write_buf_head==write_buf_tail; }\r
  82 inline int write_nearlyFull() {\r
  83   if (write_buf_tail==write_buf_head) return 0;\r
  84   if (write_buf_tail < write_buf_head) return (write_buf_head-write_buf_tail) < (BUFSIZE/2);\r
  85   return (write_buf_tail-write_buf_head) > (BUFSIZE/2);\r
  86 }\r
  87 \r
  88 inline char recv() {\r
  89   int q;\r
  90   char ret;\r
  91   while(read_empty()) cts(1);\r
  92   ret = read_buf[read_buf_head];\r
  93   read_buf_head = inc(read_buf_head);\r
  94   if (!read_nearlyFull()) cts(1);\r
  95   return ret;\r
  96 }\r
  97 \r
  98 ISR(SIG_UART0_DATA) {\r
  99   if (write_empty()) {\r
 100     UCSR0B &= ~(1 << UDRIE0);\r
 101     return;\r
 102   }\r
 103   char ret = write_buf[write_buf_head];\r
 104   write_buf_head = inc(write_buf_head);\r
 105   UDR0 = (int)ret;\r
 106   sei();\r
 107 }\r
 108 \r
 109 void send(char c) {\r
 110   while (write_full());\r
 111   write_buf[write_buf_tail] = c;\r
 112   write_buf_tail = inc(write_buf_tail);\r
 113   if (PORTE & (1<<2)) PORTE &= ~(1<<2);\r
 114   else                PORTE |=  (1<<2);\r
 115   UCSR0B |= (1 << UDRIE0);\r
 116   if (PORTE & (1<<3)) PORTE &= ~(1<<3);\r
 117   else                PORTE |=  (1<<3);\r
 118 }\r
 119 \r
 120 \r
 121 void fpga_interrupts(int on) {\r
 122   if (on/* && interrupt_count<301*/) {\r
 123     //FISUA = 0x1;\r
 124     FISCR = 0x80;\r
 125     FISUD = 0x08;\r
 126   } else {\r
 127     FISUD = 0;\r
 128     FISCR = 0;\r
 129   }\r
 130 }\r
 131 \r
 132 void init() {\r
 133   read_buf_head = 0;\r
 134   read_buf_tail = 0;\r
 135   write_buf_head = 0;\r
 136   write_buf_tail = 0;\r
 137   //initUART0(1, 0);  //for slow board\r
 138   initUART0(0, 0);  //for slow board\r
 139   //initUART0(0, 1);  //for slow board\r
 140 }\r
 141 \r
 142 void conf(int z, int y, int x, int d) {\r
 143   FPGAX = x;\r
 144   FPGAY = y;\r
 145   FPGAZ = z;\r
 146   FPGAD = d;\r
 147 }\r
 148 \r
 149 void doreset() {\r
 150   int i;\r
 151   for(i=0; i<5; i++) {\r
 152     PORTD = ~0x01;\r
 153     _delay_ms(50);\r
 154     PORTD = ~0x02;\r
 155     _delay_ms(50);\r
 156     PORTD = ~0x04;\r
 157     _delay_ms(50);\r
 158     PORTD = ~0x08;\r
 159     _delay_ms(50);\r
 160   }\r
 161   PORTD = ~0x00;\r
 162   wdt_enable(WDTO_250MS);\r
 163   while(1) { }\r
 164 }\r
 165 \r
 166 #define TIMERVAL 100\r
 167 int portdc = 0;\r
 168 \r
 169 ISR(SIG_FPGA_INTERRUPT15) { \r
 170   PORTD = portdc++;\r
 171   interrupt_count++;\r
 172   //PORTD = ~(interrupt_count & 0xff);\r
 173   //if (interrupt_count >= 301) fpga_interrupts(0);\r
 174   //sei();\r
 175   fpga_interrupts(1);\r
 176 }\r
 177 /*\r
 178 ISR(SIG_OVERFLOW0) { \r
 179   fpga_interrupts(0);\r
 180   PORTD = ~FISUA;\r
 181   TCNT0 = TIMERVAL;           // load the nearest-to-one-second value  into the timer0\r
 182   TIMSK |= (1<<TOIE0);        // enable the compare match1 interrupt and the timer/counter0 overflow interrupt\r
 183   if (sending) UDR1 = FISUA;\r
 184   fpga_interrupts(1);\r
 185   sei();\r
 186 } \r
 187 void init_timer()  { \r
 188   TCCR0 |= (1<<CS00);         // set the timer0 prescaler to CK\r
 189   TCNT0 = TIMERVAL;           // load the nearest-to-one-second value  into the timer0\r
 190   TIMSK |= (1<<TOIE0);        //enable the compare match1 interrupt and the timer/counter0 overflow interrupt\r
 191 } \r
 192 */\r
 193 ISR(SIG_INTERRUPT0) {   // use interrupt1 since interrupt0 is sent by the watchdog (I think)\r
 194   SREG = 0;\r
 195   //PORTE |=  (1<<0);\r
 196   sei();\r
 197 }\r
 198 void die() { cli(); cts(0); _delay_ms(2000); while(1) { portd(2,0); portd(2,1); } }\r
 199 \r
 200 ISR(SIG_UART0_RECV) {\r
 201 \r
 202   if (UCSR0A & (1 << FE0))    err = 201;//{ portd(2,0); portd(3,1); die(); }  // framing error, lock up with LED=01\r
 203   if ((UCSR0A & (1 << OR0)))  err = 202;//{ portd(2,1); portd(3,0); die(); }  // overflow; lock up with LED=10\r
 204   if (read_full())            err = 203;//{ portd(2,1); portd(3,1); die(); }  // buffer overrun\r
 205 \r
 206   read_buf[read_buf_tail] = UDR0;\r
 207   read_buf_tail = inc(read_buf_tail);\r
 208   if (read_nearlyFull()) cts(0);\r
 209   SREG |= 0x80;\r
 210   sei();\r
 211 }\r
 212 \r
 213 inline int hex(char c) {\r
 214   if (c >= '0' && c <= '9') return (c - '0');\r
 215   if (c >= 'a' && c <= 'f') return ((c - 'a') + 0xa);\r
 216   if (c >= 'A' && c <= 'F') return ((c - 'A') + 0xa);\r
 217   return -1;\r
 218 }\r
 219 \r
 220 int main() {\r
 221   DDRE = (1<<7) | (1<<5) | (1<<4) | (1<<3) | (1<<2);\r
 222   PORTE = 0;\r
 223 \r
 224   init();\r
 225 \r
 226   EIMF = 0xFF;\r
 227   SREG = INT0;\r
 228   sei();\r
 229 \r
 230   PORTE &= ~(1<<7);\r
 231 \r
 232   int count = 0;\r
 233   long long bad = 0;\r
 234   int left = 0;\r
 235   char v = 0;\r
 236   long int oldi = 0;\r
 237   cts(0);\r
 238   cts(1);\r
 239   /*\r
 240   while(1) {\r
 241     long int i = recv() & 0xff;\r
 242     //if (i < 0) err = 200;\r
 243     //if (err < 200) {\r
 244       long int newi = oldi+1;\r
 245       if (newi >= 256) newi -= 256;\r
 246       if (i != newi) err++;\r
 247       //}\r
 248     oldi = i;\r
 249 \r
 250     send(err >= 10 ? 255 : i);\r
 251     //send(err);\r
 252   }\r
 253   */\r
 254 \r
 255   PORTE |=  (1<<3);\r
 256   PORTE |=  (1<<5);\r
 257   recv();\r
 258   send('O');\r
 259   send('B');\r
 260   send('I');\r
 261   send('T');\r
 262   send('S');\r
 263   send('\n');\r
 264 \r
 265   int x=0, y=0, z=0;\r
 266   //while(1) send(/*FISUA*/2);\r
 267   for(;;) {\r
 268     /*\r
 269     if (PORTE & (1<<6)) PORTE &= ~(1<<6);\r
 270     else                PORTE |=  (1<<6);\r
 271     */\r
 272     int i, d=0;\r
 273     int r = recv();\r
 274     switch(r) {\r
 275       case 1:\r
 276         z = recv();\r
 277         y = recv();\r
 278         x = recv();\r
 279         d = recv();\r
 280         //portd(1,1);\r
 281         conf(z, y, x, d);\r
 282         //portd(1,0);\r
 283         break;\r
 284       case 2:\r
 285         //fpga_interrupts(0);\r
 286         //portd(1,1);\r
 287         send(FISUA);\r
 288         //portd(1,0);\r
 289         //fpga_interrupts(1);\r
 290         break;\r
 291         /*\r
 292       case 3:\r
 293         //init_timer();\r
 294         break;\r
 295       case 4:\r
 296         sending = 1;\r
 297         break;\r
 298       case 5:\r
 299         sending = 0;\r
 300         break;\r
 301       case 6: {\r
 302         int32_t local_interrupt_count = interrupt_count;\r
 303         interrupt_count = 0;\r
 304         send((local_interrupt_count >> 24) & 0xff);\r
 305         send((local_interrupt_count >> 16) & 0xff);\r
 306         send((local_interrupt_count >>  8) & 0xff);\r
 307         send((local_interrupt_count >>  0) & 0xff);\r
 308         fpga_interrupts(1);\r
 309         break;\r
 310       }\r
 311         */\r
 312         /*\r
 313       default: {\r
 314         if ((r & 0x80) == 0x80) {\r
 315           switch (r & 0x44) {\r
 316             case 0x44: z = recv(); break;\r
 317             case 0x40: z++;        break;\r
 318             case 0x04: z--;        break;\r
 319           }\r
 320           switch (r & 0x22) {\r
 321             case 0x22: y = recv(); break;\r
 322             case 0x20: y++;        break;\r
 323             case 0x02: y--;        break;\r
 324           }\r
 325           switch (r & 0x11) {\r
 326             case 0x11: x = recv(); break;\r
 327             case 0x10: x++;        break;\r
 328             case 0x01: x--;        break;\r
 329           }\r
 330           d = recv();\r
 331           portd(1,1);\r
 332           conf(z, y, x, d);\r
 333           portd(1,0);\r
 334           break;\r
 335         }\r
 336         die();\r
 337       }\r
 338         */\r
 339     }\r
 340   }\r
 341   return 0;\r
 342 \r
 343 }  \r
 344 \r