Added a check for the transmission state in the dynamixel communication module...

Added a check for the transmission state in the dynamixel communication module before starting a new transmission.
Initialized the motion_manager module before calling the user_init() function. This way the dynamixel communications are already set up when the user function is called.
The initialization of the expansion baord is attempted by default. If the board is not found, it will not be used.
Added the expansion board loop function in the motion manager loop for timing.
Added a preprocessor command to either read all expansion board data in the loop() function or read each filed using a separate function.

communications/Makefile

@@ -7,6 +7,7 @@ SOURCES=src/dynamixel.c src/dynamixel_master.c src/serial_console.c
 OBJS=$(SOURCES:.c=.o)
 SRC_DIR=./src/
 INCLUDE_DIR=./include/
+CONT_DIR=../controllers
 BUILD_DIR=./build/
 BIN_DIR=./bin/
 LIB_DIR=./lib/
@@ -38,7 +39,7 @@ $(PROJECT).a: $(OBJS)
 	mkdir -p lib
 	$(OBJCOPY) $(ARFLAGS) $(LIB_DIR)$(PROJECT).a $(OBJS)
 %.o:%.c
-	$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $<
+	$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -I$(CONT_DIR)/include -o $@ $<
 examples:
 	$(MAKE) -C src/examples	
 clean:

communications/src/dynamixel_master.c

+#include "cm510_cfg.h"
 #include "comm_cfg.h"
 #include "dynamixel_master.h"
 
