Added the slope information to the data send to the servos.

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);

include/adc_dma.h

-#ifndef _ADC_DAM_H
+#ifndef _ADC_DMA_H
 #define _ADC_DMA_H
 
 #include "stm32f4xx.h"

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

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

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;

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

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")))=
 {
   {