diff --git a/include/action.h b/include/action.h
index 27d57d2a05c2391e0d566de5febf8fd4eb36cff4..6270491fb1cb641ce016ac626fb5a940e121d009 100644
--- a/include/action.h
+++ b/include/action.h
@@ -5,6 +5,7 @@
#include "motion_pages.h"
extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
+extern uint8_t action_slope[PAGE_MAX_NUM_SERVOS];
// public functions
void action_init(uint16_t period);
diff --git a/include/adc_dma.h b/include/adc_dma.h
index c2bdcdde536f2e09e6a399dd36d365e32e0cf796..5a67f14a4d9da07200c5242f41885908ac955485 100644
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -1,4 +1,4 @@
-#ifndef _ADC_DAM_H
+#ifndef _ADC_DMA_H
#define _ADC_DMA_H
#include "stm32f4xx.h"
diff --git a/include/motion_pages.h b/include/motion_pages.h
index f9acc7b439e51a09a38f15839563ba3f1ffa0e49..cac9f755ab14ae48c278ad5a51b50bf2c05e3bb2 100644
--- a/include/motion_pages.h
+++ b/include/motion_pages.h
@@ -47,5 +47,6 @@ void pages_get_page(uint8_t page_id,TPage *page);
uint8_t pages_check_checksum(TPage *page);
void pages_clear_page(TPage *page);
void pages_copy_page(TPage *src,TPage *dst);
+inline uint8_t pages_get_slope(TPage *page,uint8_t servo_id);
#endif
diff --git a/include/ram.h b/include/ram.h
index 9184286b1e881610d79922a826e8ea5fb612aced..485bf26b1bd8aebb1aeebd3a517eec875c457763 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -126,7 +126,15 @@ typedef enum {
BIOLOID_IMU_TEMP = 0x80,
BIOLOID_ACTION_PAGE = 0x81,
BIOLOID_ACTION_CONTROL = 0x82,
- BIOLOID_ACTION_STATUS = 0x83
+ BIOLOID_ACTION_STATUS = 0x83,
+ BIOLOID_MM_PELVIS_ROLL_L = 0x84,
+ BIOLOID_MM_PELVIS_ROLL_R = 0x85,
+ BIOLOID_MM_ANKLE_ROLL_L = 0x86,
+ BIOLOID_MM_ANKLE_ROLL_R = 0x87,
+ BIOLOID_MM_ANKLE_PITCH_L = 0x88,
+ BIOLOID_MM_ANKLE_PITCH_R = 0x89,
+ BIOLOID_MM_KNEE_L = 0x8A,
+ BIOLOID_MM_KNEE_R = 0x8B
} bioloid_registers;
#define RAM_SIZE 256
diff --git a/src/action.c b/src/action.c
index 70b69b6c896792f2b970de7cf4019e2add8d855a..2cb59bccf24b3137f2535cc8409bbb1ebf6fd413 100644
--- a/src/action.c
+++ b/src/action.c
@@ -22,6 +22,7 @@ uint8_t action_end,action_stop;
uint8_t action_zero_speed_finish[PAGE_MAX_NUM_SERVOS];
uint8_t action_next_index;
uint8_t action_running;
+uint8_t action_slope[PAGE_MAX_NUM_SERVOS];// fixed point 0|8 format
// time variables (in time units (7.8 ms each time unit))
int64_t action_total_time;// fixed point 48|16 format
int64_t action_pre_time;// fixed point 48|16 format
@@ -86,6 +87,8 @@ void action_load_next_step(void)
}
}
pages_copy_page(&action_next_page,&action_current_page);// make the next page active
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ action_slope[i]=pages_get_slope(&action_current_page,i);
if(current_index!=action_next_index)
num_repetitions=action_current_page.header.repeat;
action_current_step_index=0;
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 8b3a637776ac9dac000e7856b5804eeb103102a6..c8e40404b304635709b410560b313fbe0d0e5d1f 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -28,13 +28,15 @@ void manager_send_motion_command(void)
if(manager_servos[i].enabled)
{
servo_ids[num]=manager_servos[i].id;
- data[num*2]=manager_servos[i].current_value%256;
- data[num*2+1]=manager_servos[i].current_value/256;
+ data[num*4]=action_slope[i+1];
+ data[num*4+1]=action_slope[i+1];
+ data[num*4+2]=manager_servos[i].current_value%256;
+ data[num*4+3]=manager_servos[i].current_value/256;
num++;
}
}
if(num>0)
- dyn_master_sync_write(num,servo_ids,P_GOAL_POSITION_L,2,data);
+ dyn_master_sync_write(num,servo_ids,P_CW_COMPLIANCE_SLOPE,4,data);
}
inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
@@ -95,22 +97,17 @@ void manager_get_target_position(void)
void manager_balance(void)
{
- double x_error=-msg->angular_velocity.x;
- double y_error=-msg->angular_velocity.z;
- double gain=3.0;
-
- balance_pelvis_roll_left=gain*(((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/40.0)*300)/1024);
-
-
-
- balance_pelvis_roll_right=gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/40.0)*300)/1024)*3.14159)/180.0;
- balance_ankle_roll_left=-gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/20.0)*300)/1024)*3.14159)/180.0;
- balance_ankle_roll_right=-gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/20.0)*300)/1024)*3.14159)/180.0;
-
- balance_ankle_pitch_left=-gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/18.0)*300)/1024)*3.14159)/180.0;
- balance_ankle_pitch_right=gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/18.0)*300)/1024)*3.14159)/180.0;
- balance_knee_left=-gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/54.0)*300)/1024)*3.14159)/180.0;
- balance_knee_right=gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/54.0)*300)/1024)*3.14159)/180.0;
+ int16_t x_error=0.0;
+ int16_t y_error=0.0;
+
+ manager_servos[10].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_L]*11)/2500;
+ manager_servos[9].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_R]*11)/2500;
+ manager_servos[18].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_L]*11)/2500;
+ manager_servos[17].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_R]*11)/2500;
+ manager_servos[16].current_value-=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_L]*11)/2500;
+ manager_servos[15].current_value+=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_R]*11)/2500;
+ manager_servos[14].current_value-=(x_error*ram_data[BIOLOID_MM_KNEE_L]*11)/2500;
+ manager_servos[13].current_value+=(x_error*ram_data[BIOLOID_MM_KNEE_R]*11)/2500;
}
// interrupt handlers
@@ -129,10 +126,10 @@ void MOTION_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();
// get the disabled servos position
diff --git a/src/motion_pages.c b/src/motion_pages.c
index 77bf601c1f1f0a9dfe8c2af6c7d0e569cb03b41d..afc0e0e762018842707cc128afc2407874ee1860 100644
--- a/src/motion_pages.c
+++ b/src/motion_pages.c
@@ -37,6 +37,11 @@ void pages_copy_page(TPage *src,TPage *dst)
((uint8_t *)dst)[i]=((uint8_t *)src)[i];
}
+inline uint8_t pages_get_slope(TPage *page,uint8_t servo_id)
+{
+ return 0x01<<(page->header.slope[servo_id]&0x0F);
+}
+
TPage motion_pages[MAX_PAGES] __attribute__ ((section (".pages")))=
{
{