From 118b8bffd6326022d1c1a2c5bf3dea0df926d544 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Wed, 16 Apr 2014 08:19:00 +0000
Subject: [PATCH] Added function to the motion mamager to transform between
physical magnitudes and controller values, and viceversa. Added functions to
scan and identify the servos in the initialization functions. Minor changes
in the motion files. Added a header file with the dynamixel servo register
map.
---
include/dyn_servos.h | 91 +++++++++++++++++++++
include/motion_manager.h | 21 +++++
src/bioloid_stm32.c | 11 +--
src/motion_manager.c | 168 ++++++++++++++++++++++++++++++++++++---
4 files changed, 273 insertions(+), 18 deletions(-)
create mode 100644 include/dyn_servos.h
diff --git a/include/dyn_servos.h b/include/dyn_servos.h
new file mode 100644
index 0000000..775f26d
--- /dev/null
+++ b/include/dyn_servos.h
@@ -0,0 +1,91 @@
+#ifndef _DYN_SERVOS_H
+#define _DYN_SERVOS_H
+
+// servo models
+#define SERVO_AX12A 0x000C
+#define SERVO_AX12W 0x012C
+#define SERVO_AX18A 0x0012
+#define SERVO_MX28 0x001D
+#define SERVO_RX24F 0x0018
+#define SERVO_RX28 0x001C
+#define SERVO_RX64 0x0040
+#define SERVO_MX64 0x0136
+#define SERVO_EX106 0x006B
+#define SERVO_MX106 0x0140
+
+// Servo register map
+typedef enum{
+ P_MODEL_NUMBER_L = 0,
+ P_MODEL_NUMBER_H = 1,
+ P_VERSION = 2,
+ P_ID = 3,
+ P_BAUD_RATE = 4,
+ P_RETURN_DELAY_TIME = 5,
+ P_CW_ANGLE_LIMIT_L = 6,
+ P_CW_ANGLE_LIMIT_H = 7,
+ P_CCW_ANGLE_LIMIT_L = 8,
+ P_CCW_ANGLE_LIMIT_H = 9,
+ P_SYSTEM_DATA2 = 10,
+ P_HIGH_LIMIT_TEMPERATURE = 11,
+ P_LOW_LIMIT_VOLTAGE = 12,
+ P_HIGH_LIMIT_VOLTAGE = 13,
+ P_MAX_TORQUE_L = 14,
+ P_MAX_TORQUE_H = 15,
+ P_RETURN_LEVEL = 16,
+ P_ALARM_LED = 17,
+ P_ALARM_SHUTDOWN = 18,
+ P_OPERATING_MODE = 19,
+ P_LOW_CALIBRATION_L = 20,
+ P_LOW_CALIBRATION_H = 21,
+ P_HIGH_CALIBRATION_L = 22,
+ P_HIGH_CALIBRATION_H = 23,
+ P_TORQUE_ENABLE = 24,
+ P_LED = 25,
+ P_CW_COMPLIANCE_MARGIN = 26,
+ P_CCW_COMPLIANCE_MARGIN = 27,
+ P_CW_COMPLIANCE_SLOPE = 28,
+ P_CCW_COMPLIANCE_SLOPE = 29,
+ P_D_GAIN = 26,
+ P_I_GAIN = 27,
+ P_P_GAIN = 28,
+ P_RESERVED = 29,
+ P_GOAL_POSITION_L = 30,
+ P_GOAL_POSITION_H = 31,
+ P_MOVING_SPEED_L = 32,
+ P_MOVING_SPEED_H = 33,
+ P_TORQUE_LIMIT_L = 34,
+ P_TORQUE_LIMIT_H = 35,
+ P_PRESENT_POSITION_L = 36,
+ P_PRESENT_POSITION_H = 37,
+ P_PRESENT_SPEED_L = 38,
+ P_PRESENT_SPEED_H = 39,
+ P_PRESENT_LOAD_L = 40,
+ P_PRESENT_LOAD_H = 41,
+ P_PRESENT_VOLTAGE = 42,
+ P_PRESENT_TEMPERATURE = 43,
+ P_REGISTERED_INSTRUCTION = 44,
+ P_PAUSE_TIME = 45,
+ P_MOVING = 46,
+ P_LOCK = 47,
+ P_PUNCH_L = 48,
+ P_PUNCH_H = 49,
+ P_RESERVED4 = 50,
+ P_RESERVED5 = 51,
+ P_POT_L = 52,
+ P_POT_H = 53,
+ P_PWM_OUT_L = 54,
+ P_PWM_OUT_H = 55,
+ P_P_ERROR_L = 56,
+ P_P_ERROR_H = 57,
+ P_I_ERROR_L = 58,
+ P_I_ERROR_H = 59,
+ P_D_ERROR_L = 60,
+ P_D_ERROR_H = 61,
+ P_P_ERROR_OUT_L = 62,
+ P_P_ERROR_OUT_H = 63,
+ P_I_ERROR_OUT_L = 64,
+ P_I_ERROR_OUT_H = 65,
+ P_D_ERROR_OUT_L = 66,
+ P_D_ERROR_OUT_H = 67}TDynServo;
+
+#endif
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 2b03a3e..cce7d6f 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -3,6 +3,27 @@
#include "stm32f4xx.h"
+#define MAX_NUM_SERVOS 254
+
+typedef enum{MM_NONE = 0,
+ MM_ACTION = 1,
+ MM_WALKING = 2,
+ MM_JOINTS = 3} TModules;
+
+// servo information structure
+typedef struct{
+ uint8_t id;
+ uint16_t model;
+ uint16_t encoder_resolution;
+ uint8_t gear_ratio;
+ int16_t max_angle;
+ int16_t center_angle;
+ uint16_t max_speed;
+ int16_t current_angle;
+ uint16_t current_value;
+ TModules module;
+}TServoInfo;
+
// public functions
void manager_init(uint16_t period_us);
inline uint16_t manager_get_period(void);
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 8e1d619..84ab749 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -43,10 +43,11 @@ int32_t main(void)
uint8_t status_packet[1024];
uint8_t data[1024],error;
uint16_t address;
- TPage page;
/* initialize EEPROM */
EE_Init();
+ // initialize the Dynamixel RAM memory space
+ ram_init();
/* initialize the 1ms system timer */
time_init();
/* initialize the dynamixel master interface */
@@ -60,19 +61,11 @@ int32_t main(void)
imu_init();
/* initialize the gpio */
gpio_init();
- // initialize the Dynamixel RAM memory space
- ram_init();
// initialize the Analog to digital converter
adc_init();
// initialize motion manager
manager_init(7800);
-// pages_get_page(0,&page);
- if(motion_pages[0].header.stepnum==0x06)
- gpio_set_led(NORTH_LED);
- else
- gpio_clear_led(NORTH_LED);
-
while(1) /* main function does not return */
{
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
diff --git a/src/motion_manager.c b/src/motion_manager.c
index ab12c5b..b704288 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -1,4 +1,6 @@
#include "motion_manager.h"
+#include "dynamixel_master_uart_dma.h"
+#include "dyn_servos.h"
#include "gpio.h"
#define MOTION_TIMER TIM3
@@ -7,7 +9,49 @@
#define MOTION_TIMER_CLK RCC_APB1Periph_TIM3
// private variables
-__IO uint16_t motion_period;
+__IO uint16_t manager_motion_period;
+uint8_t manager_num_servos;
+TServoInfo manager_servos[MAX_NUM_SERVOS];
+
+// private functions
+void manager_send_motion_command(void)
+{
+ uint8_t servo_ids[MAX_NUM_SERVOS];
+ uint8_t data[2048];
+ uint16_t value;
+ uint8_t i;
+
+ for(i=0;i<manager_num_servos;i++)
+ {
+// servo_ids[i]=manager_servos[i].id;
+// value=manager_get_index_value(i);
+ data[i*2]=value%256;
+ data[i*2+1]=value/256;
+ }
+ dyn_master_sync_write(manager_num_servos,servo_ids,P_GOAL_POSITION_L,2,data);
+}
+
+inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
+{
+ return ((angle+manager_servos[servo_id].center_angle)*manager_servos[servo_id].encoder_resolution)/manager_servos[servo_id].max_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;
+}
+
+inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
+{
+ if(speed>manager_servos[servo_id].max_speed)
+ speed=manager_servos[servo_id].max_speed;
+ return (speed*3)>>1;
+}
+
+inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
+{
+ return (value*2)/3;
+}
// interrupt handlers
void MOTION_TIMER_IRQHandler(void)
@@ -18,22 +62,128 @@ void MOTION_TIMER_IRQHandler(void)
{
TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1);
capture = TIM_GetCapture1(MOTION_TIMER);
- TIM_SetCompare1(MOTION_TIMER, capture + motion_period);
- gpio_toggle_led(EAST_LED);
+ TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
+ // call the action process
+ // action_process();
+ // call the joint motion process
+ // joint_motion_process();
+ // call the walking process
+ // walking_process();
+ // balance the robot
+ // manager_balance();
+ // send the motion commands to the servos
+ manager_send_motion_command();
}
}
-// private functions
-
// public functions
void manager_init(uint16_t period_us)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
+ uint8_t servo_ids[MAX_NUM_SERVOS];
+ uint16_t model;
+ uint8_t i,num;
RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE);
+ // detect the servos connected
+ manager_num_servos=0;
+ dyn_master_scan(&num,servo_ids);
+ for(i=0;i<MAX_NUM_SERVOS;i++)
+ {
+ if(i<num)
+ {
+ // read the model of the i-th device
+ dyn_master_read_word(servo_ids[i],P_MODEL_NUMBER_L,&model);
+ switch(model)
+ {
+ 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_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_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_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_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_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_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_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_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_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_speed=270;
+ break;
+ default: break;
+ }
+ manager_servos[i].id=servo_ids[i];
+ manager_servos[i].model=model;
+ manager_servos[i].module=MM_NONE;
+ // 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);
+ manager_num_servos++;
+ }
+ else
+ {
+ manager_servos[i].id=servo_ids[i];
+ manager_servos[i].model=model;
+ manager_servos[i].module=MM_NONE;
+ manager_servos[i].encoder_resolution=0;
+ manager_servos[i].gear_ratio=0;
+ manager_servos[i].max_angle=0;
+ manager_servos[i].center_angle=0;
+ manager_servos[i].max_speed=0;
+ manager_servos[i].current_value=0;
+ manager_servos[i].current_angle=0;
+ }
+ }
+
// initialize the timer interrupts
NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
@@ -53,8 +203,8 @@ void manager_init(uint16_t period_us)
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- motion_period=7800;
- TIM_OCInitStructure.TIM_Pulse = motion_period;
+ manager_motion_period=7800;
+ 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);
@@ -62,10 +212,10 @@ void manager_init(uint16_t period_us)
uint16_t manager_get_period(void)
{
- return motion_period;
+ return manager_motion_period;
}
void manager_set_period(uint16_t period_us)
{
- motion_period=period_us;
+ manager_motion_period=period_us;
}
--
GitLab