Solved some minor bugs related to the balance feature.

include/motion_manager.h

@@ -7,10 +7,31 @@ extern "C" {
 
 #include "stm32f4xx_hal.h"
 
-typedef enum{MM_NONE          = 0,
-             MM_ACTION        = 1,
-             MM_WALKING       = 2,
-             MM_JOINTS        = 3} TModules;
+typedef enum {
+  R_SHOULDER_PITCH     = 1,
+  L_SHOULDER_PITCH     = 2,
+  R_SHOULDER_ROLL      = 3,
+  L_SHOULDER_ROLL      = 4,
+  R_ELBOW              = 5,
+  L_ELBOW              = 6,
+  R_HIP_YAW            = 7,
+  L_HIP_YAW            = 8,
+  R_HIP_ROLL           = 9,
+  L_HIP_ROLL           = 10,
+  R_HIP_PITCH          = 11,
+  L_HIP_PITCH          = 12,
+  R_KNEE               = 13,
+  L_KNEE               = 14,
+  R_ANKLE_PITCH        = 15,
+  L_ANKLE_PITCH        = 16,
+  R_ANKLE_ROLL         = 17,
+  L_ANKLE_ROLL         = 18} servo_id_t;
+
+
+typedef enum {MM_NONE          = 0,
+              MM_ACTION        = 1,
+              MM_WALKING       = 2,
+              MM_JOINTS        = 3} TModules;
 
 typedef struct
 {

src/bioloid_gyro.c

@@ -36,6 +36,8 @@ void gyro_calibrate(void)
 
   gyro_fb_offset=0x0000;
   gyro_lr_offset=0x0000;
+  // dummy delay to allow enough time to the ADC to get new data
+  HAL_Delay(2*adc_get_period());    
   for(i=0;i<GYRO_MAX_CAL_SAMPLES;i++)
   {
     gyro_fb_offset+=adc_get_channel_raw(fb_ch);

src/eeprom.c

@@ -42,19 +42,24 @@
 /* Includes ------------------------------------------------------------------*/
 #include "eeprom.h"
 #include "bioloid_registers.h"
+#include "adc_dma.h"
 
 /* Private typedef -----------------------------------------------------------*/
 /* Private define ------------------------------------------------------------*/
-#define    EEPROM_SIZE               0x09
-#define    DEFAULT_DEVICE_MODEL      0x7300
-#define    DEFAULT_FIRMWARE_VERSION  0x0001
-#define    DEFAULT_DEVICE_ID         0x0002
-#define    DEFAULT_BAUDRATE          0x0001
-#define    DEFAULT_RETURN_DELAY      0x0000
-#define    DEFAULT_MM_PERIOD         0x01FF
-#define    DEFAULT_GYRO_FB_ADC_CH    0x0002
-#define    DEFAULT_GYRO_LR_ADC_CH    0x0001
-#define    DEFAULT_RETURN_LEVEL      0x0002
+#define    EEPROM_SIZE                      0x20
+#define    DEFAULT_DEVICE_MODEL             0x7300
+#define    DEFAULT_FIRMWARE_VERSION         0x0001
+#define    DEFAULT_DEVICE_ID                0x0002
+#define    DEFAULT_BAUDRATE                 0x0001
+#define    DEFAULT_RETURN_DELAY             0x0000
+#define    DEFAULT_MM_PERIOD                0x01FF
+#define    DEFAULT_BAL_KNEE_GAIN            0x04BE // 1/54 in fixed point format 0|16
+#define    DEFAULT_BAL_ANKLE_ROLL_GAIN      0x0CCD // 1/20
+#define    DEFAULT_BAL_ANKLE_PITCH_GAIN     0x0E39 // 1/18
+#define    DEFAULT_BAL_HIP_ROLL_GAIN        0x0666 // 1/40
+#define    DEFAULT_GYRO_FB_ADC_CH           (uint16_t)ADC_CH1
+#define    DEFAULT_GYRO_LR_ADC_CH           (uint16_t)ADC_CH2
+#define    DEFAULT_RETURN_LEVEL             0x0002
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
 
@@ -80,12 +85,28 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
                                                               MM_PERIOD_OFFSET,
                                                               DEFAULT_MM_PERIOD>>8,
                                                               MM_PERIOD_OFFSET+1,
+                                                              DEFAULT_BAL_KNEE_GAIN&0xFF,
+                                                              MM_BAL_KNEE_GAIN_OFFSET,
+                                                              DEFAULT_BAL_KNEE_GAIN>>8,
+                                                              MM_BAL_KNEE_GAIN_OFFSET+1,
+                                                              DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF,
+                                                              MM_BAL_ANKLE_ROLL_GAIN_OFFSET,
+                                                              DEFAULT_BAL_ANKLE_ROLL_GAIN>>8,
+                                                              MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
+                                                              DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF,
+                                                              MM_BAL_ANKLE_PITCH_GAIN_OFFSET,
+                                                              DEFAULT_BAL_ANKLE_PITCH_GAIN>>8,
+                                                              MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
+                                                              DEFAULT_BAL_HIP_ROLL_GAIN&0xFF,
+                                                              MM_BAL_HIP_ROLL_GAIN_OFFSET,
+                                                              DEFAULT_BAL_HIP_ROLL_GAIN>>8,
+                                                              MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
+                                                              DEFAULT_RETURN_LEVEL,
+                                                              RETURN_LEVEL_OFFSET,
                                                               DEFAULT_GYRO_FB_ADC_CH,
                                                               GYRO_FB_ADC_CH_OFFSET,
                                                               DEFAULT_GYRO_LR_ADC_CH,
-                                                              GYRO_LR_ADC_CH_OFFSET,
-                                                              DEFAULT_RETURN_LEVEL,
-                                                              RETURN_LEVEL_OFFSET};// return level
+                                                              GYRO_LR_ADC_CH_OFFSET};// return level
 
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/

src/motion_manager.c

@@ -106,25 +106,31 @@ void manager_get_target_position(void)
 void manager_balance(void)
 {
   adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel();
+  int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
   uint16_t fb_offset,lr_offset;
   int16_t fb_value,lr_value;
 
   if(manager_balance_enabled==0x01)// balance is enabled
   {
+    // get the balance gains
+    knee_gain=ram_data[BIOLOID_MM_BAL_KNEE_GAIN_L]+(ram_data[BIOLOID_MM_BAL_KNEE_GAIN_H]*256);
+    ankle_roll_gain=ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H]<<8);
+    ankle_pitch_gain=ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H]<<8);
+    hip_roll_gain=ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_H]<<8);
     // get the offsets of the gyroscope calibration
     gyro_get_offsets(&fb_offset,&lr_offset);
     // get the values of the gyroscope
     fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
     lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
     // compensate the servo angle values
