From 608783be63eac9364fe71a04d8d22ae38c554f8b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Mon, 11 Apr 2016 22:56:50 +0000
Subject: [PATCH] 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 | 8 ++++----
controllers/src/adc.c | 1 +
motion/include/motion_cfg.h | 14 +++++++-------
motion/src/action.c | 1 -
motion/src/balance.c | 18 +++++++++---------
motion/src/examples/main.c | 4 ++--
motion/src/motion_manager.c | 9 +++++----
7 files changed, 28 insertions(+), 27 deletions(-)
diff --git a/controllers/include/cm510_cfg.h b/controllers/include/cm510_cfg.h
index da9aad3..cebc829 100644
--- a/controllers/include/cm510_cfg.h
+++ b/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
diff --git a/controllers/src/adc.c b/controllers/src/adc.c
index be4e5ef..d977771 100644
--- a/controllers/src/adc.c
+++ b/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)
diff --git a/motion/include/motion_cfg.h b/motion/include/motion_cfg.h
index f96994c..399c1cf 100644
--- a/motion/include/motion_cfg.h
+++ b/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
diff --git a/motion/src/action.c b/motion/src/action.c
index a15ea0c..bcdc50a 100644
--- a/motion/src/action.c
+++ b/motion/src/action.c
@@ -5,7 +5,6 @@
#include "action.h"
#include "motion_pages.h"
#include "serial_console.h"
-#include <stdio.h>
/**************************************
* Section /----\
diff --git a/motion/src/balance.c b/motion/src/balance.c
index dcc9847..b9e2e3a 100644
--- a/motion/src/balance.c
+++ b/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;
}
diff --git a/motion/src/examples/main.c b/motion/src/examples/main.c
index b219628..fc603d7 100644
--- a/motion/src/examples/main.c
+++ b/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");
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 2c6130e..2f4fe00 100644
--- a/motion/src/motion_manager.c
+++ b/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;
}
--
GitLab