[bioloid_robot] Added function to read increments from compass

dyn_devices/include/exp_board.h

@@ -25,6 +25,8 @@ unsigned short int get_adc_avg_channel(unsigned char channel_id);
 // compass interface
 unsigned short int get_heading(void);
 unsigned short int get_avg_heading(void);
+unsigned short int get_avg_heading_inc(unsigned short int initial_compass);
+
 // UART inetrface
 unsigned char uart_send_byte(unsigned char data);
 unsigned char uart_send_vector(unsigned char *data, unsigned int length);

dyn_devices/src/exp_board/exp_board.c

@@ -79,6 +79,22 @@ unsigned short int get_avg_heading(void)
     return 0xFFFF;//error
 }
 
+unsigned short int get_avg_heading_inc(unsigned short int initial_compass)
+{
+
+  short int actual_compass;
+  short int increase;
+  actual_compass = dxl_read_word(exp_board_id,COMPASS_AVG);
+  increase=actual_compass-(short int)initial_compass; 
+      
+  if(increase<-1800){
+    increase=increase+3600;
+  }else if (increase>1800){
+    increase=increase-3600;
+  }
+  return increase;
+}
+
 // UART inetrface
 unsigned char uart_send_byte(unsigned char data)
 {