Solved a problem with the transmission. It got blocked due to an unknown...

Solved a problem with the transmission. It got blocked due to an unknown error. Added a timeout to avoid this error.

communications/include/comm_cfg.h

@@ -14,7 +14,7 @@
 #endif
 
 #ifndef DYN_MASTER_DEFAULT_TIMEOUT_US
-  #define DYN_MASTER_DEFAULT_TIMEOUT_US           10000
+  #define DYN_MASTER_DEFAULT_TIMEOUT_US           2000
 #endif
 
 #ifndef SERIAL_CONSOLE_MAX_BUFFER_LEN

communications/include/dynamixel_master.h

@@ -19,6 +19,8 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids);
 uint8_t dyn_master_ping(uint8_t id);
 uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data);
 uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data);
+uint8_t dyn_master_start_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
+uint8_t dyn_master_wait_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
 uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
 uint8_t dyn_master_write_byte(uint8_t id, uint16_t address, uint8_t data);
 uint8_t dyn_master_write_word(uint8_t id, uint16_t address, uint16_t data);

communications/src/dynamixel_master.c

@@ -113,7 +113,17 @@ ISR(USART0_RX_vect)
 
 uint8_t dyn_master_wait_transmission(void)
 {
-  while(dyn_master_sent_done==0);
+  dyn_master_start_timeout();
+  while(dyn_master_sent_done==0)
+  {
+    if(dyn_master_timeout())
+    {
+      dyn_master_cancel_timeout();
+      dyn_master_sent_done=0x01;
+      return DYN_TIMEOUT;
+    }
+  }
+  dyn_master_cancel_timeout();
   
   return DYN_SUCCESS;
 }
@@ -274,7 +284,8 @@ uint8_t dyn_master_ping(uint8_t id)
   uint8_t error;
 
   // wait until any previous transmissions ends
-  dyn_master_wait_transmission();
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
   // generate the ping packet for the desired device
   dyn_init_ping_packet(dyn_master_tx_buffer,id);
   dyn_master_rx_num_packets=0x01;
@@ -310,12 +321,56 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
   return error;
 }
 
