From ba9140904e686f00654bf07cd2d3567a9f8f51e1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Sun, 27 Aug 2017 21:54:58 +0200
Subject: [PATCH] First working version of the climb stairs algorithm.
Integrated the stairs algorithm to the motion manager structure. Tuned the
stairs algorithm parameters to work in simulation.
---
Makefile | 1 +
include/darwin_registers.h | 39 +--
include/eeprom.h | 2 +-
include/eeprom_init.h | 29 ++-
src/darwin_dyn_slave.c | 3 +
src/eeprom.c | 4 +-
src/motion_manager.c | 5 +-
src/ram.c | 147 +----------
src/stairs.c | 503 +++++++++++++++++++------------------
9 files changed, 312 insertions(+), 421 deletions(-)
diff --git a/Makefile b/Makefile
index c31048f..6e95e7d 100755
--- a/Makefile
+++ b/Makefile
@@ -29,6 +29,7 @@ TARGET_FILES+=src/joint_motion.c
TARGET_FILES+=src/head_tracking.c
TARGET_FILES+=src/grippers.c
TARGET_FILES+=src/smart_charger.c
+TARGET_FILES+=src/stairs.c
TARGET_PROCESSOR=STM32F103RE
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 4a4684d..f12214f 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -95,15 +95,16 @@ extern "C" {
#define STAIRS_PHASE7_TIME ((unsigned short int)0x0072)
#define STAIRS_PHASE8_TIME ((unsigned short int)0x0074)
#define STAIRS_PHASE9_TIME ((unsigned short int)0x0076)
-#define STAIRS_X_OFFSET ((unsigned short int)0x0078)
-#define STAIRS_Y_OFFSET ((unsigned short int)0x0079)
-#define STAIRS_Z_OFFSET ((unsigned short int)0x007A)
-#define STAIRS_Y_SHIFT ((unsigned short int)0x007B)
-#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007C)
-#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007D)
-#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x007E)
-#define STAIRS_Z_HEIGHT ((unsigned short int)0x007F)
-#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0080)
+#define STAIRS_PHASE10_TIME ((unsigned short int)0x0078)
+#define STAIRS_X_OFFSET ((unsigned short int)0x007A)
+#define STAIRS_Y_OFFSET ((unsigned short int)0x007B)
+#define STAIRS_Z_OFFSET ((unsigned short int)0x007C)
+#define STAIRS_Y_SHIFT ((unsigned short int)0x007D)
+#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007E)
+#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007F)
+#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x0080)
+#define STAIRS_Z_HEIGHT ((unsigned short int)0x0081)
+#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0082)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
@@ -228,6 +229,8 @@ typedef enum {
DARWIN_STAIRS_PHASE8_TIME_H = STAIRS_PHASE8_TIME+1,
DARWIN_STAIRS_PHASE9_TIME_L = STAIRS_PHASE9_TIME,
DARWIN_STAIRS_PHASE9_TIME_H = STAIRS_PHASE9_TIME+1,
+ DARWIN_STAIRS_PHASE10_TIME_L = STAIRS_PHASE10_TIME,
+ DARWIN_STAIRS_PHASE10_TIME_H = STAIRS_PHASE10_TIME+1,
DARWIN_STAIRS_X_OFFSET = STAIRS_X_OFFSET,
DARWIN_STAIRS_Y_OFFSET = STAIRS_Y_OFFSET,
DARWIN_STAIRS_Z_OFFSET = STAIRS_Z_OFFSET,
@@ -236,7 +239,8 @@ typedef enum {
DARWIN_STAIRS_X_RIGHT_SHIFT = STAIRS_X_RIGHT_SHIFT,
DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT,
DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT,
- DARWIN_STAIRS_HIP_PITCH_OFF = STAIRS_HIP_PITCH_OFF,
+ DARWIN_STAIRS_HIP_PITCH_OFF_L = STAIRS_HIP_PITCH_OFF,
+ DARWIN_STAIRS_HIP_PITCH_OFF_H = STAIRS_HIP_PITCH_OFF+1,
//RAM
DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
@@ -626,10 +630,10 @@ typedef enum {
DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x024F,
DARWIN_SMART_CHARGER_CNTRL = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | detected | enable
- DARWIN_GRIPPER_CNTRL = 0x0251 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ DARWIN_GRIPPER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// left opened | right opened | left moving | right moving | close left | open left | close right | open right
- DARWIN_STAIRS_CNTRL = 0x0252, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // current phase | climbing | | | stop stairs | start stairs
+ DARWIN_STAIRS_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // current phase | climbing | | stop stairs | start stairs
}darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100
@@ -734,7 +738,14 @@ typedef enum {
#define SMART_CHARGER_DET 0x01
#define SMART_CHARGER_EN 0x02
-
+#define STAIRS_BASE_ADDRESS 0x0252
+#define STAIRS_MEM_LENGTH 1
+#define STAIRS_EEPROM_ADDRESS 0x0066
+#define STAIRS_EEPROM_LENGTH 30
+#define STAIRS_START 0x01
+#define STAIRS_STOP 0x02
+#define STAIRS_STATUS 0x08
+#define STAIRS_PHASE 0xF0
#ifdef __cplusplus
}
diff --git a/include/eeprom.h b/include/eeprom.h
index c200358..c9c2f12 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)0x60)
+#define NB_OF_VAR ((uint8_t)0x83)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 570db02..db1405a 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -84,25 +84,28 @@ extern "C" {
#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x0080 // 1023 max force in binary format
#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC // 1500 ms
-#define DEFAULT_STAIRS_PHASE1_TIME 0x0064 // 100 ms
-#define DEFAULT_STAIRS_PHASE2_TIME 0x00C8 // 200 ms
-#define DEFAULT_STAIRS_PHASE3_TIME 0x012C // 300 ms
-#define DEFAULT_STAIRS_PHASE4_TIME 0x0190 // 400 ms
-#define DEFAULT_STAIRS_PHASE5_TIME 0x01F4 // 500 ms
-#define DEFAULT_STAIRS_PHASE6_TIME 0x0258 // 600 ms
-#define DEFAULT_STAIRS_PHASE7_TIME 0x02BC // 700 ms
-#define DEFAULT_STAIRS_PHASE8_TIME 0x0320 // 800 ms
-#define DEFAULT_STAIRS_PHASE9_TIME 0x0384 // 900 ms
+#define DEFAULT_STAIRS_PHASE1_TIME 0x0640 // 100 ms
+#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 200 ms
+#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 300 ms
+#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 400 ms
+#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 500 ms
+#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 600 ms
+#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 700 ms
+#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 800 ms
+#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 900 ms
+#define DEFAULT_STAIRS_PHASE10_TIME 0x3E80 // 1000 ms
#define DEFAULT_STAIRS_X_OFFSET 0xFFF6 // -10 mm
#define DEFAULT_STAIRS_Y_OFFSET 0x0005 // 5mm
#define DEFAULT_STAIRS_Z_OFFSET 0x0014 // 20 mm
-#define DEFAULT_STAIRS_Y_SHIFT 0x0014 // 20 mm
-#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x000A // 10 mm
-#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0032 // 50 mm
-#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000A // 10 mm
+#define DEFAULT_STAIRS_Y_SHIFT 0x0028 // 40 mm
+#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x0000 // 0 mm
+#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0050 // 80 mm
+#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000F // 15 mm
#define DEFAULT_STAIRS_Z_HEIGHT 0x001E // 30 mm
#define DEFAULT_STAIRS_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
+#define EEPROM_SIZE 131
+
#ifdef __cplusplus
}
#endif
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index dd7f12f..b287d06 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -13,6 +13,7 @@
#include "head_tracking.h"
#include "grippers.h"
#include "smart_charger.h"
+#include "stairs.h"
/* timer for the execution of the dynamixel slave loop */
#define DYN_SLAVE_TIMER TIM7
@@ -96,6 +97,8 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
// smart charger commands
if(smart_charger_in_range(address,length))
smart_charger_process_write_cmd(address,length,data);
+ if(stairs_in_range(address,length))
+ stairs_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]);
diff --git a/src/eeprom.c b/src/eeprom.c
index b6eae9e..142ad7a 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -154,7 +154,7 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF, GRIPPER_RIGHT_MIN_ANGLE,
DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8, GRIPPER_RIGHT_MIN_ANGLE+1,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE,
- DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1,
+ DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1,
DEFAULT_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD,
DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1,
DEFAULT_STAIRS_PHASE1_TIME&0xFF, STAIRS_PHASE1_TIME,
@@ -175,6 +175,8 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
DEFAULT_STAIRS_PHASE8_TIME>>8, STAIRS_PHASE8_TIME+1,
DEFAULT_STAIRS_PHASE9_TIME&0xFF, STAIRS_PHASE9_TIME,
DEFAULT_STAIRS_PHASE9_TIME>>8, STAIRS_PHASE9_TIME+1,
+ DEFAULT_STAIRS_PHASE10_TIME&0xFF, STAIRS_PHASE10_TIME,
+ DEFAULT_STAIRS_PHASE10_TIME>>8, STAIRS_PHASE10_TIME+1,
DEFAULT_STAIRS_X_OFFSET, STAIRS_X_OFFSET,
DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET,
DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_OFFSET,
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 709777c..2e72f34 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -11,7 +11,7 @@
#include "imu.h"
#include "smart_charger.h"
#include "dyn_battery.h"
-#include "gpio.h"
+#include "stairs.h"
#include <stdlib.h>
#define MANAGER_TIMER TIM5
@@ -198,6 +198,8 @@ void MANAGER_TIMER_IRQHandler(void)
walking_process();
// call the gripper process
grippers_process();
+ // call the stairs process
+ stairs_process();
// balance the robot
manager_balance();
// access to smart charger
@@ -469,6 +471,7 @@ void manager_init(uint16_t period_us)
head_tracking_init(period_us);
grippers_init(period_us);
smart_charger_init(period_us);
+ stairs_init(period_us);
}
uint16_t manager_get_period(void)
diff --git a/src/ram.c b/src/ram.c
index e708a46..be8aa9d 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -10,152 +10,11 @@ void ram_init(void)
for(i=0;i<RAM_SIZE;i++)
ram_data[i]=0x00;
// read contents from EEPROM to RAM
- if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
- ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0)
- ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0)
- ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
- ram_data[DEVICE_ID_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0)
- ram_data[BAUDRATE_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
- ram_data[RETURN_DELAY_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0)
- 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&0x00FF);
- for(i=0;i<32;i++)
+ for(i=0;i<NB_OF_VAR;i++)
{
- if(EE_ReadVariable(MM_SERVO0_OFFSET+i,&eeprom_data)==0)
- ram_data[DARWIN_MM_SERVO0_OFFSET+i]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(i,&eeprom_data)==0)
+ ram_data[i]=(uint8_t)(eeprom_data&0x00FF);
}
- if(EE_ReadVariable(WALK_X_OFFSET,&eeprom_data)==0)
- ram_data[WALK_X_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_Y_OFFSET,&eeprom_data)==0)
- ram_data[WALK_Y_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_Z_OFFSET,&eeprom_data)==0)
- ram_data[WALK_Z_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_ROLL_OFFSET,&eeprom_data)==0)
- ram_data[WALK_ROLL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PITCH_OFFSET,&eeprom_data)==0)
- ram_data[WALK_PITCH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_YAW_OFFSET,&eeprom_data)==0)
- ram_data[WALK_YAW_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_HIP_PITCH_OFF,&eeprom_data)==0)
- ram_data[WALK_HIP_PITCH_OFF]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_HIP_PITCH_OFF+1,&eeprom_data)==0)
- ram_data[WALK_HIP_PITCH_OFF+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PERIOD_TIME,&eeprom_data)==0)
- ram_data[WALK_PERIOD_TIME]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PERIOD_TIME+1,&eeprom_data)==0)
- ram_data[WALK_PERIOD_TIME+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_DSP_RATIO,&eeprom_data)==0)
- ram_data[WALK_DSP_RATIO]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_STEP_FW_BW_RATIO,&eeprom_data)==0)
- ram_data[WALK_STEP_FW_BW_RATIO]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_FOOT_HEIGHT,&eeprom_data)==0)
- ram_data[WALK_FOOT_HEIGHT]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_SWING_RIGHT_LEFT,&eeprom_data)==0)
- ram_data[WALK_SWING_RIGHT_LEFT]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_SWING_TOP_DOWN,&eeprom_data)==0)
- ram_data[WALK_SWING_TOP_DOWN]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PELVIS_OFFSET,&eeprom_data)==0)
- ram_data[WALK_PELVIS_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_ARM_SWING_GAIN,&eeprom_data)==0)
- ram_data[WALK_ARM_SWING_GAIN]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_MAX_VEL,&eeprom_data)==0)
- ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0)
- ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_P,&eeprom_data)==0)
- ram_data[HEAD_PAN_P]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_P+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_P+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I,&eeprom_data)==0)
- ram_data[HEAD_PAN_I]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_I+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_D,&eeprom_data)==0)
- ram_data[HEAD_PAN_D]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_D+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_D+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I_CLAMP,&eeprom_data)==0)
- ram_data[HEAD_PAN_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I_CLAMP+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_P,&eeprom_data)==0)
- ram_data[HEAD_TILT_P]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_P+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_P+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I,&eeprom_data)==0)
- ram_data[HEAD_TILT_I]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_I+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_D,&eeprom_data)==0)
- ram_data[HEAD_TILT_D]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_D+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_D+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I_CLAMP,&eeprom_data)==0)
- ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_TOP_ID,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_BOT_ID,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_TOP_ID,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_BOT_ID,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(SMART_CHARGER_PERIOD,&eeprom_data)==0)
- ram_data[SMART_CHARGER_PERIOD]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(SMART_CHARGER_PERIOD+1,&eeprom_data)==0)
- ram_data[SMART_CHARGER_PERIOD+1]=(uint8_t)(eeprom_data&0x00FF);
-
}
inline void ram_read_byte(uint16_t address,uint8_t *data)
diff --git a/src/stairs.c b/src/stairs.c
index dfa470a..5cefc8e 100755
--- a/src/stairs.c
+++ b/src/stairs.c
@@ -2,39 +2,40 @@
#include "darwin_kinematics.h"
#include "darwin_math.h"
#include "ram.h"
+#include <stdio.h>
#include <math.h>
-typedef enum {SHIFT_WEIGHT_LEFT,RISE_RIGHT_FOOT,ADVANCE_RIGHT_FOOT,CONTACT_RIGHT_FOOT,SHIFT_WEIGHT_RIGHT,RISE_LEFT_FOOT,ADVANCE_LEFT_FOOT,CONTACT_LEFT_FOOT,CENTER_WEIGHT} stairs_phase_t;
+typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90,STAND_UP=0xA0} stairs_phase_t;
// internal motion variables
-float m_Z_stair_height;
-float m_Z_overshoot;
-float m_Y_swap_amplitude;
-float m_X_left_swap_amplitude;
-float m_Y_right_swap_amplitude;
+float stairs_Z_stair_height;
+float stairs_Z_overshoot;
+float stairs_Y_shift_amplitude;
+float stairs_X_left_shift_amplitude;
+float stairs_X_right_shift_amplitude;
// internal offset variables
-float m_Hip_Pitch_Offset;
-float m_X_Offset;
-float m_Y_Offset;
-float m_Z_Offset;
+float stairs_Hip_Pitch_Offset;
+float stairs_X_Offset;
+float stairs_Y_Offset;
+float stairs_Z_Offset;
// internal time variables
-float m_Time;
-float m_stairs_period;
-float m_shift_weight_left_time;
-float m_rise_right_foot_time;
-float advance_right_foot_time;
-float contact_right_foot_time;
-float shift_weight_right_time;
-float rise_left_foot_time;
-float advance_left_foot_time;
-float contact_left_foot_time;
-float center_weight_time;
-float total_time;
+float stairs_Time;
+float stairs_period;
+float stairs_shift_weight_left_time;
+float stairs_rise_right_foot_time;
+float stairs_advance_right_foot_time;
+float stairs_contact_right_foot_time;
+float stairs_shift_weight_right_time;
+float stairs_rise_left_foot_time;
+float stairs_advance_left_foot_time;
+float stairs_contact_left_foot_time;
+float stairs_center_weight_time;
+float stairs_walk_ready_time;
// control variables
-uint8_t m_Ctrl_Running;
+uint8_t stairs_Ctrl_Running;
// private functions
@@ -45,11 +46,9 @@ void stairs_init(uint16_t period_us)
// initialize internal control variables
- m_Time=0;
+ stairs_Time=0;
stairs_period=((float)period_us)/1000.0;
- m_Ctrl_Running=0x00;
-
- stairs_process();
+ stairs_Ctrl_Running=0x00;
}
inline void stairs_set_period(uint16_t period_us)
@@ -64,18 +63,39 @@ inline uint16_t stairs_get_period(void)
void stairs_start(void)
{
- ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS;
- m_Ctrl_Running=0x01;
+ // load all parameters
+ stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8));
+ stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8));
+ stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8));
+ stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8));
+ stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8));
+ stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8));
+ stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8));
+ stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8));
+ stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8));
+ stairs_walk_ready_time=((float)ram_data[DARWIN_STAIRS_PHASE10_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE10_TIME_H]<<8));
+ stairs_Z_stair_height=((float)ram_data[DARWIN_STAIRS_Z_HEIGHT]);
+ stairs_Z_overshoot=((float)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]);
+ stairs_Y_shift_amplitude=((float)ram_data[DARWIN_STAIRS_Y_SHIFT]);
+ stairs_X_left_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_LEFT_SHIFT]);
+ stairs_X_right_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_RIGHT_SHIFT]);
+ stairs_Hip_Pitch_Offset=((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
+ stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]));
+ stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]));
+ stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]));
+ // start operation
+ ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
+ stairs_Ctrl_Running=0x01;
}
void stairs_stop(void)
{
- m_Ctrl_Running=0x00;
+ stairs_Ctrl_Running=0x00;
}
uint8_t is_climbing_stairs(void)
{
- if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
return 0x01;
else
return 0x00;
@@ -84,225 +104,221 @@ uint8_t is_climbing_stairs(void)
// motion manager interface functions
void stairs_process(void)
{
- float x_swap,y_swap,z_swap,a_swap,b_swap,c_swap;
- float x_move_r,y_move_r,z_move_r,a_move_r,b_move_r,c_move_r;
- float x_move_l,y_move_l,z_move_l,a_move_l,b_move_l,c_move_l;
- float pelvis_offset_r,pelvis_offset_l;
-// float m_Body_Swing_Y,m_Body_Swing_Z;
- float angle[14],ep[12];
+ float angle[14]={0},ep[12]={0};
- if(m_Time==0)
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
{
- update_param_time();
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
- ram_data[DARWIN_WALK_CNTRL]|=PHASE0;
- if(m_Ctrl_Running==0x00)
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
+ if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
{
- if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_STATUS);
- else
- {
- ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
- ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
- ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
- }
+ //1
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time;
+ ep[2]=stairs_Z_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time;
+ ep[8]=stairs_Z_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
}
- }
- else if(m_Time>=(m_Phase_Time1-stairs_period/2.0) && m_Time<(m_Phase_Time1+stairs_period/2.0))
- {
- update_param_move();
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
- ram_data[DARWIN_WALK_CNTRL]|=PHASE1;
- }
- else if(m_Time>=(m_Phase_Time2-stairs_period/2.0) && m_Time<(m_Phase_Time2+stairs_period/2.0))
- {
- update_param_time();
- m_Time=m_Phase_Time2;
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
- ram_data[DARWIN_WALK_CNTRL]|=PHASE2;
- if(m_Ctrl_Running==0x00)
+ else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
{
- if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
- ram_data[DARWIN_WALK_CNTRL]&=~WALK_STATUS;
- else
- {
- ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
- ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
- ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
- }
+ //2
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
+ ep[9]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
}
- }
- else if(m_Time>=(m_Phase_Time3-stairs_period/2.0) && m_Time<(m_Phase_Time3+stairs_period/2.0))
- {
- update_param_move();
- ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
- ram_data[DARWIN_WALK_CNTRL]|=PHASE3;
- }
- update_param_balance();
-
- // Compute endpoints
- x_swap=wsin(m_Time,m_X_Swap_PeriodTime,m_X_Swap_Phase_Shift,m_X_Swap_Amplitude,m_X_Swap_Amplitude_Shift);
- y_swap=wsin(m_Time,m_Y_Swap_PeriodTime,m_Y_Swap_Phase_Shift,m_Y_Swap_Amplitude,m_Y_Swap_Amplitude_Shift);
- z_swap=wsin(m_Time,m_Z_Swap_PeriodTime,m_Z_Swap_Phase_Shift,m_Z_Swap_Amplitude,m_Z_Swap_Amplitude_Shift);
- a_swap=0;
- b_swap=0;
- c_swap=0;
- if(m_Time<=m_SSP_Time_Start_L)
- {
- x_move_l=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
- y_move_l=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
- z_move_l=wsin(m_SSP_Time_Start_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_l=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
- x_move_r=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
- y_move_r=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
- z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_r=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
- pelvis_offset_l=0;
- pelvis_offset_r=0;
- }
- else if(m_Time<=m_SSP_Time_End_L)
- {
- x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
- y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
- z_move_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
- x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
- y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
- z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
- pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Pelvis_Swing/2.0,m_Pelvis_Swing/2.0);
- pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,-m_Pelvis_Offset/2.0,-m_Pelvis_Offset/2.0);
- }
- else if(m_Time<=m_SSP_Time_Start_R)
- {
- x_move_l=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
- y_move_l=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
- z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_l=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
- x_move_r=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
- y_move_r=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
- z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_r=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
- pelvis_offset_l=0.0;
- pelvis_offset_r=0.0;
- }
- else if(m_Time<=m_SSP_Time_End_R)
- {
- x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
- y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
- z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
- x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
- y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
- z_move_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
- pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Pelvis_Offset/2,m_Pelvis_Offset/2);
- pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,-m_Pelvis_Swing/2,-m_Pelvis_Swing/2);
- }
- else
- {
- x_move_l=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
- y_move_l=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
- z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_l=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
- x_move_r=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
- y_move_r=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
- z_move_r=wsin(m_SSP_Time_End_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
- c_move_r=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
- pelvis_offset_l=0.0;
- pelvis_offset_r=0.0;
- }
- a_move_l=0.0;
- b_move_l=0.0;
- a_move_r=0.0;
- b_move_r=0.0;
-
- ep[0]=x_swap+x_move_r+m_X_Offset;
- ep[1]=y_swap+y_move_r-m_Y_Offset / 2.0;
- ep[2]=z_swap+z_move_r+m_Z_Offset;
- ep[3]=a_swap+a_move_r-m_R_Offset / 2.0;
- ep[4]=b_swap+b_move_r+m_P_Offset;
- ep[5]=c_swap+c_move_r-m_A_Offset / 2.0;
- ep[6]=x_swap+x_move_l+m_X_Offset;
- ep[7]=y_swap+y_move_l+m_Y_Offset / 2.0;
- ep[8]=z_swap+z_move_l+m_Z_Offset;
- ep[9]=a_swap+a_move_l+m_R_Offset / 2.0;
- ep[10]=b_swap+b_move_l+m_P_Offset;
- ep[11]=c_swap+c_move_l+m_A_Offset / 2.0;
-
- // Compute body swing
-/* if(m_Time<=m_SSP_Time_End_L)
- {
- m_Body_Swing_Y=-ep[7];
- m_Body_Swing_Z=ep[8];
- }
- else
- {
- m_Body_Swing_Y=-ep[1];
- m_Body_Swing_Z=ep[2];
- }
- m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/
+ else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+ {
+ //3
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ ep[2]=stairs_Z_Offset;
+ ep[3]=-0.05;
+ ep[4]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ ep[5]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[9]=-0.05;
+ ep[10]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ ep[11]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
+ {
+ //4
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=-0.05;
+ ep[4]=0.1;
+ ep[5]=0.3;
+ ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot/(stairs_contact_right_foot_time-stairs_advance_right_foot_time)*(stairs_Time-stairs_advance_right_foot_time);
+ ep[9]=-0.05;
+ ep[10]=0.1;
+ ep[11]=0.3;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
+ {
+ //5
+ ep[0]=stairs_X_Offset-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[2]=stairs_Z_Offset;
+ ep[3]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[4]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[5]=0.3;
+ ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[10]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
+ ep[11]=0.3;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
+ }
+ else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
+ {
+ //6
+ ep[0]=stairs_X_Offset-35.0;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0;
+ ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
+ ep[3]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
+ ep[4]=-0.1;
+ ep[5]=0.3;
+ ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
+ ep[10]=-0.1;
+ ep[11]=0.3;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
+ {
+ //7
+ ep[0]=stairs_X_Offset-35.0+35.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0+10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=0.05;
+ ep[4]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[5]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0-(-35.0+stairs_X_right_shift_amplitude)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0-10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=0.05;
+ ep[10]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ep[11]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
+ {
+ //8
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
+ ep[3]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
+ ep[4]=0.0;
+ ep[5]=0.0;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
+ ep[10]=0.0;
+ ep[11]=0.0;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+ {
+ //9
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
+ ep[3]=0.0;
+ ep[4]=0.0;
+ ep[5]=0.0;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
+ ep[9]=0.0;
+ ep[10]=0.0;
+ ep[11]=0.0;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
+ }
+ else if(stairs_Time>stairs_center_weight_time && stairs_Time<=stairs_walk_ready_time)
+ {
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0;
+ ep[2]=stairs_Z_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0;
+ ep[8]=stairs_Z_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=STAND_UP;
+ }
+ else
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
+ stairs_Ctrl_Running=0x00;
+ }
+ printf("%x,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE,ep[0],ep[1],ep[2],ep[3],ep[4],ep[5],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11]);
+
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ stairs_Time += stairs_period;
+ else
+ stairs_Time=0;
- // Compute arm swing
- if(m_X_Move_Amplitude==0)
- {
- angle[12]=0; // Right
- angle[13]=0; // Left
- }
- else
- {
- angle[12]=wsin(m_Time,m_PeriodTime,0.0,-m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
- angle[13]=wsin(m_Time,m_PeriodTime,0.0,m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
- }
+ // Compute angles with the inverse kinematics
+ if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
+ (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
+ {
+ printf("Impossible to find inverse kinematics solution\n");
+ return;// Do not use angles
+ }
- if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
- {
- m_Time += stairs_period;
- if(m_Time >= m_PeriodTime)
- m_Time = 0;
+ // Compute motor value
+ if(manager_get_module(R_HIP_YAW)==MM_WALKING)
+ manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
+ if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
+ manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0;
+ if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
+ manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0;
+ if(manager_get_module(R_KNEE)==MM_WALKING)
+ manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
+ if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
+ if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
+ manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
+ if(manager_get_module(L_HIP_YAW)==MM_WALKING)
+ manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
+ if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
+ manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0;
+ if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
+ manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0;
+ if(manager_get_module(L_KNEE)==MM_WALKING)
+ manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
+ if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
+ if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
+ manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
+ if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING)
+ manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0;
+ if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
+ manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0;
}
-
- // Compute angles with the inverse kinematics
- if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
- (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
- return;// Do not use angles
-
- // Compute motor value
- if(manager_get_module(R_HIP_YAW)==MM_WALKING)
- manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
- if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
- manager_current_angles[R_HIP_ROLL]=((-180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0;
- if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
- manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-m_Hip_Pitch_Offset)*65536.0;
- if(manager_get_module(R_KNEE)==MM_WALKING)
- manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
- if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
- manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
- if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
- manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
- if(manager_get_module(L_HIP_YAW)==MM_WALKING)
- manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
- if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
- manager_current_angles[L_HIP_ROLL]=((-180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0;
- if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
- manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+m_Hip_Pitch_Offset)*65536.0;
- if(manager_get_module(L_KNEE)==MM_WALKING)
- manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
- if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
- manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
- if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
- manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
- if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING)
- manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0;
- if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
- manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0;
}
// operation functions
uint8_t stairs_in_range(unsigned short int address, unsigned short int length)
{
- if(ram_in_window(WALK_BASE_ADDRESS,WALK_MEM_LENGTH,address,length) ||
- ram_in_window(WALK_EEPROM_ADDRESS,WALK_EEPROM_LENGTH,address,length))
+ if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) ||
+ ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_EEPROM_LENGTH,address,length))
return 0x01;
else
return 0x00;
@@ -318,21 +334,14 @@ void stairs_process_write_cmd(unsigned short int address,unsigned short int leng
uint16_t i;
// walk control
- if(ram_in_range(DARWIN_WALK_CNTRL,address,length))
+ if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
{
- if(data[DARWIN_WALK_CNTRL-address]&WALK_START)
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START)
stairs_start();
- if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP)
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP)
stairs_stop();
}
- if(ram_in_range(DARWIN_WALK_STEP_FW_BW,address,length))
- ram_data[DARWIN_WALK_STEP_FW_BW]=data[DARWIN_WALK_STEP_FW_BW-address];
- if(ram_in_range(DARWIN_WALK_STEP_LEFT_RIGHT,address,length))
- ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=data[DARWIN_WALK_STEP_LEFT_RIGHT-address];
- if(ram_in_range(DARWIN_WALK_STEP_DIRECTION,address,length))
- ram_data[DARWIN_WALK_STEP_DIRECTION]=data[DARWIN_WALK_STEP_DIRECTION-address];
- // walk parameters
- for(i=WALK_EEPROM_ADDRESS;i<=WALK_EEPROM_ADDRESS+WALK_EEPROM_LENGTH;i++)
+ for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++)
if(ram_in_range(i,address,length))
ram_data[i]=data[i-address];
}
--
GitLab