diff --git a/Makefile b/Makefile
index a79a32c5d258486def72827424a01502ec862523..456e829d05af9a25d1f09e9ae62ceb8f34d9c43d 100755
--- a/Makefile
+++ b/Makefile
@@ -18,6 +18,7 @@ TARGET_FILES+=src/bioloid_dyn_master_servos.c
TARGET_FILES+=src/motion_pages.c
TARGET_FILES+=src/motion_manager.c
TARGET_FILES+=src/action.c
+TARGET_FILES+=src/bioloid_gyro.c
TARGET_PROCESSOR=STM32F407VG
HAL_PATH=../../STM32_processor/hal/f4
diff --git a/include/action.h b/include/action.h
index 693ccc167a525e600e8c1f30b103b23f593bf394..3a5dcb4d8dff7d65a4e7718463baf24b9d33bfb3 100644
--- a/include/action.h
+++ b/include/action.h
@@ -5,7 +5,7 @@
extern "C" {
#endif
-#include "stm32f4xx.h"
+#include "stm32f4xx_hal.h"
#include "motion_pages.h"
extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
diff --git a/include/action_id.h b/include/action_id.h
index 1628d2f280a83ec9cbd6ad7654b7ea9bc9e08ba2..bedb456c33964ba8fb1d6ae078aa1dc60abe8f45 100644
--- a/include/action_id.h
+++ b/include/action_id.h
@@ -15,7 +15,7 @@
#define F_ATTACK 22
#define DEFENCE 23
#define SIT_DOWN 25
-#define STANF_UP 26
+#define STAND_UP 26
#define F_GET_UP 27
#define B_GET_UP 28
#define CLAP_READY 29
@@ -137,8 +137,4 @@
#define BALANCE 224
#define CLAP_END 225
-
-#endif
-
-
#endif
diff --git a/include/adc_dma.h b/include/adc_dma.h
index eb6393b1f6daf4b07f7efa7dabe2c0a4daf29e26..35a7b0ba81d3ce3ede7031ef254e38ccfb5925a7 100644
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -13,6 +13,7 @@ typedef enum {ADC_CH1,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH6,ADC_CH8} adc_ch_t;
void adc_init(void);
void adc_start(void);
void adc_set_period(uint8_t period_ms);
+inline uint8_t adc_get_period(void);
unsigned short adc_get_channel(adc_ch_t channel);
unsigned short adc_get_channel_raw(adc_ch_t channel);
unsigned short adc_get_temperature(void);
diff --git a/include/bioloid_gyro.h b/include/bioloid_gyro.h
index f9bca06e99a21e48ed3098d183f0bc9b4346087f..62074964f49e8c5504357aa4fd22a827e04d2e34 100644
--- a/include/bioloid_gyro.h
+++ b/include/bioloid_gyro.h
@@ -1,8 +1,18 @@
#ifndef BIOLOID_GYRO_H
#define BIOLOID_GYRO_H
+#include "stm32f4xx_hal.h"
+#include "adc_dma.h"
+
/* public functions */
void gyro_init(void);
-
+inline adc_ch_t gyro_get_fb_adc_channel(void);
+inline adc_ch_t gyro_get_lr_adc_channel(void);
+void gyro_calibrate(void);
+inline void gyro_get_offsets(uint16_t *fb_offset,uint16_t *lr_offset);
+// operation functions
+uint8_t gyro_in_range(unsigned short int address, unsigned short int length);
+void gyro_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void gyro_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
#endif
diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index f466735464b284bb320a626a98029390509f29b1..3c550ef0ace010bcd7720f7afb6ae498c0d1a2d9 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -11,8 +11,8 @@ extern "C" {
#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
#define MM_PERIOD_OFFSET ((unsigned short int)0x0006)
-#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0007)
-#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0008)
+#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0008)
+#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0009)
#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
@@ -118,76 +118,76 @@ typedef enum {
// Enable servo 28 | assigned module | Enable servo 29 | assigned module
BIOLOID_MM_MODULE_EN15 = 0x59, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 30 | assigned module | Enable servo 31 | assigned module
- BIOLOID_MM_SERVO0_CUR_POS_L = 0x5A,
+ BIOLOID_MM_SERVO0_CUR_POS_L = 0x5A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO0_CUR_POS_H = 0x5B,
- BIOLOID_MM_SERVO1_CUR_POS_L = 0x5C,
+ BIOLOID_MM_SERVO1_CUR_POS_L = 0x5C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO1_CUR_POS_H = 0x5D,
- BIOLOID_MM_SERVO2_CUR_POS_L = 0x5E,
+ BIOLOID_MM_SERVO2_CUR_POS_L = 0x5E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO2_CUR_POS_H = 0x5F,
- BIOLOID_MM_SERVO3_CUR_POS_L = 0x60,
+ BIOLOID_MM_SERVO3_CUR_POS_L = 0x60, // angle in fixed point format 9|7
BIOLOID_MM_SERVO3_CUR_POS_H = 0x61,
- BIOLOID_MM_SERVO4_CUR_POS_L = 0x62,
+ BIOLOID_MM_SERVO4_CUR_POS_L = 0x62, // angle in fixed point format 9|7
BIOLOID_MM_SERVO4_CUR_POS_H = 0x63,
- BIOLOID_MM_SERVO5_CUR_POS_L = 0x64,
+ BIOLOID_MM_SERVO5_CUR_POS_L = 0x64, // angle in fixed point format 9|7
BIOLOID_MM_SERVO5_CUR_POS_H = 0x65,
- BIOLOID_MM_SERVO6_CUR_POS_L = 0x66,
+ BIOLOID_MM_SERVO6_CUR_POS_L = 0x66, // angle in fixed point format 9|7
BIOLOID_MM_SERVO6_CUR_POS_H = 0x67,
- BIOLOID_MM_SERVO7_CUR_POS_L = 0x68,
+ BIOLOID_MM_SERVO7_CUR_POS_L = 0x68, // angle in fixed point format 9|7
BIOLOID_MM_SERVO7_CUR_POS_H = 0x69,
- BIOLOID_MM_SERVO8_CUR_POS_L = 0x6A,
+ BIOLOID_MM_SERVO8_CUR_POS_L = 0x6A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO8_CUR_POS_H = 0x6B,
- BIOLOID_MM_SERVO9_CUR_POS_L = 0x6C,
+ BIOLOID_MM_SERVO9_CUR_POS_L = 0x6C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO9_CUR_POS_H = 0x6D,
- BIOLOID_MM_SERVO10_CUR_POS_L = 0x6E,
+ BIOLOID_MM_SERVO10_CUR_POS_L = 0x6E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO10_CUR_POS_H = 0x6F,
- BIOLOID_MM_SERVO11_CUR_POS_L = 0x70,
+ BIOLOID_MM_SERVO11_CUR_POS_L = 0x70, // angle in fixed point format 9|7
BIOLOID_MM_SERVO11_CUR_POS_H = 0x71,
- BIOLOID_MM_SERVO12_CUR_POS_L = 0x72,
+ BIOLOID_MM_SERVO12_CUR_POS_L = 0x72, // angle in fixed point format 9|7
BIOLOID_MM_SERVO12_CUR_POS_H = 0x73,
- BIOLOID_MM_SERVO13_CUR_POS_L = 0x74,
+ BIOLOID_MM_SERVO13_CUR_POS_L = 0x74, // angle in fixed point format 9|7
BIOLOID_MM_SERVO13_CUR_POS_H = 0x75,
- BIOLOID_MM_SERVO14_CUR_POS_L = 0x76,
+ BIOLOID_MM_SERVO14_CUR_POS_L = 0x76, // angle in fixed point format 9|7
BIOLOID_MM_SERVO14_CUR_POS_H = 0x77,
- BIOLOID_MM_SERVO15_CUR_POS_L = 0x78,
+ BIOLOID_MM_SERVO15_CUR_POS_L = 0x78, // angle in fixed point format 9|7
BIOLOID_MM_SERVO15_CUR_POS_H = 0x79,
- BIOLOID_MM_SERVO16_CUR_POS_L = 0x7A,
+ BIOLOID_MM_SERVO16_CUR_POS_L = 0x7A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO16_CUR_POS_H = 0x7B,
- BIOLOID_MM_SERVO17_CUR_POS_L = 0x7C,
+ BIOLOID_MM_SERVO17_CUR_POS_L = 0x7C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO17_CUR_POS_H = 0x7D,
- BIOLOID_MM_SERVO18_CUR_POS_L = 0x7E,
+ BIOLOID_MM_SERVO18_CUR_POS_L = 0x7E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO18_CUR_POS_H = 0x7F,
- BIOLOID_MM_SERVO19_CUR_POS_L = 0x80,
+ BIOLOID_MM_SERVO19_CUR_POS_L = 0x80, // angle in fixed point format 9|7
BIOLOID_MM_SERVO19_CUR_POS_H = 0x81,
- BIOLOID_MM_SERVO20_CUR_POS_L = 0x82,
+ BIOLOID_MM_SERVO20_CUR_POS_L = 0x82, // angle in fixed point format 9|7
BIOLOID_MM_SERVO20_CUR_POS_H = 0x83,
- BIOLOID_MM_SERVO21_CUR_POS_L = 0x84,
+ BIOLOID_MM_SERVO21_CUR_POS_L = 0x84, // angle in fixed point format 9|7
BIOLOID_MM_SERVO21_CUR_POS_H = 0x85,
- BIOLOID_MM_SERVO22_CUR_POS_L = 0x86,
+ BIOLOID_MM_SERVO22_CUR_POS_L = 0x86, // angle in fixed point format 9|7
BIOLOID_MM_SERVO22_CUR_POS_H = 0x87,
- BIOLOID_MM_SERVO23_CUR_POS_L = 0x88,
+ BIOLOID_MM_SERVO23_CUR_POS_L = 0x88, // angle in fixed point format 9|7
BIOLOID_MM_SERVO23_CUR_POS_H = 0x89,
- BIOLOID_MM_SERVO24_CUR_POS_L = 0x8A,
+ BIOLOID_MM_SERVO24_CUR_POS_L = 0x8A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO24_CUR_POS_H = 0x8B,
- BIOLOID_MM_SERVO25_CUR_POS_L = 0x8C,
+ BIOLOID_MM_SERVO25_CUR_POS_L = 0x8C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO25_CUR_POS_H = 0x8D,
- BIOLOID_MM_SERVO26_CUR_POS_L = 0x8E,
+ BIOLOID_MM_SERVO26_CUR_POS_L = 0x8E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO26_CUR_POS_H = 0x8F,
- BIOLOID_MM_SERVO27_CUR_POS_L = 0x90,
+ BIOLOID_MM_SERVO27_CUR_POS_L = 0x90, // angle in fixed point format 9|7
BIOLOID_MM_SERVO27_CUR_POS_H = 0x91,
- BIOLOID_MM_SERVO28_CUR_POS_L = 0x92,
+ BIOLOID_MM_SERVO28_CUR_POS_L = 0x92, // angle in fixed point format 9|7
BIOLOID_MM_SERVO28_CUR_POS_H = 0x93,
- BIOLOID_MM_SERVO29_CUR_POS_L = 0x94,
+ BIOLOID_MM_SERVO29_CUR_POS_L = 0x94, // angle in fixed point format 9|7
BIOLOID_MM_SERVO29_CUR_POS_H = 0x95,
- BIOLOID_MM_SERVO30_CUR_POS_L = 0x96,
+ BIOLOID_MM_SERVO30_CUR_POS_L = 0x96, // angle in fixed point format 9|7
BIOLOID_MM_SERVO30_CUR_POS_H = 0x97,
- BIOLOID_MM_SERVO31_CUR_POS_L = 0x98,
+ BIOLOID_MM_SERVO31_CUR_POS_L = 0x98, // angle in fixed point format 9|7
BIOLOID_MM_SERVO31_CUR_POS_H = 0x99,
BIOLOID_ACTION_PAGE = 0x9A,
BIOLOID_ACTION_CNTRL = 0x9B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | page running | interrupt flag | enable interrupt | stop page | start page
- BIOLOID_GYRO_CNTRL = 0x9C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- //
- BIOLOID_GYRO_STATUS = 0x9D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ BIOLOID_GYRO_CNTRL = 0x9C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | enable fall int | enable fall det | enable calibrate int | calibrate
+ BIOLOID_GYRO_STATUS = 0x9D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
//
BIOLOID_GYRO_FWD_FALL_THD = 0x9E,
BIOLOID_GYRO_BWD_FALL_THD = 0x9F,
@@ -231,6 +231,13 @@ typedef enum {
#define ACTION_INT_FLAG 0x08
#define ACTION_STATUS 0x10
+#define GYRO_BASE_ADDRESS 0x9C
+#define GYRO_MEM_LENGTH 6
+#define GYRO_CALIBRATE 0x01
+#define GYRO_EN_CAL_INT 0x02
+#define GYRO_EN_FALL_DET 0x04
+#define GYRO_EN_FALL_DET_INT 0x08
+
#ifdef __cplusplus
}
#endif
diff --git a/src/action.c b/src/action.c
index c0c6d9773b17d61a2a78ace51c4420b174bd595b..72f2d0ef97d396dd24e12ece3e2a4f7bf16732f9 100644
--- a/src/action.c
+++ b/src/action.c
@@ -80,6 +80,7 @@ void action_load_next_step(void)
if(action_next_index!=current_index)
{
pages_get_page(action_next_index,&action_next_page);
+ ram_data[BIOLOID_ACTION_PAGE]=action_next_index;
if(!pages_check_checksum(&action_next_page))
action_end=0x01;
}
@@ -112,6 +113,7 @@ void action_load_next_step(void)
if(action_next_index!=current_index)
{
pages_get_page(action_next_index,&action_next_page);
+ ram_data[BIOLOID_ACTION_PAGE]=action_next_index;
if(!pages_check_checksum(&action_next_page))
action_end=0x01;
}
diff --git a/src/adc_dma.c b/src/adc_dma.c
index c3bb736db3c5706baeb3d0df2eccc5865591631d..31d3e8e6f87e763704ab3d9a877d16107d9acd1f 100644
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -186,7 +186,7 @@ void adc_init(void)
/* initialize the internal variables */
adc_period_ms=840;// equivalent to 10 ms
- ram_data[BIOLOID_ADC_PERIOD]=84;
+ ram_data[BIOLOID_ADC_PERIOD]=10;
/* enable clocks */
ADC1_CH1_ENABLE_PORT_CLK;
@@ -360,6 +360,12 @@ void adc_start(void)
void adc_set_period(uint8_t period_ms)
{
adc_period_ms=period_ms*84;
+ ram_data[BIOLOID_ADC_PERIOD]=period_ms;
+}
+
+inline uint8_t adc_get_period(void)
+{
+ return ram_data[BIOLOID_ADC_PERIOD];
}
uint16_t adc_get_channel(adc_ch_t channel)
@@ -368,18 +374,19 @@ uint16_t adc_get_channel(adc_ch_t channel)
switch(channel)
{
- case ADC_CH1: ram_data[BIOLOID_ADC_CH1_L]+ram_data[BIOLOID_ADC_CH1_H]*256;
+ case ADC_CH1: value=ram_data[BIOLOID_ADC_CH1_L]+ram_data[BIOLOID_ADC_CH1_H]*256;
break;
- case ADC_CH2: ram_data[BIOLOID_ADC_CH2_L]+ram_data[BIOLOID_ADC_CH2_H]*256;
+ case ADC_CH2: value=ram_data[BIOLOID_ADC_CH2_L]+ram_data[BIOLOID_ADC_CH2_H]*256;
break;
- case ADC_CH3: ram_data[BIOLOID_ADC_CH3_L]+ram_data[BIOLOID_ADC_CH3_H]*256;
+ case ADC_CH3: value=ram_data[BIOLOID_ADC_CH3_L]+ram_data[BIOLOID_ADC_CH3_H]*256;
break;
- case ADC_CH4: ram_data[BIOLOID_ADC_CH4_L]+ram_data[BIOLOID_ADC_CH4_H]*256;
+ case ADC_CH4: value=ram_data[BIOLOID_ADC_CH4_L]+ram_data[BIOLOID_ADC_CH4_H]*256;
break;
- case ADC_CH6: ram_data[BIOLOID_ADC_CH6_L]+ram_data[BIOLOID_ADC_CH6_H]*256;
+ case ADC_CH6: value=ram_data[BIOLOID_ADC_CH6_L]+ram_data[BIOLOID_ADC_CH6_H]*256;
break;
- case ADC_CH8: ram_data[BIOLOID_ADC_CH8_L]+ram_data[BIOLOID_ADC_CH8_H]*256;
+ case ADC_CH8: value=ram_data[BIOLOID_ADC_CH8_L]+ram_data[BIOLOID_ADC_CH8_H]*256;
break;
+ default: value=0x0000;
}
return value;
@@ -403,6 +410,7 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
break;
case ADC_CH8: value=(adc_data[3]&0xFFFF0000)>>16;
break;
+ default: value=0x0000;
}
return value;
diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index d117a2655d1259dd54b17bf820aac020303a5c94..86d6f8ffff9525794a0ebe5b975c3538e701c5d3 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -6,6 +6,7 @@
#include "gpio.h"
#include "adc_dma.h"
#include "zigbee.h"
+#include "bioloid_gyro.h"
#include "motion_manager.h"
#include "action.h"
@@ -85,6 +86,9 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
// action commands
if(action_in_range(address,length))
action_process_write_cmd(address,length,data);
+ // action commands
+ if(gyro_in_range(address,length))
+ gyro_process_write_cmd(address,length,data);
// write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
diff --git a/src/bioloid_gyro.c b/src/bioloid_gyro.c
new file mode 100644
index 0000000000000000000000000000000000000000..8587e386394a972bb131586462b654723f43b1b7
--- /dev/null
+++ b/src/bioloid_gyro.c
@@ -0,0 +1,75 @@
+#include "bioloid_gyro.h"
+#include "ram.h"
+
+#define GYRO_MAX_CAL_SAMPLES 256
+
+/* private variables */
+uint32_t gyro_fb_offset;
+uint32_t gyro_lr_offset;
+
+/* private functions */
+
+/* public functions */
+void gyro_init(void)
+{
+ /* initialize internal variables */
+ gyro_fb_offset=0x0000;
+ gyro_lr_offset=0x0000;
+ /* calibrate the sensor */
+ gyro_calibrate();
+}
+
+inline adc_ch_t gyro_get_fb_adc_channel(void)
+{
+ return ram_data[BIOLIOD_GYRO_FB_ADC_CH];
+}
+
+inline adc_ch_t gyro_get_lr_adc_channel(void)
+{
+ return ram_data[BIOLIOD_GYRO_LR_ADC_CH];
+}
+
+void gyro_calibrate(void)
+{
+ adc_ch_t fb_ch=ram_data[BIOLIOD_GYRO_FB_ADC_CH],lr_ch=ram_data[BIOLIOD_GYRO_LR_ADC_CH];
+ uint16_t i;
+
+ gyro_fb_offset=0x0000;
+ gyro_lr_offset=0x0000;
+ for(i=0;i<GYRO_MAX_CAL_SAMPLES;i++)
+ {
+ gyro_fb_offset+=adc_get_channel_raw(fb_ch);
+ gyro_lr_offset+=adc_get_channel_raw(lr_ch);
+ /* wait for the next sample */
+ HAL_Delay(adc_get_period());
+ }
+ gyro_fb_offset/=GYRO_MAX_CAL_SAMPLES;
+ gyro_lr_offset/=GYRO_MAX_CAL_SAMPLES;
+}
+
+inline void gyro_get_offsets(uint16_t *fb_offset,uint16_t *lr_offset)
+{
+ *fb_offset=gyro_fb_offset;
+ *lr_offset=gyro_lr_offset;
+}
+
+// operation functions
+uint8_t gyro_in_range(unsigned short int address, unsigned short int length)
+{
+ if(ram_in_window(GYRO_BASE_ADDRESS,GYRO_MEM_LENGTH,address,length) ||
+ ram_in_window(BIOLIOD_GYRO_FB_ADC_CH,BIOLIOD_GYRO_LR_ADC_CH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+void gyro_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
+void gyro_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
diff --git a/src/bioloid_gyro.h b/src/bioloid_gyro.h
deleted file mode 100644
index b56982a1b87d4727471769d3ddee304da3827f5f..0000000000000000000000000000000000000000
--- a/src/bioloid_gyro.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#include "bioloid_gyro.h"
-#include "adc_dma.h"
-
-/* private functions */
-
-/* public functions */
-void gyro_init(void)
-{
-
-}
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 37533df226e5d8baebbd7556d7f153cec0a79f68..b34b9d4fe471f0e2f2f7078f754c20ca7a382c6f 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -4,6 +4,7 @@
#include "ram.h"
#include "adc_dma.h"
#include "zigbee.h"
+#include "bioloid_gyro.h"
#include "bioloid_time.h"
#include "bioloid_dyn_slave.h"
#include "bioloid_dyn_master_sensors.h"
@@ -26,11 +27,14 @@ int32_t main(void)
bioloid_time_init();
/* initialize zigbee module */
zigbee_init();
+ zigbee_enable_power();
/* initialize the dynamixel slave interface */
bioloid_dyn_slave_init();
bioloid_dyn_slave_start();
/* initialize the dynamixel master module for the sensors */
bioloid_dyn_master_sensors_init();
+ /* initialize gyro module */
+ gyro_init();
/* initialize motion manager module */
EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
period=eeprom_data&0x00FF;
diff --git a/src/eeprom.c b/src/eeprom.c
index b41aaaf57739324b675d7cd87a6b2d181882eed2..74e9ec522249cda4119266d3b321fa4a9bf4f687 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -52,6 +52,8 @@
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x01FF
+#define DEFAULT_GYRO_FB_ADC_CH 0x0002
+#define DEFAULT_GYRO_LR_ADC_CH 0x0001
#define DEFAULT_RETURN_LEVEL 0x0002
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
@@ -78,6 +80,10 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
MM_PERIOD_OFFSET,
DEFAULT_MM_PERIOD>>8,
MM_PERIOD_OFFSET+1,
+ DEFAULT_GYRO_FB_ADC_CH,
+ GYRO_FB_ADC_CH_OFFSET,
+ DEFAULT_GYRO_LR_ADC_CH,
+ GYRO_LR_ADC_CH_OFFSET,
DEFAULT_RETURN_LEVEL,
RETURN_LEVEL_OFFSET};// return level
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 9f1f39a303bc3391b8480de073a42374aacc23b3..dfed4f3c7596ce748a6eec40c9b2cd4e393e4b52 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -4,6 +4,8 @@
#include "ram.h"
#include "gpio.h"
#include "action.h"
+#include "adc_dma.h"
+#include "bioloid_gyro.h"
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
@@ -91,7 +93,7 @@ void manager_get_target_position(void)
{
if(manager_servos[i].module==MM_ACTION)
{
- manager_servos[i].current_angle=((manager_current_angles[i]+manager_balance_offset[i])>>9);
+ manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]);
//>>16 from the action codification, <<7 from the manager codification
manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
@@ -103,6 +105,27 @@ void manager_get_target_position(void)
void manager_balance(void)
{
+ adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel();
+ uint16_t fb_offset,lr_offset;
+ int16_t fb_value,lr_value;
+
+ if(manager_balance_enabled==0x01)// balance is enabled
+ {
+ // get the offsets of the gyroscope calibration
+ gyro_get_offsets(&fb_offset,&lr_offset);
+ // get the values of the gyroscope
+ fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
+ lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
+ // compensate the servo angle values
+ manager_balance_offset[13]=(fb_value*manager_servos[13].max_angle)/(manager_servos[13].encoder_resolution*54);
+ manager_balance_offset[15]=(fb_value*manager_servos[15].max_angle)/(manager_servos[15].encoder_resolution*18);
+ manager_balance_offset[14]=-(fb_value*manager_servos[14].max_angle)/(manager_servos[14].encoder_resolution*54);
+ manager_balance_offset[16]=-(fb_value*manager_servos[16].max_angle)/(manager_servos[16].encoder_resolution*18);
+ manager_balance_offset[9]=(lr_value*manager_servos[9].max_angle)/(manager_servos[9].encoder_resolution*40);
+ manager_balance_offset[10]=(lr_value*manager_servos[10].max_angle)/(manager_servos[10].encoder_resolution*40);
+ manager_balance_offset[17]=-(lr_value*manager_servos[17].max_angle)/(manager_servos[17].encoder_resolution*20);
+ manager_balance_offset[18]=-(lr_value*manager_servos[18].max_angle)/(manager_servos[18].encoder_resolution*20);
+ }
}
// interrupt handlers
@@ -123,12 +146,12 @@ void MANAGER_TIMER_IRQHandler(void)
// joint_motion_process();
// call the walking process
// walking_process();
+ // balance the robot
+ manager_balance();
// get the target angles from all modules
manager_get_target_position();
- // balance the robot
- // manager_balance();
// send the motion commands to the servos
- // manager_send_motion_command();
+ manager_send_motion_command();
// get the disabled servos position
// manager_get_current_position();
}