diff --git a/include/dyn_servos.h b/include/dyn_servos.h
new file mode 100644
index 0000000000000000000000000000000000000000..775f26d6ccbede393c8efa80a9da52f2c6c558e9
--- /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 2b03a3ea635a64a67e7dba8734044d625588a3ea..cce7d6f3b0eddcfdff64402d330981f1faf5ae56 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 8e1d619ea94570ab701733cfef9afa469d49dad9..84ab749109bf0cae40d07a2466a802f4ec2118f7 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 ab12c5b7b30af142c45c35e1dcbe4b1038a7b6ec..b7042885f285fab8459a97a878019ce110ca5f47 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;
}