Solved a problem with the balance gains (they were not correctly set).

Added the balance_init() function to the manager_init() function.
Changed the name of some functions to make them coincide with their definition in the balance module.

controllers/include/cm510_cfg.h

@@ -33,9 +33,9 @@
 #define BALANCE_MAX_CAL_GYRO_ERROR              20.0
 #define BALANCE_FORWARD_FALL_THD_VALUE          (-200)
 #define BALANCE_BACKWARD_FALL_THD_VALUE         200
-#define BALANCE_KNEE_GAIN                       (4.0/54.0)
-#define BALANCE_ANKLE_ROLL_GAIN                 (4.0/18.0)
-#define BALANCE_HIP_PITCH_GAIN                  (4.0/20.0)
-#define BALANCE_ANKLE_PITCH_GAIN                (4.0/40.0)
+#define BALANCE_KNEE_GAIN                       (2.0/54.0)
+#define BALANCE_ANKLE_PITCH_GAIN                (2.0/18.0)
+#define BALANCE_ANKLE_ROLL_GAIN                 (2.0/20.0)
+#define BALANCE_HIP_ROLL_GAIN                   (2.0/40.0)
 
 #endif

controllers/src/adc.c

@@ -143,6 +143,7 @@ void init_adc(void)
       adc_ch_data[i][j]=0x0266;
   }
   adc_current_ch=0;
+  adc_current_sample=0;
   adc_voltage_alarm=0x00;
 
   // configure the timer 2 to perform periodic conversions (1 ms period)

motion/include/motion_cfg.h

@@ -23,7 +23,7 @@
   #define BALANCE_GYRO_CAL_NUM_SAMPLES            10
 #endif 
 
-#ifndef BALANCE_GYRO_X_CHANNHEL
+#ifndef BALANCE_GYRO_X_CHANNEL
   #define BALANCE_GYRO_X_CHANNEL                  3
 #endif 
 
@@ -56,19 +56,19 @@
 #endif
 
 #ifndef BALANCE_KNEE_GAIN
-  #define BALANCE_KNEE_GAIN                       (4.0/54.0)
+  #define BALANCE_KNEE_GAIN                       (2.0/54.0)
 #endif
 
 #ifndef BALANCE_ANKLE_ROLL_GAIN
-  #define BALANCE_ANKLE_ROLL_GAIN                 (4.0/18.0)
+  #define BALANCE_ANKLE_PITCH_GAIN                (2.0/18.0)
 #endif
 
-#ifndef BALANCE_HIP_PITCH_GAIN
-  #define BALANCE_HIP_PITCH_GAIN                  (4.0/20.0)
+#ifndef BALANCE_ANKLE_ROLL_GAIN
+  #define BALANCE_ANKLE_ROLL_GAIN                 (2.0/20.0)
 #endif
 
-#ifndef BALANCE_ANKLE_PITCH_GAIN
-  #define BALANCE_ANKLE_PITCH_GAIN                (4.0/40.0)
+#ifndef BALANCE_HIP_ROLL_GAIN
+  #define BALANCE_HIP_ROLL_GAIN                   (2.0/40.0)
 #endif
 
 #endif

motion/src/action.c

@@ -5,7 +5,6 @@
 #include "action.h"
 #include "motion_pages.h"
 #include "serial_console.h"
-#include <stdio.h>
 
 /**************************************
  * Section             /----\

motion/src/balance.c

@@ -38,15 +38,15 @@ void balance_loop(void)
 
     float x_error1,x_error2,y_error1,y_error2;
     x_error1=gyro_x*BALANCE_KNEE_GAIN;//4.0/54.0;
-    x_error2=gyro_x*BALANCE_ANKLE_ROLL_GAIN;//4.0/18.0;
-    y_error1=gyro_y*BALANCE_HIP_PITCH_GAIN;//4.0/20.0;
-    y_error2=gyro_y*BALANCE_ANKLE_PITCH_GAIN;//4.0/40.0;
+    x_error2=gyro_x*BALANCE_ANKLE_PITCH_GAIN;//4.0/18.0;
+    y_error1=gyro_y*BALANCE_ANKLE_ROLL_GAIN;//4.0/20.0;
+    y_error2=gyro_y*BALANCE_HIP_ROLL_GAIN;//4.0/40.0;
 
     balance_offsets[8] = (uint16_t)y_error1;
     balance_offsets[9] = (uint16_t)y_error1;
-    balance_offsets[12] = (uint16_t)x_error1;//-1.0;
-    balance_offsets[13] = (uint16_t)-x_error1;//+1.0;
-    balance_offsets[14] = (uint16_t)+x_error2;
+    balance_offsets[12] = (uint16_t)x_error1;
+    balance_offsets[13] = (uint16_t)-x_error1;
+    balance_offsets[14] = (uint16_t)x_error2;
     balance_offsets[15] = (uint16_t)-x_error2;
     balance_offsets[16] = (uint16_t)-y_error2;
     balance_offsets[17] = (uint16_t)-y_error2;
@@ -126,17 +126,17 @@ uint8_t balance_calibrate_gyro(void)
   return 0x01;
 }
 
-void balance_enable(void)
+void balance_enable_gyro(void)
 {
   balance_enabled=0x01;
 }
 
-void balance_disable(void)
+void balance_disable_gyro(void)
 {
   balance_enabled=0x00;
 }
 
-uint8_t balance_is_enabled(void)
+uint8_t balance_is_gyro_enabled(void)
 {
   return balance_enabled;
 }

motion/src/examples/main.c

@@ -14,7 +14,7 @@ void user_loop(void)
     if(is_action_running()==0x00)
     {
       printf("Walk ready\n");
-      if(action_set_page(1)==0)
+      if(action_set_page(25)==0)
         action_start_page();
       else
         printf("Error loading page\n");
@@ -25,7 +25,7 @@ void user_loop(void)
     if(is_action_running()==0x00)
     {
       printf("start walking\n");
-      if(action_set_page(3)==0)
+      if(action_set_page(26)==0)
         action_start_page();
       else
         printf("Error loading page\n");

motion/src/motion_manager.c

@@ -10,7 +10,6 @@
 #include "action.h"
 #include "balance.h"
 #include "buzzer.h"
-#include <stdio.h>
 
 // external functions
 extern void buzzer_start_alarm(note_t note,uint16_t on_time_100ms,uint16_t off_time_100ms);
@@ -169,7 +168,7 @@ void manager_loop(void)
     // call the action process
     action_process(); //action_ready
     // balance the robot
-//    balance_loop();
+    balance_loop();
     // send the motion commands to the servos
     manager_send_motion_command();
   }
@@ -207,15 +206,17 @@ uint8_t manager_init(uint8_t num_servos)
     }
   } 
 
-/*  if(num_servos != manager_num_servos)
+  if(num_servos != manager_num_servos)
     buzzer_start_alarm(MANAGER_MISSING_SERVOS_ALARM_NOTE,MANAGER_MISSING_SERVOS_ALARM_TIME_ON,MANAGER_MISSING_SERVOS_ALARM_TIME_OFF);
   else
-    buzzer_stop_alarm();*/
+    buzzer_stop_alarm();
 
   /* initialize the period timer */
   manager_timer_init();
   /* initialize the action module */
   action_init();
+  /* initialize the balance module */
+  balance_init();
 
   return num;
 }