diff --git a/include/imu_9dof_dma.h b/include/imu_9dof_dma.h
index 3ece2d8ceb7f310dccbf9edecc6010a18fe5c852..8163d5341d33605b433b0cf5c608e8ac3fbff6fc 100644
--- a/include/imu_9dof_dma.h
+++ b/include/imu_9dof_dma.h
@@ -4,47 +4,43 @@
#include "stm32f4xx.h"
// accelerometer configuration data
-#define RATE_3200HZ 0x0F
-#define RATE_1600HZ 0x0E
-#define RATE_800HZ 0x0D
-#define RATE_400HZ 0x0C
-#define RATE_200HZ 0x0B
-#define RATE_100HZ 0x0A
-#define RATE_50HZ 0x09
-#define RATE_25HZ 0x08
-#define RATE_12_5HZ 0x07
-#define RATE_6_25HZ 0x06
-
-#define ENABLE_LINK 0x20
-#define ENABLE_AUTOSLEEP 0x10
-#define ENABLE_MEASURE 0x08
-#define ENABLE_SLEEP 0x04
-#define SLEEP_8HZ 0x00
-#define SLEEP_4HZ 0x01
-#define SLEEP_2HZ 0x02
-#define SLEEP_1HZ 0x03
-
-#define ENABLE_FUL_RES 0x08
-#define JUSTIFY_LEFT 0x04
-#define JUSTIFY_RIGHT 0x00
-#define RANGE_2G 0x00
-#define RANGE_4G 0x01
-#define RANGE_8G 0x02
-#define RANGE_16G 0x03
+#define ACCEL_RATE_3200HZ 0x0F
+#define ACCEL_RATE_1600HZ 0x0E
+#define ACCEL_RATE_800HZ 0x0D
+#define ACCEL_RATE_400HZ 0x0C
+#define ACCEL_RATE_200HZ 0x0B
+#define ACCEL_RATE_100HZ 0x0A
+#define ACCEL_RATE_50HZ 0x09
+#define ACCEL_RATE_25HZ 0x08
+#define ACCEL_RATE_12_5HZ 0x07
+#define ACCEL_RATE_6_25HZ 0x06
+
+#define ACCEL_ENABLE_LINK 0x20
+#define ACCEL_ENABLE_AUTOSLEEP 0x10
+#define ACCEL_ENABLE_MEASURE 0x08
+#define ACCEL_ENABLE_SLEEP 0x04
+#define ACCEL_SLEEP_8HZ 0x00
+#define ACCEL_SLEEP_4HZ 0x01
+#define ACCEL_SLEEP_2HZ 0x02
+#define ACCEL_SLEEP_1HZ 0x03
+
+#define ACCEL_ENABLE_FULL_RES 0x08
+#define ACCEL_JUSTIFY_LEFT 0x04
+#define ACCEL_JUSTIFY_RIGHT 0x00
+#define ACCEL_RANGE_2G 0x00
+#define ACCEL_RANGE_4G 0x01
+#define ACCEL_RANGE_8G 0x02
+#define ACCEL_RANGE_16G 0x03
+
+#define ACCEL_FIFO_BYPASS 0x00
+#define ACCEL_FIFO_BUFFER 0x40
+#define ACCEL_FIFO_STREAM 0x80
+#define ACCEL_FIFO_TRIGGER 0xC0
// general IMU public functions
void imu_init(void);
-// Accelerometer public functions
-uint8_t imu_accel_detect(void);
-void imu_accel_set_offset(uint8_t off_x, uint8_t off_y, uint8_t off_z);
-void imu_accel_set_data_rate(uint8_t rate_hz);
-void imu_accel_set_measure_mode(void);
-void imu_accel_set_sleep_mode(void);
-void imu_accel_set_sleep_rate(uint8_t rate_hz);
-void imu_accel_set_range(uint8_t range);
-void imu_accel_justify_left(void);
-void imu_accel_justify_right(void);
-void imu_accel_config(void);
+void imu_start(void);
+void imu_stop(void);
// gyroscope public functions
uint8_t imu_gyro_detect(void);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index fe7f6b66956d787442927b93ea8e5c9ec3105268..17afdbbb9a40267040c204fa250fd15cc8d54dc4 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -16,8 +16,8 @@ typedef struct{
uint16_t model;
uint16_t encoder_resolution;
uint8_t gear_ratio;
- int16_t max_angle;
- int16_t center_angle;
+ uint16_t max_angle;
+ uint16_t center_angle;
uint16_t max_speed;
int16_t current_angle;
uint16_t current_value;
diff --git a/include/ram.h b/include/ram.h
index 4524dded57dac296c278dcba6a95ab8e6b6e0092..188dd50047df6072a559b2634d7bfa501163553d 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -40,7 +40,77 @@ typedef enum {
BIOLOID_MM_MODULE_EN12 = 0x2A,
BIOLOID_MM_MODULE_EN13 = 0x2B,
BIOLOID_MM_MODULE_EN14 = 0x2C,
- BIOLOID_MM_MODULE_EN15 = 0x2D
+ BIOLOID_MM_MODULE_EN15 = 0x2D,
+ BIOLOID_MM_SERVO0_CUR_POS_L = 0x2E,
+ BIOLOID_MM_SERVO0_CUR_POS_H = 0x2F,
+ BIOLOID_MM_SERVO1_CUR_POS_L = 0x30,
+ BIOLOID_MM_SERVO1_CUR_POS_H = 0x31,
+ BIOLOID_MM_SERVO2_CUR_POS_L = 0x32,
+ BIOLOID_MM_SERVO2_CUR_POS_H = 0x33,
+ BIOLOID_MM_SERVO3_CUR_POS_L = 0x34,
+ BIOLOID_MM_SERVO3_CUR_POS_H = 0x35,
+ BIOLOID_MM_SERVO4_CUR_POS_L = 0x36,
+ BIOLOID_MM_SERVO4_CUR_POS_H = 0x37,
+ BIOLOID_MM_SERVO5_CUR_POS_L = 0x38,
+ BIOLOID_MM_SERVO5_CUR_POS_H = 0x39,
+ BIOLOID_MM_SERVO6_CUR_POS_L = 0x3A,
+ BIOLOID_MM_SERVO6_CUR_POS_H = 0x3B,
+ BIOLOID_MM_SERVO7_CUR_POS_L = 0x3C,
+ BIOLOID_MM_SERVO7_CUR_POS_H = 0x3D,
+ BIOLOID_MM_SERVO8_CUR_POS_L = 0x3E,
+ BIOLOID_MM_SERVO8_CUR_POS_H = 0x3F,
+ BIOLOID_MM_SERVO9_CUR_POS_L = 0x40,
+ BIOLOID_MM_SERVO9_CUR_POS_H = 0x41,
+ BIOLOID_MM_SERVO10_CUR_POS_L = 0x42,
+ BIOLOID_MM_SERVO10_CUR_POS_H = 0x43,
+ BIOLOID_MM_SERVO11_CUR_POS_L = 0x44,
+ BIOLOID_MM_SERVO11_CUR_POS_H = 0x45,
+ BIOLOID_MM_SERVO12_CUR_POS_L = 0x46,
+ BIOLOID_MM_SERVO12_CUR_POS_H = 0x47,
+ BIOLOID_MM_SERVO13_CUR_POS_L = 0x48,
+ BIOLOID_MM_SERVO13_CUR_POS_H = 0x49,
+ BIOLOID_MM_SERVO14_CUR_POS_L = 0x4A,
+ BIOLOID_MM_SERVO14_CUR_POS_H = 0x4B,
+ BIOLOID_MM_SERVO15_CUR_POS_L = 0x4C,
+ BIOLOID_MM_SERVO15_CUR_POS_H = 0x4D,
+ BIOLOID_MM_SERVO16_CUR_POS_L = 0x4E,
+ BIOLOID_MM_SERVO16_CUR_POS_H = 0x4F,
+ BIOLOID_MM_SERVO17_CUR_POS_L = 0x50,
+ BIOLOID_MM_SERVO17_CUR_POS_H = 0x51,
+ BIOLOID_MM_SERVO18_CUR_POS_L = 0x52,
+ BIOLOID_MM_SERVO18_CUR_POS_H = 0x53,
+ BIOLOID_MM_SERVO19_CUR_POS_L = 0x54,
+ BIOLOID_MM_SERVO19_CUR_POS_H = 0x55,
+ BIOLOID_MM_SERVO20_CUR_POS_L = 0x56,
+ BIOLOID_MM_SERVO20_CUR_POS_H = 0x57,
+ BIOLOID_MM_SERVO21_CUR_POS_L = 0x58,
+ BIOLOID_MM_SERVO21_CUR_POS_H = 0x59,
+ BIOLOID_MM_SERVO22_CUR_POS_L = 0x5A,
+ BIOLOID_MM_SERVO22_CUR_POS_H = 0x5B,
+ BIOLOID_MM_SERVO23_CUR_POS_L = 0x5C,
+ BIOLOID_MM_SERVO23_CUR_POS_H = 0x5D,
+ BIOLOID_MM_SERVO24_CUR_POS_L = 0x5E,
+ BIOLOID_MM_SERVO24_CUR_POS_H = 0x5F,
+ BIOLOID_MM_SERVO25_CUR_POS_L = 0x60,
+ BIOLOID_MM_SERVO25_CUR_POS_H = 0x61,
+ BIOLOID_MM_SERVO26_CUR_POS_L = 0x62,
+ BIOLOID_MM_SERVO26_CUR_POS_H = 0x63,
+ BIOLOID_MM_SERVO27_CUR_POS_L = 0x64,
+ BIOLOID_MM_SERVO27_CUR_POS_H = 0x65,
+ BIOLOID_MM_SERVO28_CUR_POS_L = 0x66,
+ BIOLOID_MM_SERVO28_CUR_POS_H = 0x67,
+ BIOLOID_MM_SERVO29_CUR_POS_L = 0x68,
+ BIOLOID_MM_SERVO29_CUR_POS_H = 0x69,
+ BIOLOID_MM_SERVO30_CUR_POS_L = 0x6A,
+ BIOLOID_MM_SERVO30_CUR_POS_H = 0x6B,
+ BIOLOID_IMU_STATUS = 0x6C,
+ BIOLOID_IMU_CONTROL = 0x6D,
+ BIOLOID_IMU_ACCEL_X_L = 0x6E,
+ BIOLOID_IMU_ACCEL_X_H = 0x6F,
+ BIOLOID_IMU_ACCEL_Y_L = 0x7A,
+ BIOLOID_IMU_ACCEL_Y_H = 0x7B,
+ BIOLOID_IMU_ACCEL_Z_L = 0x7C,
+ BIOLOID_IMU_ACCEL_Z_H = 0x7D
} bioloid_registers;
#define RAM_SIZE 256
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 8b735cdad9de35a729c49f6c02328882c1161309..a69bf5f3fb4bfa0f91a7c3514d7dbc7b48ccca8c 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -74,6 +74,20 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
}
else break;
}
+ if(ram_in_range(BIOLOID_IMU_CONTROL,BIOLOID_IMU_CONTROL,address,length))
+ {
+ if(data[address-BIOLOID_IMU_CONTROL]&0x01)
+ imu_start();
+ else
+ imu_stop();
+ }
+ if(ram_in_range(BIOLOID_MM_CONTROL,BIOLOID_MM_CONTROL,address,length))
+ {
+ if(data[address-BIOLOID_MM_CONTROL]&0x01)
+ manager_enable();
+ else
+ manager_disable();
+ }
// write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
@@ -112,7 +126,7 @@ int32_t main(void)
EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
dyn_slave_set_return_level((uint8_t)eeprom_data);
/* initialize the IMU */
-// imu_init();
+ imu_init();
// initialize the Analog to digital converter
// adc_init();
// initialize motion manager
diff --git a/src/imu_9dof_dma.c b/src/imu_9dof_dma.c
index 2d595a2941485d813543706e241e6646274a487f..5099fb129579d5e3f38ca383feb9e8b7872df831 100644
--- a/src/imu_9dof_dma.c
+++ b/src/imu_9dof_dma.c
@@ -1,4 +1,5 @@
#include "imu_9dof_dma.h"
+#include "ram.h"
#define I2C_IMU_DMA DMA1
#define I2C_IMU_DMA_CHANNEL DMA_Channel_3
@@ -41,26 +42,63 @@
#define I2C_IMU_EV_IRQHandler I2C3_EV_IRQHandler
#define I2C_IMU_ER_IRQHandler I2C3_ER_IRQHandler
+#define IMU_TIMER TIM4
+#define IMU_TIMER_IRQn TIM4_IRQn
+#define IMU_TIMER_IRQHandler TIM4_IRQHandler
+#define IMU_TIMER_CLK RCC_APB1Periph_TIM4
+
+// accelerometer registers
+#define ACCEL_THRESH_TAP 0x1D
+#define ACCEL_OFSX 0x1E
+#define ACCEL_OFSY 0x1F
+#define ACCEL_OFSZ 0x20
+#define ACCEL_DUR 0x21
+#define ACCEL_LATENT 0x22
+#define ACCEL_WINDOW 0x23
+#define ACCEL_THRESH_ACT 0x24
+#define ACCEL_THRESH_INACT 0x25
+#define ACCEL_TIME_INACT 0x26
+#define ACCEL_ACT_INACT_CTL 0x27
+#define ACCEL_THRESH_FF 0x28
+#define ACCEL_TIME_FF 0x29
+#define ACCEL_TAP_AXES 0x2A
+#define ACCEL_ACT_TAP_STATUS 0x2B
+#define ACCEL_BW_RATE 0x2C
+#define ACCEL_POWER_CTL 0x2D
+#define ACCEL_INT_ENABLE 0x2E
+#define ACCEL_INT_MAP 0x2F
+#define ACCEL_INT_SOURCE 0x30
+#define ACCEL_DATA_FORMAT 0x31
+
+
typedef enum {I2C_OP_DONE,I2C_WR_OP,I2C_RD_OP_CMD,I2C_RD_OP_DATA} i2c_op_t;
+typedef enum {IMU_GET_ACCEL,IMU_GET_GYRO,IMU_GET_COMPASS} imu_state_t;
// private variables
// I2C private variables
volatile uint8_t i2c_error;
volatile i2c_op_t i2c_op;
-uint8_t i2c_conf[32];
-uint8_t i2c_data[32];
uint8_t i2c_rd_addr;
uint8_t i2c_wr_addr;
+uint8_t i2c_data_out[32];
// accelerometer data and configuration
const uint8_t accel_id_reg=0x00;
const uint8_t accel_id=0xE5;
const uint8_t accel_id_len=0x01;
const uint8_t accel_conf_reg=0x1D;
-const uint8_t accel_conf_len=0x16;
+const uint8_t accel_conf_len=0x15;
const uint8_t accel_data_reg=0x32;
const uint8_t accel_data_len=0x06;
+const uint8_t accel_fifo_reg=0x38;
+const uint8_t accel_fifo_len=0x01;
+const uint8_t accel_status_reg=0x39;
+const uint8_t accel_status_len=0x01;
const uint8_t accel_rd_addr=0xA6;
const uint8_t accel_wr_addr=0xA7;
+uint8_t i2c_accel_conf[21];
+uint8_t i2c_accel_fifo_conf;
+uint8_t i2c_accel_data[6];
+uint8_t i2c_accel_status;
// gyroscope data and configuration
const uint8_t gyro_id_reg=0x00;
const uint8_t gyro_id=0x68;
@@ -71,6 +109,8 @@ const uint8_t gyro_data_reg=0x1B;
const uint8_t gyro_data_len=0x08;
const uint8_t gyro_rd_addr=0xD0;
const uint8_t gyro_wr_addr=0xD1;
+uint8_t i2c_gyro_conf[4];
+uint8_t i2c_gyro_data[8];
// compass data adn configuration
const uint8_t compass_id_reg=0x0A;
const uint8_t compass_id[3]={'H','4','3'};
@@ -81,11 +121,193 @@ const uint8_t compass_data_reg=0x03;
const uint8_t compass_data_len=0x06;
const uint8_t compass_rd_addr=0x3D;
const uint8_t compass_wr_addr=0x3C;
+uint8_t i2c_compass_conf[4];
+uint8_t i2c_compass_data[6];
// dma initialization structures;
DMA_InitTypeDef IMU_DMA_TX_InitStructure;
DMA_InitTypeDef IMU_DMA_RX_InitStructure;
+// private functions
+void imu_wait_op_done(void)
+{
+ while(i2c_op!=I2C_OP_DONE);
+}
+
+uint8_t imu_is_op_done(void)
+{
+ if(i2c_op==I2C_OP_DONE)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+// private accelerometer functions
+void imu_accel_get_config(void)
+{
+ i2c_op=I2C_RD_OP_CMD;
+ IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_conf_len;
+ IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_accel_conf;
+ DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)&accel_conf_reg;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+ imu_wait_op_done();
+}
+
+void imu_accel_start(void)
+{
+ // enter measure mode
+ i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP]|=ACCEL_ENABLE_MEASURE;
+ i2c_data_out[0]=ACCEL_POWER_CTL;
+ i2c_data_out[1]=i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP];
+ // write the fifo configuration
+ i2c_op=I2C_WR_OP;
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=2;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+ imu_wait_op_done();
+}
+
+void imu_accel_stop(void)
+{
+ // enter measure mode
+ i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP]&=~ACCEL_ENABLE_MEASURE;
+ i2c_data_out[0]=ACCEL_POWER_CTL;
+ i2c_data_out[1]=i2c_accel_conf[ACCEL_POWER_CTL-ACCEL_THRESH_TAP];
+ // write the fifo configuration
+ i2c_op=I2C_WR_OP;
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=2;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+ imu_wait_op_done();
+}
+
+void imu_accel_get_data(void)
+{
+ i2c_op=I2C_RD_OP_CMD;
+ IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_data_len;
+ IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_accel_data;
+ DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)&accel_data_reg;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+}
+
+void imu_accel_set_config(void)
+{
+ uint8_t i;
+
+ // set the configuration
+ i2c_accel_conf[ACCEL_BW_RATE-ACCEL_THRESH_TAP]|=ACCEL_RATE_100HZ;
+ i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]&=0xF0;
+ i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]|=ACCEL_RANGE_8G;// +/-8g range
+ i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]|=ACCEL_JUSTIFY_RIGHT;// justify right
+ i2c_accel_conf[ACCEL_DATA_FORMAT-ACCEL_THRESH_TAP]|=ACCEL_ENABLE_FULL_RES;// enable full resolution according to the range
+ // write the configuration
+ for(i=0;i<accel_conf_len;i++)
+ i2c_data_out[i+1]=i2c_accel_conf[i];
+ i2c_data_out[0]=accel_conf_reg;
+ i2c_op=I2C_WR_OP;
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=accel_conf_len+1;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+ imu_wait_op_done();
+
+ // set the fifo configuration
+ i2c_accel_fifo_conf=ACCEL_FIFO_STREAM;
+ i2c_data_out[0]=accel_fifo_reg;
+ i2c_data_out[1]=i2c_accel_fifo_conf;
+ // write the fifo configuration
+ i2c_op=I2C_WR_OP;
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=accel_fifo_len+1;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data_out;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+ imu_wait_op_done();
+}
+
+uint8_t imu_accel_detect(void)
+{
+ i2c_accel_data[0]=0x00;
+ i2c_op=I2C_RD_OP_CMD;
+ IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_id_len;
+ IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)i2c_accel_data;
+ DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
+ IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr=(uint32_t)&accel_id_reg;
+ DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
+ i2c_rd_addr=accel_rd_addr;
+ i2c_wr_addr=accel_wr_addr;
+ I2C_GenerateSTART(I2C_IMU, ENABLE);
+ imu_wait_op_done();
+ if(i2c_accel_data[0]==accel_id)
+ {
+ imu_accel_get_config();
+ imu_accel_set_config();
+ return 0x01;
+ }
+ else
+ return 0x00;
+}
+
+void imu_accel_process_data(void)
+{
+ uint8_t i;
+ int16_t value;
+
+ for(i=0;i<3;i++)
+ {
+ value=((int16_t)(i2c_accel_data[i*2]+(i2c_accel_data[i*2+1]<<8)))<<2;
+ ram_data[BIOLOID_IMU_ACCEL_X_L+i*2]=value&0xFF;
+ ram_data[BIOLOID_IMU_ACCEL_X_H+i*2]=(value>>8);
+ }
+}
+
// interrupt handlers
+void IMU_TIMER_IRQHandler(void)
+{
+ static imu_state_t state=IMU_GET_COMPASS;
+ uint16_t capture;
+
+ if(TIM_GetITStatus(IMU_TIMER, TIM_IT_CC1)!=RESET)
+ {
+ TIM_ClearITPendingBit(IMU_TIMER,TIM_IT_CC1);
+ capture = TIM_GetCapture1(IMU_TIMER);
+ TIM_SetCompare1(IMU_TIMER, capture + 1000);
+ if(imu_is_op_done())
+ {
+ switch(state)
+ {
+ case IMU_GET_ACCEL: imu_accel_get_data();
+ state=IMU_GET_GYRO;
+ break;
+ case IMU_GET_GYRO: imu_accel_process_data();
+ state=IMU_GET_COMPASS;
+ break;
+ case IMU_GET_COMPASS: state=IMU_GET_ACCEL;
+ break;
+ }
+ }
+ }
+}
+
void I2C_IMU_EV_IRQHandler(void)
{
switch (I2C_GetLastEvent(I2C_IMU))
@@ -117,6 +339,14 @@ void I2C_IMU_EV_IRQHandler(void)
DMA_Cmd(I2C_IMU_DMA_STREAM_RX, ENABLE);
break;
+ case I2C_EVENT_MASTER_BYTE_TRANSMITTED:
+ if(i2c_op==I2C_WR_OP)
+ {
+ i2c_op=I2C_OP_DONE;
+ I2C_GenerateSTOP(I2C_IMU, ENABLE);
+ }
+ break;
+
default:
break;
}
@@ -149,11 +379,6 @@ void I2C_IMU_TX_DMA_IRQHandler(void)
I2C_GenerateSTART(I2C_IMU, ENABLE);
i2c_op=I2C_RD_OP_DATA;
}
- else
- {
- I2C_GenerateSTOP(I2C_IMU, ENABLE);
- i2c_op=I2C_OP_DONE;
- }
}
void I2C_IMU_RX_DMA_IRQHandler(void)
@@ -165,115 +390,23 @@ void I2C_IMU_RX_DMA_IRQHandler(void)
i2c_op=I2C_OP_DONE;
}
-// private functions
-void imu_wait_op_done(void)
-{
- while(i2c_op!=I2C_OP_DONE);
-}
-
-uint8_t imu_is_op_done(void)
-{
- if(i2c_op==I2C_OP_DONE)
- return 0x01;
- else
- return 0x00;
-}
-
-// accelerometer public functions
-uint8_t imu_accel_detect(void)
-{
- i2c_op=I2C_RD_OP_CMD;
- IMU_DMA_RX_InitStructure.DMA_BufferSize=accel_id_len;
- IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data;
- DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
- IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
- IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
- DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
- i2c_conf[0]=accel_id_reg;
- i2c_data[0]=0x00;
- i2c_rd_addr=accel_rd_addr;
- i2c_wr_addr=accel_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
-
- imu_wait_op_done();
- if(i2c_data[0]==accel_id)
- return 0x01;
- else
- return 0x00;
-}
-
-void imu_accel_set_offset(uint8_t off_x, uint8_t off_y, uint8_t off_z)
-{
-
-}
-
-void imu_accel_set_data_rate(uint8_t rate_hz)
-{
-
-}
-
-void imu_accel_set_measure_mode(void)
-{
-
-}
-
-void imu_accel_set_sleep_mode(void)
-{
-
-}
-
-void imu_accel_set_sleep_rate(uint8_t rate_hz)
-{
-
-}
-
-void imu_accel_set_range(uint8_t range)
-{
-
-}
-
-void imu_accel_justify_left(void)
-{
-
-}
-
-void imu_accel_justify_right(void)
-{
-
-}
-
-void imu_accel_config(void)
-{
- i2c_op=I2C_WR_OP;
- IMU_DMA_TX_InitStructure.DMA_BufferSize=accel_conf_len+1;
- IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
- DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
- i2c_conf[0]=accel_conf_reg;
- i2c_conf[15]=0x0A;
- i2c_conf[20]=0x02;
- i2c_rd_addr=accel_rd_addr;
- i2c_wr_addr=accel_wr_addr;
- I2C_GenerateSTART(I2C_IMU, ENABLE);
-}
-
// gyroscope public functions
uint8_t imu_gyro_detect(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=gyro_id_len;
- IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data;
+ IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_gyro_data;
DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
- IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&gyro_id_reg;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
- i2c_conf[0]=gyro_id_reg;
- i2c_data[0]=0x00;
+ i2c_gyro_data[0]=0x00;
i2c_rd_addr=gyro_rd_addr;
i2c_wr_addr=gyro_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
- if((i2c_data[0]&0x7E)==gyro_id)
+ if((i2c_gyro_data[0]&0x7E)==gyro_id)
return 0x01;
else
return 0x00;
@@ -284,21 +417,20 @@ uint8_t imu_compass_detect(void)
{
i2c_op=I2C_RD_OP_CMD;
IMU_DMA_RX_InitStructure.DMA_BufferSize=compass_id_len;
- IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_data;
+ IMU_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_compass_data;
DMA_Init(I2C_IMU_DMA_STREAM_RX,&IMU_DMA_RX_InitStructure);
IMU_DMA_TX_InitStructure.DMA_BufferSize=1;
- IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)i2c_conf;
+ IMU_DMA_TX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&compass_id_reg;
DMA_Init(I2C_IMU_DMA_STREAM_TX,&IMU_DMA_TX_InitStructure);
- i2c_conf[0]=compass_id_reg;
- i2c_data[0]=0x00;
- i2c_data[1]=0x00;
- i2c_data[2]=0x00;
+ i2c_compass_data[0]=0x00;
+ i2c_compass_data[1]=0x00;
+ i2c_compass_data[2]=0x00;
i2c_rd_addr=compass_rd_addr;
i2c_wr_addr=compass_wr_addr;
I2C_GenerateSTART(I2C_IMU, ENABLE);
imu_wait_op_done();
- if(i2c_data[0]==compass_id[0] && i2c_data[1]==compass_id[1] && i2c_data[2]==compass_id[2])
+ if(i2c_compass_data[0]==compass_id[0] && i2c_compass_data[1]==compass_id[1] && i2c_compass_data[2]==compass_id[2])
return 0x01;
else
return 0x00;
@@ -308,7 +440,8 @@ uint8_t imu_compass_detect(void)
void imu_init(void)
{
uint8_t i;
-
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ TIM_OCInitTypeDef TIM_OCInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
@@ -322,6 +455,8 @@ void imu_init(void)
RCC_AHB1PeriphClockCmd(I2C_IMU_SCL_GPIO_CLK, ENABLE);
/* Enable the DMA clock */
RCC_AHB1PeriphClockCmd(DMA_IMU_CLK, ENABLE);
+ /* Enable the IMU timer clock */
+ RCC_APB1PeriphClockCmd(IMU_TIMER_CLK,ENABLE);
/* GPIO Configuration */
/*Configure I2C SCL pin */
@@ -369,10 +504,7 @@ void imu_init(void)
i2c_error=0x00;
i2c_op=I2C_OP_DONE;
for(i=0;i<32;i++)
- {
- i2c_data[i]=0x00;
- i2c_conf[i]=0x00;
- }
+ i2c_data_out[i]=0x00;
/* configure DMA TX */
DMA_DeInit(I2C_IMU_DMA_STREAM_TX);
IMU_DMA_TX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
@@ -420,4 +552,65 @@ void imu_init(void)
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(I2C_IMU_DMA_STREAM_RX,DMA_IT_TC,ENABLE);
DMA_ITConfig(I2C_IMU_DMA_STREAM_RX,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
+
+ // configure the timer
+ // initialize the timer interrupts
+ NVIC_InitStructure.NVIC_IRQChannel = IMU_TIMER_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* motion timer configuration */
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ TIM_TimeBaseStructure.TIM_Prescaler = 82;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseInit(IMU_TIMER,&TIM_TimeBaseStructure);
+ TIM_SetClockDivision(IMU_TIMER,TIM_CKD_DIV1);
+
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+ TIM_OCInitStructure.TIM_Pulse = 1000;// 1ms period
+ TIM_OC1Init(IMU_TIMER, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(IMU_TIMER, TIM_OCPreload_Disable);
+
+ /* detect the three sensors in the imu */
+ if(imu_accel_detect())
+ ram_set_bit(BIOLOID_IMU_STATUS,1);
+ else
+ ram_clear_bit(BIOLOID_IMU_STATUS,1);
+ if(imu_gyro_detect())
+ ram_set_bit(BIOLOID_IMU_STATUS,2);
+ else
+ ram_clear_bit(BIOLOID_IMU_STATUS,2);
+ if(imu_compass_detect())
+ ram_set_bit(BIOLOID_IMU_STATUS,3);
+ else
+ ram_clear_bit(BIOLOID_IMU_STATUS,3);
+}
+
+void imu_start(void)
+{
+ // start the accelerometer
+ imu_accel_start();
+
+ // start the timer to get the imu data
+ TIM_Cmd(IMU_TIMER, ENABLE);
+ TIM_ITConfig(IMU_TIMER, TIM_IT_CC1, ENABLE);
+
+ ram_set_bit(BIOLOID_IMU_STATUS,0);
+}
+
+void imu_stop(void)
+{
+ // stop the timer to get the imu data
+ TIM_Cmd(IMU_TIMER, DISABLE);
+ TIM_ITConfig(IMU_TIMER, TIM_IT_CC1, DISABLE);
+
+ // stop the accelerometer
+ imu_accel_stop();
+
+ ram_clear_bit(BIOLOID_IMU_STATUS,0);
}
diff --git a/src/motion_manager.c b/src/motion_manager.c
index e79cbc0456ed37241bb6710464c68a22fceb0904..c7dc63d94f1ae580ce8b2ad454eb0da7e333d85a 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -12,7 +12,6 @@
// private variables
__IO uint16_t manager_motion_period;
uint8_t manager_num_servos;
-uint8_t manager_enabled;
TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// private functions
@@ -40,7 +39,7 @@ inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
inline int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value)
{
- return (value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution-manager_servos[servo_id].center_angle;
+ return (((value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution)-manager_servos[servo_id].center_angle);
}
inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
@@ -55,6 +54,29 @@ inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
return (value*2)/3;
}
+void manager_get_current_position(void)
+{
+ uint8_t i;
+
+ for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+ {
+ if(manager_servos[i].model!=0x0000)// servo is present
+ {
+ if(manager_servos[i].enabled)// servo is enabled
+ {
+ // the current position is fixed by one of the motion modules
+ }
+ else// servo is disabled
+ {
+ dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ }
+ }
+ }
+}
+
// interrupt handlers
void MOTION_TIMER_IRQHandler(void)
{
@@ -75,6 +97,7 @@ void MOTION_TIMER_IRQHandler(void)
// manager_balance();
// send the motion commands to the servos
// manager_send_motion_command();
+ manager_get_current_position();
}
}
@@ -103,62 +126,62 @@ void manager_init(uint16_t period_us)
{
case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=254;
- manager_servos[i].max_angle=3000;
- manager_servos[i].center_angle=1500;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=354;
break;
case SERVO_AX12W: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=32;
- manager_servos[i].max_angle=3000;
- manager_servos[i].center_angle=1500;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=2830;
break;
case SERVO_AX18A: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=254;
- manager_servos[i].max_angle=3000;
- manager_servos[i].center_angle=1500;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=582;
break;
case SERVO_MX28: manager_servos[i].encoder_resolution=4095;
manager_servos[i].gear_ratio=193;
- manager_servos[i].max_angle=3600;
- manager_servos[i].center_angle=1800;
+ manager_servos[i].max_angle=360<<7;
+ manager_servos[i].center_angle=180<<7;
manager_servos[i].max_speed=330;
break;
case SERVO_RX24F: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=193;
- manager_servos[i].max_angle=3000;
- manager_servos[i].center_angle=1500;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=756;
break;
case SERVO_RX28: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=193;
- manager_servos[i].max_angle=3000;
- manager_servos[i].center_angle=1500;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=402;
break;
case SERVO_RX64: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=200;
- manager_servos[i].max_angle=3000;
- manager_servos[i].center_angle=1500;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=294;
break;
case SERVO_MX64: manager_servos[i].encoder_resolution=4095;
manager_servos[i].gear_ratio=200;
- manager_servos[i].max_angle=3600;
- manager_servos[i].center_angle=1800;
+ manager_servos[i].max_angle=360<<7;
+ manager_servos[i].center_angle=180<<7;
manager_servos[i].max_speed=378;
break;
case SERVO_EX106: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=184;
- manager_servos[i].max_angle=2500;
- manager_servos[i].center_angle=1250;
+ manager_servos[i].max_angle=250<<7;
+ manager_servos[i].center_angle=125<<7;
manager_servos[i].max_speed=414;
break;
case SERVO_MX106: manager_servos[i].encoder_resolution=4095;
manager_servos[i].gear_ratio=225;
- manager_servos[i].max_angle=3600;
- manager_servos[i].center_angle=1800;
+ manager_servos[i].max_angle=360<<7;
+ manager_servos[i].center_angle=180<<7;
manager_servos[i].max_speed=270;
break;
default: break;
@@ -170,6 +193,8 @@ void manager_init(uint16_t period_us)
// get the servo's current position
dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
manager_num_servos++;
}
else
@@ -202,7 +227,6 @@ void manager_init(uint16_t period_us)
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure);
- TIM_Cmd(MOTION_TIMER, ENABLE);
TIM_SetClockDivision(MOTION_TIMER,TIM_CKD_DIV1);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
@@ -212,9 +236,8 @@ void manager_init(uint16_t period_us)
TIM_OCInitStructure.TIM_Pulse = manager_motion_period;
TIM_OC1Init(MOTION_TIMER, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(MOTION_TIMER, TIM_OCPreload_Disable);
- TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE);
- manager_enabled=0x00;
+ ram_clear_bit(BIOLOID_MM_STATUS,0);
}
uint16_t manager_get_period(void)
@@ -231,19 +254,19 @@ inline void manager_enable(void)
{
TIM_Cmd(MOTION_TIMER, ENABLE);
TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE);
- manager_enabled=0x01;
+ ram_set_bit(BIOLOID_MM_STATUS,0);
}
inline void manager_disable(void)
{
TIM_Cmd(MOTION_TIMER, DISABLE);
TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, DISABLE);
- manager_enabled=0x00;
+ ram_clear_bit(BIOLOID_MM_STATUS,0);
}
inline uint8_t manager_is_enabled(void)
{
- return manager_enabled;
+ return ram_data[BIOLOID_MM_STATUS]&0x01;
}
inline uint8_t manager_get_num_servos(void)