+
+uint8_t dyn_master_start_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+  uint8_t error;
+
+  // wait until any previous transmissions ends
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
+  // generate the read packet for the desired device
+  dyn_init_read_packet(dyn_master_tx_buffer,id,address,length);
+  dyn_master_rx_num_packets=0x01;
+  if(dyn_master_return_level==no_return || id==DYN_BROADCAST_ID)
+    dyn_master_rx_no_answer=0x01;
+  else
+    dyn_master_rx_no_answer=0x00;
+  // enable transmission
+  dyn_master_set_tx_mode();
+  // send the data
+  if((error=dyn_master_send())!=DYN_SUCCESS)
+  {
+    dyn_master_set_rx_mode();
+    return error;
+  }
+  return error;
+}
+
+uint8_t dyn_master_wait_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+  uint8_t error=DYN_SUCCESS;
+
+  // wait for the replay within the given timeout
+  if(dyn_master_return_level!=no_return && id!=DYN_BROADCAST_ID)
+  {
+    if((error=dyn_master_wait_reception())==DYN_SUCCESS)
+    {
+      if(dyn_get_read_status_data(dyn_master_rx_buffer,data)!=length)// not enough data
+        error=DYN_INST_ERROR;
+    }
+  }
+
+  return error;
+}
+
 uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
 {
   uint8_t error;
 
   // wait until any previous transmissions ends
-  dyn_master_wait_transmission();
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
   // generate the read packet for the desired device
   dyn_init_read_packet(dyn_master_tx_buffer,id,address,length);
   dyn_master_rx_num_packets=0x01;
@@ -363,7 +418,8 @@ uint8_t dyn_master_write_table(uint8_t id, uint16_t address, uint16_t length, ui
   uint8_t error;
 
   // wait until any previous transmissions ends
-  dyn_master_wait_transmission();
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
   // generate the write packet for the desired device
   dyn_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
   dyn_master_rx_num_packets=0x01;
@@ -391,7 +447,8 @@ uint8_t dyn_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint
   uint8_t error;
 
   // wait until any previous transmissions ends
-  dyn_master_wait_transmission();
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
   // generate the registered write packet for the desired device
   dyn_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
   dyn_master_rx_num_packets=0x01;
@@ -419,7 +476,8 @@ uint8_t dyn_master_action(void)
   uint8_t error;
 
   // wait until any previous transmissions ends
-  dyn_master_wait_transmission();
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
   // generate the action packet for the desired device
   dyn_init_action_packet(dyn_master_tx_buffer);
   dyn_master_rx_num_packets=0x01;
@@ -441,7 +499,8 @@ uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_
   uint8_t error;
 
   // wait until any previous transmissions ends
-  dyn_master_wait_transmission();
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
   // generate the write packet for the desired device
   dyn_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
   dyn_master_rx_num_packets=0x01;
@@ -463,7 +522,8 @@ uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint16_t *address, uint16_
   uint8_t error,i,ver1_address[255],ver1_length[255];
 
   // wait until any previous transmissions ends
-  dyn_master_wait_transmission();
+  if(dyn_master_wait_transmission()==DYN_TIMEOUT)
+    return DYN_TIMEOUT;
   // generate the read packet for the desired device
   for(i=0;i<num;i++)
   {

communications/src/serial_console.c

@@ -18,17 +18,14 @@ static FILE *device;
 /* interrupt handlers */
 SIGNAL(USART1_RX_vect)
 {
-  cli();
   serial_console_rx_buffer[serial_console_rx_write_ptr]=UDR1;
   serial_console_rx_write_ptr=(serial_console_rx_write_ptr+1)%SERIAL_CONSOLE_MAX_BUFFER_LEN;
   if(serial_console_rx_num_data<SERIAL_CONSOLE_MAX_BUFFER_LEN)
     serial_console_rx_num_data++;
-  sei();
 }
 
 SIGNAL(USART1_TX_vect)
 {
-  cli();
   if(serial_console_tx_num_data>0)
   {
     UDR1=serial_console_tx_buffer[serial_console_tx_read_ptr];
@@ -37,7 +34,6 @@ SIGNAL(USART1_TX_vect)
   }
   else
     serial_console_sending=0x00;
-  sei();
 }
 
 /* private functions */
@@ -53,7 +49,6 @@ void serial_console_set_baudrate(uint32_t baudrate)
 int serial_console_putchar(char c,FILE *dev)
 {
 
-  cli();
   if(c=='\n')
   {
     serial_console_tx_buffer[serial_console_tx_write_ptr]='\r';
@@ -70,7 +65,6 @@ int serial_console_putchar(char c,FILE *dev)
     serial_console_tx_num_data--;
     serial_console_sending=0x01;
   }
-  sei();
 
   return 0;
 }
@@ -79,7 +73,6 @@ int serial_console_getchar(FILE *dev)
 {
   int8_t rx;
 
-  cli();
   if(serial_console_rx_num_data>0)
   {
     rx=serial_console_rx_buffer[serial_console_rx_read_ptr];
@@ -87,14 +80,10 @@ int serial_console_getchar(FILE *dev)
     serial_console_rx_num_data--;
     if(rx=='\r')
       rx='\n';
-    sei();
     return rx;
   }
   else
-  {
-    sei();
     return -1;
-  }
 }
 
 /* pubic functions */

controllers/include/cm510_cfg.h

@@ -16,7 +16,7 @@
 #define DYN_MASTER_MAX_TX_BUFFER_LEN            128
 #define DYN_MASTER_MAX_RX_BUFFER_LEN            128
 #define DYN_MASTER_DEFAULT_BAUDRATE             1000000
-#define DYN_MASTER_DEFAULT_TIMEOUT_US           1000
+#define DYN_MASTER_DEFAULT_TIMEOUT_US           2000
 #define SERIAL_CONSOLE_MAX_BUFFER_LEN           128
 
 // motion configuration parameters
@@ -31,8 +31,8 @@
 #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_ON   10
 #define BALANCE_GYRO_CAL_FAILED_ALARM_TIME_OFF  100
 #define BALANCE_MAX_CAL_GYRO_ERROR              20.0
-#define BALANCE_FORWARD_FALL_THD_VALUE          (-200)
-#define BALANCE_BACKWARD_FALL_THD_VALUE         200
+#define BALANCE_FORWARD_FALL_THD_VALUE          200
+#define BALANCE_BACKWARD_FALL_THD_VALUE         (-200)
 #define BALANCE_KNEE_GAIN                       (4.0/54.0)
 #define BALANCE_ANKLE_PITCH_GAIN                (4.0/18.0)
 #define BALANCE_ANKLE_ROLL_GAIN                 (4.0/20.0)

dyn_devices/src/exp_board/exp_board.c

@@ -41,7 +41,17 @@ uint8_t exp_board_int_data[BLOCK_LENGTH];
 
 /* private functions */
 #if EXP_BOARD_USE_LOOP==1
+void exp_board_loop_start(void)
+{
+  dyn_master_start_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
+}
+
+void exp_board_loop_read(void)
+{
+  dyn_master_wait_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
+}
 void exp_board_loop(void)
+
 {
   dyn_master_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
 }

examples/movements/Makefile

@@ -2,7 +2,7 @@ PROJECT=movements
 ########################################################
 # afegir tots els fitxers que s'han de compilar aquí
 ########################################################
-SOURCES=movements.c mtn_library.c
+SOURCES=movements.c
 
 OBJS=$(SOURCES:.c=.o)
 SRC_DIR=./

examples/movements/movements.c

@@ -14,13 +14,14 @@ void user_init(void)
   balance_init();
   balance_calibrate_gyro();
   balance_enable_gyro();
+  user_time_set_period(100);
   mtn_lib_init();
 }
 
 typedef uint8_t (*fnct_ptr)(void);
 
-fnct_ptr fnct1=fast_walk_forward;
-//fnct_ptr fnct1=walk_forward;
+//fnct_ptr fnct1=fast_walk_forward;
+fnct_ptr fnct1=walk_forward;
 //fnct_ptr fnct1=walk_right;
 //fnct_ptr fnct1=walk_backward_turn_left;
 
@@ -31,6 +32,7 @@ fnct_ptr fnct2=turn_left;
 void user_loop(void)
 {
   static main_states state=wait_start;
+  static unsigned char i=0;
 
   switch(state)
   {
@@ -66,7 +68,14 @@ void user_loop(void)
                if(fnct1()==0x01)
                  state=wait_cmd;
                else
+               {
                  state=walk;
+//                 if(user_time_is_period_done())
+//                 {
+                   printf("%d\n",i);
+                   i++;
+//                 }
+               }
                break;
     case walk2: if(is_button_rising_edge(BTN_RIGHT))
                   mtn_lib_stop_mtn();

examples/vision/cm510_vision.c

@@ -40,9 +40,10 @@ void user_loop(void)
                        break;
       case wait_cmd: if(serial_console_get_num_data()>0)
                      {
-                       printf("New data received\n");
+//                       printf("New data received\n");
                        cm510_scanf("%c",&cmd);
-                       switch(cmd)
+                       printf("%c",cmd);
+/*                       switch(cmd)
                        {
                          case 'l': pan_set_speed(200);
                                    pan_move_angle(-70);
@@ -56,7 +57,8 @@ void user_loop(void)
                                    break;
                          default: state=wait_cmd;
                                   break;
-                       }
+                       }*/
+                       state=wait_cmd;
                      }
                      else
                        state=wait_cmd;

motion/include/motion_cfg.h

@@ -48,11 +48,11 @@
 #endif
 
 #ifndef BALANCE_FORWARD_FALL_THD_VALUE
-  #define BALANCE_FORWARD_FALL_THD_VALUE          (-200)
+  #define BALANCE_FORWARD_FALL_THD_VALUE          200
 #endif
 
 #ifndef BALANCE_BACKWARD_FALL_THD_VALUE
-  #define BALANCE_BACKWARD_FALL_THD_VALUE         200
+  #define BALANCE_BACKWARD_FALL_THD_VALUE         (-200)
 #endif
 
 #ifndef BALANCE_KNEE_GAIN

motion/src/motion_manager.c

@@ -22,6 +22,8 @@ extern void balance_get_all_offsets(int16_t **offsets);
 extern void action_init(void);
 extern void action_process(void);
 extern void exp_board_loop(void);
+extern void exp_board_loop_start(void);
+extern void exp_board_loop_read(void);
 
 // private variables
 uint8_t manager_num_servos;
@@ -150,9 +152,13 @@ void manager_loop(void)
 {
   if(manager_period_done()==0x01)
   {
-    #if EXP_BOARD_USE_LOOP==1
+/*    #if EXP_BOARD_USE_LOOP==1
       if(exp_board_is_present())
         exp_board_loop(); 
+    #endif*/
+    #if EXP_BOARD_USE_LOOP==1
+      if(exp_board_is_present())
+        exp_board_loop_start(); 
     #endif
     // call the action process
     action_process(); //action_ready
@@ -160,6 +166,10 @@ void manager_loop(void)
     balance_loop();
     // update the pan&tilt angles
     pan_tilt_process();
+    #if EXP_BOARD_USE_LOOP==1
+      if(exp_board_is_present())
+        exp_board_loop_read(); 
+    #endif
     // send the motion commands to the servos
     manager_send_motion_command();
   }
@@ -205,7 +215,7 @@ uint8_t manager_init(uint8_t num_servos)
       manager_servos[id].min_value=0;
       manager_servos[id].center_value=512;
       manager_servos[id].current_value=get_current_position(id);
-      get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
+      get_angle_limits(id,&manager_servos[id].cw_limit,&manager_servos[id].ccw_limit);
       manager_servos[id].module=MM_ACTION;
       manager_servos[id].slope=5;
       manager_num_servos++;
@@ -218,7 +228,7 @@ uint8_t manager_init(uint8_t num_servos)
       manager_servos[id].min_value=0;
       manager_servos[id].center_value=512;
       manager_servos[id].current_value=get_current_position(id);
-      get_angle_limits(id,&manager_servos[i].cw_limit,&manager_servos[id].ccw_limit);
+      get_angle_limits(id,&manager_servos[id].cw_limit,&manager_servos[id].ccw_limit);
       manager_servos[id].module=MM_JOINTS;
       manager_servos[id].slope=5;
       manager_num_servos++;