Added the action module to the motion manager.

Makefile

@@ -17,6 +17,7 @@ TARGET_FILES+=src/bioloid_dyn_master_sensors.c
 TARGET_FILES+=src/bioloid_dyn_master_servos.c
 TARGET_FILES+=src/motion_pages.c
 TARGET_FILES+=src/motion_manager.c
+TARGET_FILES+=src/action.c
 TARGET_PROCESSOR=STM32F407VG
 
 HAL_PATH=../../STM32_processor/hal/f4

include/action.h

@@ -19,7 +19,9 @@ uint8_t action_load_page(uint8_t page_id);
 void action_start_page(void);
 void action_stop_page(void);
 uint8_t action_is_running(void);
-
+// operation functions
+uint8_t action_in_range(unsigned short int address, unsigned short int length);
+void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
 // motion manager interface
 void action_process(void);
 

include/bioloid_registers.h

@@ -177,8 +177,10 @@ typedef enum {
   BIOLOID_MM_SERVO30_CUR_POS_L    = 0x96,
   BIOLOID_MM_SERVO30_CUR_POS_H    = 0x97,
   BIOLOID_MM_SERVO31_CUR_POS_L    = 0x98,
-  BIOLOID_MM_SERVO31_CUR_POS_H    = 0x99
-
+  BIOLOID_MM_SERVO31_CUR_POS_H    = 0x99,
+  BIOLOID_ACTION_PAGE             = 0x9A,
+  BIOLOID_ACTION_CNTRL            = 0x9B,
+  BIOLOID_ACTION_STATUS           = 0x9C
 } bioloid_registers;
 
 #define      GPIO_BASE_ADDRESS    0x20
@@ -199,6 +201,9 @@ typedef enum {
 #define      MANAGER_BASE_ADDRESS 0x48
 #define      MANAGER_MEM_LENGTH   50
 
+#define      ACTION_BASE_ADDRESS  0x9A
+#define      ACTION_MEM_LENGTH    3
+
 #ifdef __cplusplus
 }
 #endif

include/motion_manager.h

@@ -33,7 +33,7 @@ typedef struct{
 }TServoInfo;
 
 // public variables
-extern int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
 
 // public functions
 void manager_init(uint16_t period_us);

src/bioloid_dyn_slave.c

@@ -7,6 +7,7 @@
 #include "adc_dma.h"
 #include "zigbee.h"
 #include "motion_manager.h"
+#include "action.h"
 
 /* external interrupt pin */
 #define     DYN_SLAVE_EXT_INT_PIN             GPIO_PIN_8
@@ -78,6 +79,9 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
   // Manager commands
   if(manager_in_range(address,length))
     manager_process_write_cmd(address,length,data);
+  // action commands
+  if(action_in_range(address,length))
+    action_process_write_cmd(address,length,data);
   // write eeprom
   for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
     EE_WriteVariable(i,data[j]);

src/motion_manager.c

@@ -3,6 +3,7 @@
 #include "dyn_servos.h"
 #include "ram.h"
 #include "gpio.h"
+#include "action.h"
 
 #define MANAGER_TIMER                   TIM3
 #define ENABLE_MANAGER_TIMER_CLK        __HAL_RCC_TIM3_CLK_ENABLE()
@@ -69,13 +70,13 @@ void manager_get_current_position(void)
 
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
-    if(!manager_servos[i].enabled)// servo is not enabled
+    if(!manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is not enabled but it is present
     {
       dyn_master_read_word(&bioloid_dyn_master_servos,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);
     }
-    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);
   }
 }
 
@@ -85,13 +86,15 @@ void manager_get_target_position(void)
 
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
   {
-    if(manager_servos[i].enabled)// servo is enabled
+    if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is enabled and present
     {
       if(manager_servos[i].module==MM_ACTION)
       {
         manager_servos[i].current_angle=((manager_current_angles[i]+manager_balance_offset[i])>>9);
         //>>16 from the action codification, <<7 from the manager codification
         manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
+        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);
       }
     }
   }
@@ -114,20 +117,19 @@ void MANAGER_TIMER_IRQHandler(void)
       capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
       __HAL_TIM_SET_COMPARE(&MANAGER_TIM_Handle, TIM_CHANNEL_1, (capture + manager_motion_period));
       // call the action process
-      // action_process();
+      action_process();
       // call the joint motion process
       // joint_motion_process();
       // call the walking process
       // walking_process();
       // get the target angles from all modules
-      // manager_get_target_position();
+      manager_get_target_position();
       // balance the robot
       // manager_balance();
       // send the motion commands to the servos
-      // manager_send_motion_command();
+      manager_send_motion_command();
       // get the disabled servos position
       // manager_get_current_position();
-      gpio_toggle_led(RXD_LED);
     }
   }
 }
@@ -283,6 +285,11 @@ void manager_init(uint16_t period_us)
   for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
     manager_balance_offset[i]=0;
   manager_balance_enabled=0x00;
+
+  /* initialize action module */
+  action_init(period_us);
+
+  gpio_blink_led(RXD_LED,1000);
 }
 
 uint16_t manager_get_period(void)