@@ -116,11 +117,6 @@ uint8_t dyn_master_wait_transmission(void)
 
 uint8_t dyn_master_send(void)
 {
-  uint8_t error;
-
-  // wait until any previous transmission ends
-  if((error=dyn_master_wait_transmission())!=DYN_SUCCESS)
-    return error;
   // set the transfer
   dyn_master_sent_bytes=1;
   UDR0=dyn_master_tx_buffer[0];
@@ -268,6 +264,8 @@ uint8_t dyn_master_ping(uint8_t id)
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the ping packet for the desired device
   dyn_init_ping_packet(dyn_master_tx_buffer,id);
   dyn_master_rx_num_packets=0x01;
@@ -313,6 +311,8 @@ uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // 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;
@@ -365,6 +365,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();
   // 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;
@@ -397,6 +399,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();
   // 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;
@@ -429,6 +433,8 @@ uint8_t dyn_master_action(void)
 {
   uint8_t error;
 
+  // wait until any previous transmissions ends
+  dyn_master_wait_transmission();
   // generate the action packet for the desired device
   dyn_init_action_packet(dyn_master_tx_buffer);
   dyn_master_rx_num_packets=0x01;
@@ -455,6 +461,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();
   // 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;
@@ -481,6 +489,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();
   // generate the read packet for the desired device
   for(i=0;i<num;i++)
   {

controllers/include/cm510.h

@@ -32,6 +32,7 @@ extern "C" {
 #include "action.h"
 #include "user_time.h"
 #include "serial_console.h"
+#include "exp_board.h"
 
 // general interface
 /**

controllers/include/cm510_cfg.h

@@ -38,4 +38,8 @@
 #define BALANCE_ANKLE_ROLL_GAIN                 (4.0/20.0)
 #define BALANCE_HIP_ROLL_GAIN                   (4.0/40.0)
 
+// expansion board configuration parameters
+#define EXP_BOARD_USE_LOOP                      1
+#define EXP_BOARD_ID                            192
+
 #endif

controllers/src/adc.c

@@ -21,7 +21,10 @@ void adc_set_channel(uint8_t ch_id)
 {
   ADMUX&=0xF8;
   ADMUX|=(ch_id&0x07);
-  PORTA=~(0x80>>ch_id);// enable the desired channel
+  if(ch_id==0)
+    PORTA&=0xFC;
+  else
+    PORTA=~(0x80>>(ch_id-1));// enable the desired channel
 }
 
 void adc_set_sample_period(void)

controllers/src/cm510.c

@@ -21,6 +21,7 @@
 #include <util/delay.h>
 #include "cm510_cfg.h"
 #include "cm510.h"
+#include "exp_board.h"
 
 /* external user functions */
 extern void user_init(void);
@@ -41,6 +42,7 @@ void init_cm510(void)
   init_adc();
   init_buzzer();
   init_user_time();
+  exp_board_init(EXP_BOARD_ID);
 }
 
 int16_t main(void)
@@ -48,8 +50,8 @@ int16_t main(void)
   init_cm510();
   sei();
   /* call the user initialization function */
-  user_init();
   manager_init(18);
+  user_init();
   // turn BAT_LED on to indicate the initialization is done
   turn_led_on(LED_BAT);
 

dyn_devices/Makefile

@@ -5,11 +5,12 @@ SRC_DIR=./src/
 BIN_DIR=./build/
 LIB_DIR=./lib/
 COMM_DIR=../communications
+CONT_DIR=../controllers
 CC=avr-gcc
 OBJCOPY=avr-ar
 MMCU=atmega2561
 
-INC_DIRS=-I./include/ -I$(COMM_DIR)/include
+INC_DIRS=-I./include/ -I$(COMM_DIR)/include -I$(CONT_DIR)/include
 
 LIBS=$(COMM_DIR)/lib/libcomm.a
 

dyn_devices/include/exp_board.h

@@ -40,6 +40,7 @@ typedef enum {DAC0=0,DAC1} dac_id_t;
 typedef enum {ADC0=0,ADC1,ADC2,ADC3,ADC4,ADC5,ADC6,ADC7} adc_id_t;
 
 uint8_t exp_board_init(uint8_t board_id);
+uint8_t exp_board_is_present(void);
 
 /* ADC interface */
 uint8_t exp_adc_start(void);

dyn_devices/include/exp_board_reg.h

@@ -180,6 +180,66 @@
 #define UART_USB_BAUDRATE_L       0x94
 #define UART_USB_BAUDRATE_H       0x95
 
-#define RAM_SIZE                  150
+// block read of all the expansion board data
+#define BLOCK_START_ADDRESS             0x96
+#define BLOCK_LENGTH                    55
+
+#define GPIO0_data_block                0x96   
+#define GPIO1_data_block                0x97   
+#define GPIO2_data_block                0x98   
+#define GPIO3_data_block                0x99   
+#define GPIO4_data_block                0x9A   
+#define GPIO5_data_block                0x9B   
+#define GPIO6_data_block                0x9C   
+#define GPIO7_data_block                0x9D   
+#define GPIO8_data_block                0x9E   
+#define GPIO9_data_block                0x9F   
+#define GPIO10_data_block               0xA0   
+#define GPIO11_data_block               0xA1   
+#define GPIO12_data_block               0xA2   
+#define GPIO13_data_block               0xA3   
+#define GPIO14_data_block               0xA4   
+#define GPIO15_data_block               0xA5   
+#define GPIO16_data_block               0xA6   
+#define GPIO17_data_block               0xA7   
+#define GPIO18_data_block               0xA8   
+#define ADC_CHANNEL0_L_block            0xA9
+#define ADC_CHANNEL0_H_block            0xAA
+#define ADC_CHANNEL1_L_block            0xAB
+#define ADC_CHANNEL1_H_block            0xAC
+#define ADC_CHANNEL2_L_block            0xAD
+#define ADC_CHANNEL2_H_block            0xAE
+#define ADC_CHANNEL3_L_block            0xAF
+#define ADC_CHANNEL3_H_block            0xB0
+#define ADC_CHANNEL4_L_block            0xB1
+#define ADC_CHANNEL4_H_block            0xB2
+#define ADC_CHANNEL5_L_block            0xB3
+#define ADC_CHANNEL5_H_block            0xB4
+#define ADC_CHANNEL6_L_block            0xB5
+#define ADC_CHANNEL6_H_block            0xB6
+#define ADC_CHANNEL7_L_block            0xB7
+#define ADC_CHANNEL7_H_block            0xB8
+#define ADC_AVG_CHANNEL0_L_block        0xB9
+#define ADC_AVG_CHANNEL0_H_block        0xBA
+#define ADC_AVG_CHANNEL1_L_block        0xBB
+#define ADC_AVG_CHANNEL1_H_block        0xBC
+#define ADC_AVG_CHANNEL2_L_block        0xBD
+#define ADC_AVG_CHANNEL2_H_block        0xBE
+#define ADC_AVG_CHANNEL3_L_block        0xBF
+#define ADC_AVG_CHANNEL3_H_block        0xC0
+#define ADC_AVG_CHANNEL4_L_block        0xC1
+#define ADC_AVG_CHANNEL4_H_block        0xC2
+#define ADC_AVG_CHANNEL5_L_block        0xC3
+#define ADC_AVG_CHANNEL5_H_block        0xC4
+#define ADC_AVG_CHANNEL6_L_block        0xC5
+#define ADC_AVG_CHANNEL6_H_block        0xC6
+#define ADC_AVG_CHANNEL7_L_block        0xC7
+#define ADC_AVG_CHANNEL7_H_block        0xC8
+#define COMPASS_HEADING_VAL_L_block     0xC9
+#define COMPASS_HEADING_VAL_H_block     0xCA
+#define COMPASS_AVG_HEADING_VAL_L_block 0xCB
+#define COMPASS_AVG_HEADING_VAL_H_block 0xCC
+
+#define RAM_SIZE                  205
 
 #endif

dyn_devices/src/exp_board/exp_board.c

@@ -15,11 +15,20 @@
  *  along with iri-libbioloid.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "cm510_cfg.h"
 #include "exp_board_reg.h"
 #include "exp_board.h"
 #include "dynamixel_master.h"
 #include <util/delay.h>
 
+#ifndef EXP_BOARD_USE_LOOP
+  #define EXP_BOARD_USE_LOOP     0
+#endif
+
+#ifndef EXP_BOARD_ID
+  #define EXP_BOARD_ID           192
+#endif
+
 uint8_t exp_board_id;
 uint8_t exp_board_present;
 uint8_t exp_board_dac_present;
@@ -27,11 +36,15 @@ uint8_t exp_board_uart_ttl_present;
 uint8_t exp_board_pwm_present;
 uint8_t exp_board_uart_usb_present;
 
+uint8_t exp_board_int_data[BLOCK_LENGTH];
+
 /* private functions */
+#if EXP_BOARD_USE_LOOP==1
 void exp_board_loop(void)
 {
-  
+  dyn_master_read_table(exp_board_id,BLOCK_START_ADDRESS,BLOCK_LENGTH,exp_board_int_data);
 }
+#endif
 
 // public functions
 // expansion board initialize
@@ -90,6 +103,11 @@ uint8_t exp_board_init(uint8_t board_id)
   }
 }
 
+uint8_t exp_board_is_present(void)
+{  
+  return exp_board_present;
+}
+
 /* ADC interface */
 uint8_t exp_adc_start(void)
 {
@@ -160,22 +178,30 @@ uint8_t exp_adc_get_sample_period(void)
 
 uint16_t exp_adc_get_channel(adc_id_t channel_id)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[ADC_CHANNEL0_L_block+channel_id*2-BLOCK_START_ADDRESS]+exp_board_int_data[ADC_CHANNEL0_H_block+channel_id*2-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t voltage;
 
   if(dyn_master_read_word(exp_board_id,ADC_CHANNEL0_L+channel_id*2,&voltage)==DYN_SUCCESS)
     return ((uint32_t)voltage*5000)/1024;
   else
     return 0xFF;
+  #endif
 }
 
 uint16_t exp_adc_get_avg_channel(adc_id_t channel_id)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[ADC_AVG_CHANNEL0_L_block+channel_id*2-BLOCK_START_ADDRESS]+exp_board_int_data[ADC_AVG_CHANNEL0_H_block+channel_id*2-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t voltage;
-
+ 
   if(dyn_master_read_word(exp_board_id,ADC_AVG_CHANNEL0_L+channel_id*2,&voltage)==DYN_SUCCESS)
     return ((uint32_t)voltage*5000)/1024;
   else
     return 0xFF;
+  #endif
 }
 
 /* GPIO interface */
@@ -191,6 +217,9 @@ uint8_t exp_gpio_config(gpio_id_t gpio_id, uint8_t dir)
 
 uint8_t exp_gpio_get_value(gpio_id_t gpio_id)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[GPIO0_data_block+gpio_id-BLOCK_START_ADDRESS];
+  #else
   uint8_t level;
 
   if(gpio_id<GPIO0 || gpio_id>GPIO18)
@@ -199,6 +228,7 @@ uint8_t exp_gpio_get_value(gpio_id_t gpio_id)
     return level;
   else
     return 0xFF;
+  #endif
 }
 
 uint8_t exp_gpio_set_value(gpio_id_t gpio_id, uint8_t level)
@@ -595,22 +625,30 @@ uint8_t exp_compass_is_calibrating(void)
 
 uint16_t exp_compass_get_heading(void)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[COMPASS_HEADING_VAL_L_block-BLOCK_START_ADDRESS]+exp_board_int_data[COMPASS_HEADING_VAL_H_block-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t heading;
 
   if(dyn_master_read_word(exp_board_id,COMPASS_HEADING_VAL_L,&heading)==DYN_SUCCESS)
     return heading;
   else
     return 0xFFFF;
+  #endif
 }
 
 uint16_t exp_compass_get_avg_heading(void)
 {
+  #if EXP_BOARD_USE_LOOP==1
+    return exp_board_int_data[COMPASS_AVG_HEADING_VAL_L_block-BLOCK_START_ADDRESS]+exp_board_int_data[COMPASS_AVG_HEADING_VAL_H_block-BLOCK_START_ADDRESS]*256;
+  #else
   uint16_t heading;
 
   if(dyn_master_read_word(exp_board_id,COMPASS_AVG_HEADING_VAL_L,&heading)==DYN_SUCCESS)
     return heading;
   else
     return 0xFFFF;
+  #endif
 }
 
 uint8_t exp_compass_set_avg_samples(uint8_t num_samples)
@@ -963,4 +1001,3 @@ uint8_t exp_uart_usb_receive_data(void)
   else
     return 0xFF;
 }
