Skip to content
Snippets Groups Projects
Commit 7dc5c36f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved a problem in the I2C write operation (the last byte was not transmited). 

Fully implemented an tested the accelerometer.
IMU integrated into the dynamixel interface.
parent 9c0236ac
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/imu_9dof_dma.h
+ 34
− 38
View file @ 7dc5c36f
......@@ -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);
......
include/motion_manager.h
+ 2
− 2
View file @ 7dc5c36f
......@@ -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;
......
include/ram.h
+ 71
− 1
View file @ 7dc5c36f
......@@ -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
......
src/bioloid_stm32.c
+ 15
− 1
View file @ 7dc5c36f
......@@ -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
......
src/imu_9dof_dma.c
+ 309
− 116
View file @ 7dc5c36f
#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);
}
src/motion_manager.c
+ 51
− 28
View file @ 7dc5c36f
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment