fag/speciale/nodes/src/images/battery/module.cpp

   1 #ifdef HAVE_CONFIG_H
   2 # include "config.h"
   3 #endif
   4 #include "common.h"
   5 #include "hal/uart.h"
   6 #include "nodetime.h"
   7 #include "stdio.h"
   8 #include "linear_actuator/s02V2.h"
   9 #include "hal/AT91SAM7S256.h"
  10 #include "linear_actuator/board.h"
  11 #include "linear_actuator/s02V2.h"
  12
  13 struct leg_t *leg;
  14
  15 /* Defines */
  16 #define TYPE_POSE 1
  17 #define TYPE_ID 2
  18
  19 #define NTHREADS 2
  20 #define STACKSIZE ( CYGNUM_HAL_STACK_SIZE_TYPICAL + 4096 )
  21
  22 /* STATICS */
  23
  24 bool new_command = false;
  25 volatile int position = 0;
  26
  27 extern "C" void
  28 _impure_ptr (void)
  29 {
  30 }
  31
  32 #if (MODULE_ID == -1)
  33 # error MODULE_ID not defined
  34 #endif
  35
  36 extern "C" void recieve_data0(void *ptr) {
  37   struct uart_t *uart = (struct uart_t *)ptr;
  38   byte buffer[28];
  39   static int pose_id = MODULE_ID+3;
  40
  41   uart_recieve_data(uart, &buffer, 28);
  42
  43   if(buffer[2] == TYPE_ID)
  44     {
  45       printf("Requested redefinition of gait ID\n");
  46       for (int i=3;i<28;i++)
  47         {
  48           if(buffer[i] == MODULE_ID)
  49             {
  50               pose_id = i;
  51               printf("Recieved column id: %d\n",pose_id-3);
  52             }
  53         }
  54     }
  55
  56   if(buffer[2] == TYPE_POSE)
  57     {
  58       position = buffer[pose_id];
  59       printf("Recieved pose %d\n",position);
  60       if ((position > 0 && position < 62) || position == 87)
  61         new_command = true;
  62     }
  63
  64   buffer[0] &= 127; // set bit 0 to 0
  65   //buffer[1] = MODULE_ID;
  66   //uart_send_message(uart,buffer,28);
  67 }
  68
  69 extern "C" void
  70 cyg_user_start (void)
  71 {
  72   /* Set up system LED (SYSLED) and Power Control pin (PWRCTRL) */
  73   AT91C_BASE_PIOA->PIO_PER = (SYSLED|PWRCTRL);
  74   AT91C_BASE_PIOA->PIO_OER = (SYSLED|PWRCTRL);
  75   AT91C_BASE_PIOA->PIO_SODR = (SYSLED|PWRCTRL);
  76
  77   struct uart_t *uart1 = create_uart (1);
  78   struct uart_t *uart0 = create_uart (0);
  79
  80   uart_tie (uart0, uart1);
  81
  82   uart_register_recieve_callback(uart0,recieve_data0,uart0,1);
  83   uart_register_recieve_callback(uart1,recieve_data0,uart1,1);
  84
  85   printf("MODULE_ID: %X\n",MODULE_ID);
  86
  87 }