-

motion/src/examples/walk_straight.c

@@ -111,10 +111,10 @@ void user_loop(void)
                             state=wait_walk_ready;
                           else
                           {
-                            initial_heading=exp_compass_get_avg_heading();
-                            action_set_page(32);
-                            action_start_page();
-                            state=check_compass;
+//                            initial_heading=exp_compass_get_avg_heading();
+//                            action_set_page(32);
+//                            action_start_page();
+//                            state=check_compass;
                           }
                           break;
     case check_compass: if((increment_heading=get_avg_heading_inc(initial_heading))>200 || (increment_heading=get_avg_heading_inc(initial_heading))<-200)

motion/src/motion_manager.c

 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
+#include "cm510_cfg.h"
 #include "motion_cfg.h"
 #include "motion_manager.h"
 #include "motion_pages.h"
@@ -10,6 +11,7 @@
 #include "action.h"
 #include "balance.h"
 #include "buzzer.h"
+#include "exp_board.h"
 
 // external functions
 extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
@@ -18,6 +20,7 @@ extern void balance_loop(void);
 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);
 
 // private variables
 uint8_t manager_num_servos;
@@ -165,6 +168,10 @@ void manager_loop(void)
 {
   if(manager_period_done()==0x01)
   {
+    #if EXP_BOARD_USE_LOOP==1
+      if(exp_board_is_present())
+        exp_board_loop(); 
+    #endif
     // call the action process
     action_process(); //action_ready
     // balance the robot