-    manager_balance_offset[13]=(fb_value*manager_servos[13].max_angle)/(manager_servos[13].encoder_resolution*54);
-    manager_balance_offset[15]=(fb_value*manager_servos[15].max_angle)/(manager_servos[15].encoder_resolution*18);    
-    manager_balance_offset[14]=-(fb_value*manager_servos[14].max_angle)/(manager_servos[14].encoder_resolution*54);    
-    manager_balance_offset[16]=-(fb_value*manager_servos[16].max_angle)/(manager_servos[16].encoder_resolution*18);
-    manager_balance_offset[9]=(lr_value*manager_servos[9].max_angle)/(manager_servos[9].encoder_resolution*40);
-    manager_balance_offset[10]=(lr_value*manager_servos[10].max_angle)/(manager_servos[10].encoder_resolution*40);
-    manager_balance_offset[17]=-(lr_value*manager_servos[17].max_angle)/(manager_servos[17].encoder_resolution*20);
-    manager_balance_offset[18]=-(lr_value*manager_servos[18].max_angle)/(manager_servos[18].encoder_resolution*20);
+    manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)/65536;
+    manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;    
+    manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)/65536;    
+    manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
+    manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
+    manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
+    manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
+    manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
   }
 }
 
@@ -456,7 +462,7 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
 uint8_t manager_in_range(unsigned short int address, unsigned short int length)
 {
   if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) ||
-     ram_in_window(BIOLOID_MM_PERIOD_L,BIOLOID_MM_PERIOD_H,address,length))
+     ram_in_window(BIOLOID_MM_PERIOD_L,MANAGER_EEPROM_LENGTH,address,length))
     return 0x01;
   else
     return 0x00;

src/ram.c

@@ -26,8 +26,28 @@ void ram_init(void)
     ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
     ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0)
+    ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0)
+    ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0)
+    ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
+    ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0)
+    ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0)
+    ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0)
+    ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
+    ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
   if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
     ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data;
+  if(EE_ReadVariable(GYRO_FB_ADC_CH_OFFSET,&eeprom_data)==0)
+    ram_data[GYRO_FB_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+  if(EE_ReadVariable(GYRO_LR_ADC_CH_OFFSET,&eeprom_data)==0)
+    ram_data[GYRO_LR_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
 }
 
 inline void ram_read_byte(uint8_t address,uint8_t *data)