From 9a06538b4f79329dce51f3f4d48e41702a2180d5 Mon Sep 17 00:00:00 2001 From: Irene Garcia Camacho Date: Mon, 22 May 2017 16:33:38 +0200 Subject: [PATCH 01/30] Minor changes in some files related to the smart charger module --- include/darwin_registers.h | 78 ++++++++------------------------------ include/dyn_battery.h | 20 +++++----- include/smart_charger.h | 4 +- src/cm730_fw.c | 1 - src/motion_manager.c | 34 ++++++++--------- src/smart_charger.c | 43 +++++++++++---------- src/test_charger.c | 27 ++++++++----- 7 files changed, 83 insertions(+), 124 deletions(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 86ee43f..ae7cf2e 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -568,67 +568,19 @@ typedef enum { DARWIN_HEAD_MIN_TILT_H = 0x0244, DARWIN_HEAD_TILT_TARGET_L = 0x0245, // angle in fixed point format 9|7 DARWIN_HEAD_TILT_TARGET_H = 0x0246, - -// to do: include more registers os smart charger's memory map to read -/* DARWIN_BATT_CHARGER_STATUS = 0x0247, - DARWIN_BATT_INPUT_CURRENT_L = 0x0248, - DARWIN_BATT_INPUT_CURRENT_H = 0x0249, - DARWIN_BATT_CHARGE_CURRENT_L = 0x024A, - DARWIN_BATT_CHARGE_CURRENT_H = 0x024B, - DARWIN_BATT_CHARGE_VOLTAGE_L = 0x024C, - DARWIN_BATT_CHARGE_VOLTAGE_H = 0x024D, - DARWIN_BATT_LIMIT_CURRENT_L = 0x024E, - DARWIN_BATT_LIMIT_CURRENT_H = 0x024F, // - DARWIN_BATT_TEMPERATURE_L = 0x0250, - DARWIN_BATT_TEMPERATURE_H = 0x0251, - DARWIN_BATT_VOLTAGE_L = 0x0252, - DARWIN_BATT_VOLTAGE_H = 0x0253, - DARWIN_BATT_CURRENT_L = 0x0254, - DARWIN_BATT_CURRENT_H = 0x0255, - DARWIN_BATT_AVG_CURRENT_L = 0x0256, - DARWIN_BATT_AVG_CURRENT_H = 0x0257, - DARWIN_BATT_RELATIVE_SOC = 0x0258,// - DARWIN_BATT_ABSOLUTE_SOC = 0x0259, - DARWIN_BATT_REMAINING_CAP_L = 0x025A, - DARWIN_BATT_REMAINING_CAP_H = 0x025B, - DARWIN_BATT_FULL_CHARGE_CAP_L = 0x025C, - DARWIN_BATT_FULL_CHARGE_CAP_H = 0x025D, - DARWIN_BATT_RUN_TIME_EMPTY_L = 0x025E, // - DARWIN_BATT_RUN_TIME_EMPTY_H = 0x025F, - DARWIN_BATT_AVG_TIME_EMPTY_L = 0x0260, - DARWIN_BATT_AVG_TIME_EMPTY_H = 0x0261, - DARWIN_BATT_AVG_TIME_FULL_L = 0x0262, - DARWIN_BATT_AVG_TIME_FULL_H = 0x0263, - DARWIN_BATT_STATUS_L = 0x0264, - DARWIN_BATT_STATUS_H = 0x0265, - DARWIN_BATT_DESIGN_CAP_L = 0x0266, - DARWIN_BATT_DESIGN_CAP_H = 0x0267, - DARWIN_BATT_DESIGN_VOLTAGE_L = 0x0268, - DARWIN_BATT_DESIGN_VOLTAGE_H = 0x0269, - DARWIN_BATT_CELL1_VOLTAGE_L = 0x026A, - DARWIN_BATT_CELL1_VOLTAGE_H = 0x026B, - DARWIN_BATT_CELL2_VOLTAGE_L = 0x026C, - DARWIN_BATT_CELL2_VOLTAGE_H = 0x026D, - DARWIN_BATT_CELL3_VOLTAGE_L = 0x026E, - DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F, - DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270, - DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271, -*/ -//añadidos + DARWIN_SMART_CHARGER_ID = 0x0247, - DARWIN_SMART_CHARGER_STATUS = 0x0248, - DARWIN_SMART_CHARGER_LIMIT_CURRENT_L = 0x0249, - DARWIN_SMART_CHARGER_LIMIT_CURRENT_H = 0x024A, - DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L = 0x024B, - DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H = 0x024C, - DARWIN_SMART_CHARGER_AVG_TIME_FULL_L = 0x024D, - DARWIN_SMART_CHARGER_AVG_TIME_FULL_H = 0x024E, - DARWIN_SMART_CHARGER_BATT_STATUS_L = 0x024F, - DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x0250, - DARWIN_SMART_CHARGER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | detected | enable -//Cambio registro - DARWIN_GRIPPER_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + DARWIN_SMART_CHARGER_LIMIT_CURRENT_L = 0x0248, // Input max current in mA + DARWIN_SMART_CHARGER_LIMIT_CURRENT_H = 0x0249, + DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L = 0x024A, // Average time to empty batteries in min + DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H = 0x024B, + DARWIN_SMART_CHARGER_AVG_TIME_FULL_L = 0x024C, // Average time to full batteries in min + DARWIN_SMART_CHARGER_AVG_TIME_FULL_H = 0x024D, + DARWIN_SMART_CHARGER_BATT_STATUS_L = 0x024E, // Battery status + 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 // left opened | right opened | | | close left | open left | close right | open right }darwin_registers; @@ -711,7 +663,7 @@ typedef enum { #define HEAD_STOP 0x02 #define HEAD_STATUS 0x10 -#define GRIPPER_BASE_ADDRESS 0x0252//0x0274 +#define GRIPPER_BASE_ADDRESS 0x0251 #define GRIPPER_MEM_LENGTH 1 #define GRIPPER_EEPROM_ADDRESS 0x0054 #define GRIPPER_EEPROM_LENGTH 14 @@ -721,8 +673,8 @@ typedef enum { #define GRIPPER_CLOSE_LEFT 0x08 #define SMART_CHARGER_BASE_ADDRESS 0x0247 -#define SMART_CHARGER_MEM_LENGTH 10 //45 -#define SMART_CHARGER_EEPROM_BASE 0x0060 +#define SMART_CHARGER_MEM_LENGTH 10 +#define SMART_CHARGER_EEPROM_BASE 0x0062 #define SMART_CHARGER_EEPROM_LENGTH 2 #define SMART_CHARGER_DET 0x01 #define SMART_CHARGER_EN 0x02 diff --git a/include/dyn_battery.h b/include/dyn_battery.h index ac02ce8..70e47b6 100644 --- a/include/dyn_battery.h +++ b/include/dyn_battery.h @@ -45,7 +45,7 @@ typedef enum { BATTERY_OUTPUT_VOLTAGE_H = 0x1E, /* RAM */ BATTERY_CHARGER_STATUS = 0x1F, - BATTERY_INPUT_CURRENT_L = 0x20,//corriente consumida (mA) + BATTERY_INPUT_CURRENT_L = 0x20, BATTERY_INPUT_CURRENT_H = 0x21, BATTERY_CHARGE_CURRENT_L = 0x22, BATTERY_CHARGE_CURRENT_H = 0x23, @@ -61,25 +61,25 @@ typedef enum { BATTERY_CURRENT_H = 0x2D, BATTERY_AVG_CURRENT_L = 0x2E, BATTERY_AVG_CURRENT_H = 0x2F, - BATTERY_RELATIVE_SOC = 0x30,//State Of Charge (%) + BATTERY_RELATIVE_SOC = 0x30, BATTERY_ABSOLUTE_SOC = 0x31, - BATTERY_REMAINING_CAP_L = 0x32,//mAh (tiempo de vida) + BATTERY_REMAINING_CAP_L = 0x32, BATTERY_REMAINING_CAP_H = 0x33, - BATTERY_FULL_CHARGE_CAP_L = 0x34,// + BATTERY_FULL_CHARGE_CAP_L = 0x34, BATTERY_FULL_CHARGE_CAP_H = 0x35, - BATTERY_RUN_TIME_EMPTY_L = 0x36,//(min) + BATTERY_RUN_TIME_EMPTY_L = 0x36, BATTERY_RUN_TIME_EMPTY_H = 0x37, BATTERY_AVG_TIME_EMPTY_L = 0x38, - BATTERY_AVG_TIME_EMPTY_H = 0x39,//tiempo hasta que este descargada (min) - BATTERY_AVG_TIME_FULL_L = 0x3A,//tiempo hasta que este cargada (min) + BATTERY_AVG_TIME_EMPTY_H = 0x39, + BATTERY_AVG_TIME_FULL_L = 0x3A, BATTERY_AVG_TIME_FULL_H = 0x3B, - BATTERY_STATUS_L = 0x3C,//cargando o descargando, cargada, descargada,... + BATTERY_STATUS_L = 0x3C, BATTERY_STATUS_H = 0x3D, - BATTERY_DESIGN_CAP_L = 0x3E,//Capacidad de la bateria - numero constante + BATTERY_DESIGN_CAP_L = 0x3E, BATTERY_DESIGN_CAP_H = 0x3F, BATTERY_DESIGN_VOLTAGE_L = 0x40, BATTERY_DESIGN_VOLTAGE_H = 0x41, - BATTERY_CELL1_VOLTAGE_L = 0x42,//Voltage de cada celda + BATTERY_CELL1_VOLTAGE_L = 0x42, BATTERY_CELL1_VOLTAGE_H = 0x43, BATTERY_CELL2_VOLTAGE_L = 0x44, BATTERY_CELL2_VOLTAGE_H = 0x45, diff --git a/include/smart_charger.h b/include/smart_charger.h index d6f8659..5d3587c 100644 --- a/include/smart_charger.h +++ b/include/smart_charger.h @@ -37,13 +37,13 @@ void smart_charger_init(uint16_t period_us); * * \param master pointer to a TDynamixelMaster structure ((darwin_dyn_master or darwin_dyn_master_v2). */ -inline void smart_charger_set_version(TDynamixelMaster *master); +inline void smart_charger_set_master(TDynamixelMaster *master); /** * \brief Function to get the master version of the Dynamixel protocol set to the module * \return the Dynamixel master structure associated to the smart charger module (darwin_dyn_master or darwin_dyn_master_v2). */ -inline TDynamixelMaster* smart_charger_get_version(void); +inline TDynamixelMaster* smart_charger_get_master(void); /** * \brief Function to set smart charger's period * diff --git a/src/cm730_fw.c b/src/cm730_fw.c index d074c76..5c1040f 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -14,7 +14,6 @@ int main(void) { uint16_t eeprom_data,period; - uint8_t i; /* initialize the HAL module */ HAL_Init(); diff --git a/src/motion_manager.c b/src/motion_manager.c index 4672002..eeb4d17 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -322,20 +322,20 @@ void manager_init(uint16_t period_us) } //Other devices detected - smart charger - for(i=current; i Default: (1500ms*1000)/7800us uint8_t smart_charger_write; //write smart charger signal uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; -#define fifosize 32 +#define FIFOSIZE 32 unsigned char *infifo; //Pointer to fifo position where to store next input data unsigned char *outfifo; //Pointer to fifo position where is stored the data to be written unsigned char numdata; //Number of elements stored in the fifo -unsigned char write_fifo[fifosize]; //Vector with data to write (data_write) +unsigned char write_fifo[FIFOSIZE]; //Vector with data to write (data_write) unsigned char current[2]; //--------------------------------------- @@ -29,11 +29,10 @@ TDynamixelMaster *dyn_battery_master; void smart_charger_init(uint16_t period_us) { smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID]; - smart_charger_enabled = 0x00; + smart_charger_enabled = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_EN; smart_charger_detected = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_DET; - counter = 0; - smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L]; - smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8; + // counter = 0; + smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_L]) + ((ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8); smart_charger_count = (smart_charger_period*1000)/period_us; smart_charger_write = 0x00; infifo=outfifo=&write_fifo[0]; @@ -41,20 +40,20 @@ void smart_charger_init(uint16_t period_us) ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs); } -inline void smart_charger_set_version(TDynamixelMaster *master) +inline void smart_charger_set_master(TDynamixelMaster *master) { dyn_battery_master=master; } -inline TDynamixelMaster* smart_charger_get_version(void) +inline TDynamixelMaster* smart_charger_get_master(void) { return dyn_battery_master; } -void smart_charger_set_period(uint16_t period) //en ms +void smart_charger_set_period(uint16_t period) { - uint16_t mm_period=manager_get_period_us(); //default: 7800us - smart_charger_count = (period*1000)/mm_period; //(ms*1000)/us + uint16_t mm_period=manager_get_period_us(); + smart_charger_count = (period*1000)/mm_period; smart_charger_period = period; ram_data[DARWIN_SMART_CHARGER_PERIOD_L]=period&0x00FF; ram_data[DARWIN_SMART_CHARGER_PERIOD_H]=(period&0xFF00)>>8; @@ -94,8 +93,8 @@ void smart_charger_process_read_cmd(unsigned short int address,unsigned short in void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) { - //uint8_t i; - uint16_t period,i; + uint16_t period;//,i; + //Enable/Disable smart charger if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length)) @@ -119,9 +118,9 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; - /* if(infifo==&write_fifo[fifosize]); + /* if(infifo==&write_fifo[FIFOSIZE]); infifo=&write_fifo[0]; //go to first position of fifo - if(numdata luz verde } - +/* ////READ CHARGER STATUS unsigned short int address=DARWIN_SMART_CHARGER_STATUS; unsigned short int length=1; @@ -84,9 +84,9 @@ int main(void) error=darwin_on_read(address,length,&data_read); if(data_read&0x01) gpio_set_led(LED_2); //verde - +*/ -/* ////READ PERIOD SMART CHARGER + ////READ PERIOD SMART CHARGER unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L; unsigned short int length=2; unsigned char data_read[2]; @@ -98,7 +98,7 @@ int main(void) gpio_set_led(LED_3); //verde else if(dread==0) gpio_set_led(LED_4); //azul -*/ + /* ////READ ID SMART CHARGER unsigned char data_read2; @@ -110,7 +110,7 @@ int main(void) //else if(data_read==0) */ -/* ////WRITE PERIOD SMART CHARGER + ////WRITE PERIOD SMART CHARGER //HAL_Delay(500); unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L; unsigned short int length=2; @@ -126,9 +126,9 @@ int main(void) gpio_set_led(LED_3); //verde else if(dread==1500) gpio_set_led(LED_4); //azul -*/ -/* ////ENABLE SMART CHARGER MODULE + + ////ENABLE SMART CHARGER MODULE address=DARWIN_SMART_CHARGER_CNTRL; length=1; uint8_t data_read; @@ -145,14 +145,21 @@ int main(void) darwin_on_read(address,length,&data_read); if(data_read==3) //Smart charger control = detected + enabled gpio_set_led(LED_3); //verde -*/ + -/* ////ENABLE MOTION MANAGER TIMER + ////ENABLE MOTION MANAGER TIMER address=DARWIN_MM_CNTRL; data_write=MANAGER_ENABLE; darwin_on_write(address,length,&data_write); HAL_Delay(500); -*/ + + ////WRITE LIMIT CURRENT +/* address=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L; + length=2; + data_write=0.512; + darwin_on_write(address,length,&data_write); + HAL_Delay(500); +*/ /* //Read battery status address=DARWIN_BATT_AVG_TIME_EMPTY_L; -- GitLab From d57e7c1cee36b1e57a4ded44607bfb749dda842a Mon Sep 17 00:00:00 2001 From: Irene Garcia Camacho Date: Wed, 24 May 2017 11:12:03 +0200 Subject: [PATCH 02/30] Improved codign style, deleted comments. --- include/darwin_registers.h | 202 ++++++++++++++++++------------------ include/eeprom_init.h | 155 ++++++++++++++-------------- src/eeprom.c | 204 ++++++++++++++++++------------------- src/motion_manager.c | 6 +- src/ram.c | 1 + src/smart_charger.c | 60 ++--------- 6 files changed, 294 insertions(+), 334 deletions(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index ae7cf2e..4ed2722 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -88,106 +88,106 @@ extern "C" { #define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) typedef enum { - DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET, - DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1, - DARWIN_VERSION = FIRMWARE_VERSION_OFFSET, - DARWIN_ID = DEVICE_ID_OFFSET, - DARWIN_BAUD_RATE = BAUDRATE_OFFSET, - DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET, - DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET, - DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1, - DARWIN_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16 - DARWIN_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1, - DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16 - DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1, - DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16 - DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1, - DARWIN_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16 - DARWIN_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1, - DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET, - DARWIN_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO3_OFFSET = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO4_OFFSET = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO5_OFFSET = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO6_OFFSET = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO7_OFFSET = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO8_OFFSET = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO9_OFFSET = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO10_OFFSET = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO11_OFFSET = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO12_OFFSET = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO13_OFFSET = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO14_OFFSET = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO15_OFFSET = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO16_OFFSET = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO17_OFFSET = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO18_OFFSET = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO19_OFFSET = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO20_OFFSET = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO21_OFFSET = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO22_OFFSET = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO23_OFFSET = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO24_OFFSET = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO25_OFFSET = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO26_OFFSET = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO27_OFFSET = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO28_OFFSET = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO29_OFFSET = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO30_OFFSET = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4 - DARWIN_WALK_X_OFFSET = WALK_X_OFFSET, - DARWIN_WALK_Y_OFFSET = WALK_Y_OFFSET, - DARWIN_WALK_Z_OFFSET = WALK_Z_OFFSET, - DARWIN_WALK_ROLL_OFFSET = WALK_ROLL_OFFSET, - DARWIN_WALK_PITCH_OFFSET = WALK_PITCH_OFFSET, - DARWIN_WALK_YAW_OFFSET = WALK_YAW_OFFSET, - DARWIN_WALK_HIP_PITCH_OFF_L = WALK_HIP_PITCH_OFF, - DARWIN_WALK_HIP_PITCH_OFF_H = WALK_HIP_PITCH_OFF+1, - DARWIN_WALK_PERIOD_TIME_L = WALK_PERIOD_TIME, - DARWIN_WALK_PERIOD_TIME_H = WALK_PERIOD_TIME+1, - DARWIN_WALK_DSP_RATIO = WALK_DSP_RATIO, - DARWIN_WALK_STEP_FW_BW_RATIO = WALK_STEP_FW_BW_RATIO, - DARWIN_WALK_FOOT_HEIGHT = WALK_FOOT_HEIGHT, - DARWIN_WALK_SWING_RIGHT_LEFT = WALK_SWING_RIGHT_LEFT, - DARWIN_WALK_SWING_TOP_DOWN = WALK_SWING_TOP_DOWN, - DARWIN_WALK_PELVIS_OFFSET = WALK_PELVIS_OFFSET, - DARWIN_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN, - DARWIN_WALK_MAX_VEL = WALK_MAX_VEL, - DARWIN_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL, - DARWIN_HEAD_PAN_P_L = HEAD_PAN_P,// constant in fixed point format 0|16 - DARWIN_HEAD_PAN_P_H = HEAD_PAN_P+1, - DARWIN_HEAD_PAN_I_L = HEAD_PAN_I,// constant in fixed point format 0|16 - DARWIN_HEAD_PAN_I_H = HEAD_PAN_I+1, - DARWIN_HEAD_PAN_D_L = HEAD_PAN_D,// constant in fixed point format 0|16 - DARWIN_HEAD_PAN_D_H = HEAD_PAN_D+1, - DARWIN_HEAD_PAN_I_CLAMP_L = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7 - DARWIN_HEAD_PAN_I_CLAMP_H = HEAD_PAN_I_CLAMP+1, - DARWIN_HEAD_TILT_P_L = HEAD_TILT_P,// constant in fixed point format 0|16 - DARWIN_HEAD_TILT_P_H = HEAD_TILT_P+1, - DARWIN_HEAD_TILT_I_L = HEAD_TILT_I,// constant in fixed point format 0|16 - DARWIN_HEAD_TILT_I_H = HEAD_TILT_I+1, - DARWIN_HEAD_TILT_D_L = HEAD_TILT_D,// constant in fixed point format 0|16 - DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1, - DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 - DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1, - DARWIN_GRIPPER_LEFT_ID = GRIPPER_LEFT_ID, - DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE, - DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1, - DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE, - DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1, - DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE, - DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1, - DARWIN_GRIPPER_RIGHT_ID = GRIPPER_RIGHT_ID, - DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE, - DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1, - DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE, - DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1, - DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE, - DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1, - DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms - DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1, + DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET, + DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1, + DARWIN_VERSION = FIRMWARE_VERSION_OFFSET, + DARWIN_ID = DEVICE_ID_OFFSET, + DARWIN_BAUD_RATE = BAUDRATE_OFFSET, + DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET, + DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET, + DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1, + DARWIN_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1, + DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1, + DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1, + DARWIN_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1, + DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET, + DARWIN_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO3_OFFSET = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO4_OFFSET = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO5_OFFSET = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO6_OFFSET = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO7_OFFSET = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO8_OFFSET = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO9_OFFSET = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO10_OFFSET = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO11_OFFSET = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO12_OFFSET = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO13_OFFSET = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO14_OFFSET = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO15_OFFSET = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO16_OFFSET = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO17_OFFSET = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO18_OFFSET = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO19_OFFSET = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO20_OFFSET = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO21_OFFSET = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO22_OFFSET = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO23_OFFSET = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO24_OFFSET = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO25_OFFSET = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO26_OFFSET = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO27_OFFSET = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO28_OFFSET = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO29_OFFSET = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO30_OFFSET = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_WALK_X_OFFSET = WALK_X_OFFSET, + DARWIN_WALK_Y_OFFSET = WALK_Y_OFFSET, + DARWIN_WALK_Z_OFFSET = WALK_Z_OFFSET, + DARWIN_WALK_ROLL_OFFSET = WALK_ROLL_OFFSET, + DARWIN_WALK_PITCH_OFFSET = WALK_PITCH_OFFSET, + DARWIN_WALK_YAW_OFFSET = WALK_YAW_OFFSET, + DARWIN_WALK_HIP_PITCH_OFF_L = WALK_HIP_PITCH_OFF, + DARWIN_WALK_HIP_PITCH_OFF_H = WALK_HIP_PITCH_OFF+1, + DARWIN_WALK_PERIOD_TIME_L = WALK_PERIOD_TIME, + DARWIN_WALK_PERIOD_TIME_H = WALK_PERIOD_TIME+1, + DARWIN_WALK_DSP_RATIO = WALK_DSP_RATIO, + DARWIN_WALK_STEP_FW_BW_RATIO = WALK_STEP_FW_BW_RATIO, + DARWIN_WALK_FOOT_HEIGHT = WALK_FOOT_HEIGHT, + DARWIN_WALK_SWING_RIGHT_LEFT = WALK_SWING_RIGHT_LEFT, + DARWIN_WALK_SWING_TOP_DOWN = WALK_SWING_TOP_DOWN, + DARWIN_WALK_PELVIS_OFFSET = WALK_PELVIS_OFFSET, + DARWIN_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN, + DARWIN_WALK_MAX_VEL = WALK_MAX_VEL, + DARWIN_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL, + DARWIN_HEAD_PAN_P_L = HEAD_PAN_P,// constant in fixed point format 0|16 + DARWIN_HEAD_PAN_P_H = HEAD_PAN_P+1, + DARWIN_HEAD_PAN_I_L = HEAD_PAN_I,// constant in fixed point format 0|16 + DARWIN_HEAD_PAN_I_H = HEAD_PAN_I+1, + DARWIN_HEAD_PAN_D_L = HEAD_PAN_D,// constant in fixed point format 0|16 + DARWIN_HEAD_PAN_D_H = HEAD_PAN_D+1, + DARWIN_HEAD_PAN_I_CLAMP_L = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7 + DARWIN_HEAD_PAN_I_CLAMP_H = HEAD_PAN_I_CLAMP+1, + DARWIN_HEAD_TILT_P_L = HEAD_TILT_P,// constant in fixed point format 0|16 + DARWIN_HEAD_TILT_P_H = HEAD_TILT_P+1, + DARWIN_HEAD_TILT_I_L = HEAD_TILT_I,// constant in fixed point format 0|16 + DARWIN_HEAD_TILT_I_H = HEAD_TILT_I+1, + DARWIN_HEAD_TILT_D_L = HEAD_TILT_D,// constant in fixed point format 0|16 + DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1, + DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 + DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1, + DARWIN_GRIPPER_LEFT_ID = GRIPPER_LEFT_ID, + DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE, + DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1, + DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE, + DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1, + DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE, + DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1, + DARWIN_GRIPPER_RIGHT_ID = GRIPPER_RIGHT_ID, + DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE, + DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1, + DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE, + DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1, + DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE, + DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1, + DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms + DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1, //RAM DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // | | | | blink | toggle | value | internally used @@ -673,7 +673,7 @@ typedef enum { #define GRIPPER_CLOSE_LEFT 0x08 #define SMART_CHARGER_BASE_ADDRESS 0x0247 -#define SMART_CHARGER_MEM_LENGTH 10 +#define SMART_CHARGER_MEM_LENGTH 11 #define SMART_CHARGER_EEPROM_BASE 0x0062 #define SMART_CHARGER_EEPROM_LENGTH 2 #define SMART_CHARGER_DET 0x01 diff --git a/include/eeprom_init.h b/include/eeprom_init.h index 7ba89ed..dafe223 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -5,84 +5,83 @@ extern "C" { #endif -#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 0x1E78 //7800us -#define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16 -#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999 -#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9 -#define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5 -#define DEFAULT_RETURN_LEVEL 0x0002 -#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 -#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm -#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm -#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm -#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 -#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 -#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 -#define DEFAULT_WALK_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10 -#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms -#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8 -#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8 -#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm -#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm -#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm -#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3 -#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5 -#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s -#define DEFAULT_WALK_MAX_ROT_VEL 0x0040 // 8 degrees/s in fixed point format 5 | 3 -#define DEFAULT_HEAD_PAN_P 0x028F // 0.01 in fixed point format 0|16 -#define DEFAULT_HEAD_PAN_I 0x0000 // 0.0 in fixed point format 0|16 -#define DEFAULT_HEAD_PAN_D 0x0000 // 0.0 in fixed point format 0|16 -#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 -#define DEFAULT_HEAD_TILT_P 0x028F // 0.01 in fixed point format 0|16 -#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16 -#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16 -#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 -#define DEFAULT_GRIPPER_LEFT_ID 0x0017 // ID 23 for the left gripper -#define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 -#define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 -#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format -#define DEFAULT_GRIPPER_RIGHT_ID 0x0018 // ID 24 for the left gripper -#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 -#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 -#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format - -#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms (7,8ms*200) +#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 0x1E78 //7800us +#define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16 +#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999 +#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9 +#define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5 +#define DEFAULT_RETURN_LEVEL 0x0002 +#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm +#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm +#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm +#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_WALK_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10 +#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms +#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8 +#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8 +#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm +#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm +#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm +#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5 +#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s +#define DEFAULT_WALK_MAX_ROT_VEL 0x0040 // 8 degrees/s in fixed point format 5 | 3 +#define DEFAULT_HEAD_PAN_P 0x028F // 0.01 in fixed point format 0|16 +#define DEFAULT_HEAD_PAN_I 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_PAN_D 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 +#define DEFAULT_HEAD_TILT_P 0x028F // 0.01 in fixed point format 0|16 +#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 +#define DEFAULT_GRIPPER_LEFT_ID 0x0017 // ID 23 for the left gripper +#define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 +#define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 +#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format +#define DEFAULT_GRIPPER_RIGHT_ID 0x0018 // ID 24 for the left gripper +#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 +#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 +#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format +#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms diff --git a/src/eeprom.c b/src/eeprom.c index d511af2..4fa6f8e 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -53,108 +53,108 @@ uint16_t DataVar = 0; /* Virtual address defined by the user: 0xFFFF value is prohibited */ -uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF, - DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB - DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB - DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version - DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id - DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate - DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time - DEFAULT_MM_PERIOD&0xFF, 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,// return level - DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET, - DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET, - DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET, - DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET, - DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET, - DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET, - DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET, - DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET, - DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET, - DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET, - DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET, - DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET, - DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET, - DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET, - DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET, - DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET, - DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET, - DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET, - DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET, - DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET, - DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET, - DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET, - DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET, - DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET, - DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET, - DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET, - DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET, - DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET, - DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET, - DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET, - DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET, - DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET, - DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET, - DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET, - DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET, - DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET, - DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET, - DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET, - DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF, - DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1, - DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME, - DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1, - DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO, - DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO, - DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT, - DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT, - DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN, - DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET, - DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN, - DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL, - DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL, - DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P, - DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1, - DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I, - DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1, - DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D, - DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, - DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP, - DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1, - DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, - DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P, - DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1, - DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I, - DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1, - DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D, - DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1, - DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP, - DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1, - DEFAULT_GRIPPER_LEFT_ID, GRIPPER_LEFT_ID, - DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE, - DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1, - DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE, - DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1, - DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE, - DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1, - DEFAULT_GRIPPER_RIGHT_ID, GRIPPER_RIGHT_ID, - DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE, - DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1, - 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_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD, - DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1}; +uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF, + DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB + DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB + DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version + DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id + DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate + DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time + DEFAULT_MM_PERIOD&0xFF, 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,// return level + DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET, + DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET, + DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET, + DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET, + DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET, + DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET, + DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET, + DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET, + DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET, + DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET, + DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET, + DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET, + DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET, + DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET, + DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET, + DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET, + DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET, + DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET, + DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET, + DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET, + DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET, + DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET, + DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET, + DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET, + DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET, + DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET, + DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET, + DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET, + DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET, + DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET, + DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET, + DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET, + DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET, + DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET, + DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET, + DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET, + DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET, + DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET, + DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF, + DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1, + DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME, + DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1, + DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO, + DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO, + DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT, + DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT, + DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN, + DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET, + DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN, + DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL, + DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL, + DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P, + DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1, + DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I, + DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1, + DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D, + DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, + DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP, + DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1, + DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, + DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P, + DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1, + DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I, + DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1, + DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D, + DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1, + DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP, + DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1, + DEFAULT_GRIPPER_LEFT_ID, GRIPPER_LEFT_ID, + DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE, + DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1, + DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE, + DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1, + DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE, + DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1, + DEFAULT_GRIPPER_RIGHT_ID, GRIPPER_RIGHT_ID, + DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE, + DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1, + 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_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD, + DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1}; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ diff --git a/src/motion_manager.c b/src/motion_manager.c index eeb4d17..6e7768a 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -11,6 +11,7 @@ #include "imu.h" #include "smart_charger.h" #include "dyn_battery.h" +#include "gpio.h" #define MANAGER_TIMER TIM5 #define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE() @@ -332,7 +333,7 @@ void manager_init(uint16_t period_us) smart_charger_set_master(&darwin_dyn_master); //set bus Dynamixel master version // Set smart charger's memory map dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]); - dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]); + dyn_master_write_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]); break; default: break; } @@ -389,12 +390,13 @@ void manager_init(uint16_t period_us) dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model); switch(model) { + // gpio_set_led(LED_RX); case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]=SMART_CHARGER_DET; //smart charger detected ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID smart_charger_set_master(&darwin_dyn_master_v2); //Set bus Dynamixel master version // Set smart charger's memory map dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]); - dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]); + //dyn_master_write_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]); break; default: break; } diff --git a/src/ram.c b/src/ram.c index f2c505d..e061c05 100755 --- a/src/ram.c +++ b/src/ram.c @@ -151,6 +151,7 @@ void ram_init(void) 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/smart_charger.c b/src/smart_charger.c index 3812797..03a9ed8 100644 --- a/src/smart_charger.c +++ b/src/smart_charger.c @@ -9,18 +9,10 @@ uint8_t smart_charger_id; //smart charger dynamixel ID uint8_t smart_charger_enabled; //smart charger module enable signal uint8_t smart_charger_detected; //smart charger detect signal uint16_t smart_charger_period; //smart charger period value in ms -//uint16_t counter; //for read access operations uint16_t smart_charger_count; //counter = sc period / mm period --> Default: (1500ms*1000)/7800us uint8_t smart_charger_write; //write smart charger signal uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; -#define FIFOSIZE 32 -unsigned char *infifo; //Pointer to fifo position where to store next input data -unsigned char *outfifo; //Pointer to fifo position where is stored the data to be written -unsigned char numdata; //Number of elements stored in the fifo -unsigned char write_fifo[FIFOSIZE]; //Vector with data to write (data_write) -unsigned char current[2]; - //--------------------------------------- TDynamixelMaster *dyn_battery_master; @@ -31,12 +23,9 @@ void smart_charger_init(uint16_t period_us) smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID]; smart_charger_enabled = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_EN; smart_charger_detected = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_DET; - // counter = 0; - smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_L]) + ((ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8); + smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L] + (ram_data[DARWIN_SMART_CHARGER_PERIOD_H]<<8); smart_charger_count = (smart_charger_period*1000)/period_us; smart_charger_write = 0x00; - infifo=outfifo=&write_fifo[0]; - numdata=0; //fifo empty ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs); } @@ -93,7 +82,7 @@ void smart_charger_process_read_cmd(unsigned short int address,unsigned short in void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) { - uint16_t period;//,i; + uint16_t period; //Enable/Disable smart charger @@ -114,28 +103,8 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i //Write Battery limit current if(ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,address,length) && ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_H,address,length)) { - current[0]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address]; - current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; - - - /* if(infifo==&write_fifo[FIFOSIZE]); - infifo=&write_fifo[0]; //go to first position of fifo - if(numdata Date: Wed, 31 May 2017 14:36:05 +0200 Subject: [PATCH 03/30] Added support to detect when the robot falls using the accelerometer. --- include/darwin_registers.h | 6 ++++-- include/imu.h | 1 + include/motion_manager.h | 6 ++++++ src/imu.c | 37 +++++++++++++++++++++-------------- src/motion_manager.c | 40 ++++++++++++++++++++++++++++++++++++++ 5 files changed, 73 insertions(+), 17 deletions(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 4ed2722..c91a832 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -272,8 +272,8 @@ typedef enum { DARWIN_IMU_ACCEL_Z_H = 0x0149, DARWIN_MM_NUM_SERVOS = 0x014A, - DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // scanning | | | | | Enable power | Enable balance | Enable manager + DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // scanning | fwd fall | bwd fall | | | Enable power | Enable balance | Enable manager DARWIN_MM_MODULE_EN0 = 0x014C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i // Enable servo 0 | assigned module | Enable servo 1 | assigned module // | 000 -> none | @@ -627,6 +627,8 @@ typedef enum { #define MANAGER_EN_BAL 0x02 #define MANAGER_EN_PWR 0x04 #define MANAGER_SCANNING 0x80 +#define MANAGER_FWD_FALL 0x40 +#define MANAGER_BWD_FALL 0x20 #define MANAGER_EVEN_SER_EN 0x80 #define MANAGER_EVEN_SER_MOD 0x70 #define MANAGER_ODD_SER_EN 0x08 diff --git a/include/imu.h b/include/imu.h index 079304b..f3e1c3d 100755 --- a/include/imu.h +++ b/include/imu.h @@ -14,6 +14,7 @@ void imu_stop(void); void imu_set_calibration_samples(uint16_t num_samples); void imu_start_gyro_cal(void); void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z); +void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z); uint8_t imu_is_gyro_calibrated(void); // operation functions uint8_t imu_in_range(unsigned short int address,unsigned short int length); diff --git a/include/motion_manager.h b/include/motion_manager.h index 10f053f..641c39d 100755 --- a/include/motion_manager.h +++ b/include/motion_manager.h @@ -38,6 +38,10 @@ typedef enum {MM_NONE = 0, MM_JOINTS = 3, MM_HEAD = 4} TModules; +typedef enum {MM_FWD_FALL =0, + MM_BWD_FALL =1, + MM_STANDING =2} TFall; + typedef struct { void (*process_fnct)(void); @@ -86,6 +90,8 @@ inline void manager_disable(void); inline uint8_t manager_is_enabled(void); void manager_enable_balance(void); void manager_disable_balance(void); +inline uint8_t manager_has_fallen(void); +TFall manager_get_fallen_position(void); inline uint8_t manager_get_num_servos(void); void manager_set_module(uint8_t servo_id,TModules module); inline TModules manager_get_module(uint8_t servo_id); diff --git a/src/imu.c b/src/imu.c index 3d3f047..e792585 100755 --- a/src/imu.c +++ b/src/imu.c @@ -31,11 +31,11 @@ // accelerometer registers #define ACCEL_ID 0x32 #define ACCEL_WHO_AM_I 0x0F -#define ACCEL_CNTRL_REG1 0x20 -#define ACCEL_CNTRL_REG2 0x21 -#define ACCEL_CNTRL_REG3 0x22 -#define ACCEL_CNTRL_REG4 0x23 -#define ACCEL_CNTRL_REG5 0x24 +#define ACCEL_CNTRL_REG1 0x20 +#define ACCEL_CNTRL_REG2 0x21 +#define ACCEL_CNTRL_REG3 0x22 +#define ACCEL_CNTRL_REG4 0x23 +#define ACCEL_CNTRL_REG5 0x24 #define ACCEL_HP_FILTER_RESET 0x25 #define ACCEL_REFERENCE 0x26 #define ACCEL_STATUS_REG 0x27 @@ -58,11 +58,11 @@ // gyroscope registers #define GYRO_ID 0xD3 #define GYRO_WHO_AM_I 0x0F -#define GYRO_CNTRL_REG1 0x20 -#define GYRO_CNTRL_REG2 0x21 -#define GYRO_CNTRL_REG3 0x22 -#define GYRO_CNTRL_REG4 0x23 -#define GYRO_CNTRL_REG5 0x24 +#define GYRO_CNTRL_REG1 0x20 +#define GYRO_CNTRL_REG2 0x21 +#define GYRO_CNTRL_REG3 0x22 +#define GYRO_CNTRL_REG4 0x23 +#define GYRO_CNTRL_REG5 0x24 #define GYRO_REFERENCE 0x25 #define GYRO_OUT_TEMP 0x26 #define GYRO_STATUS_REG 0x27 @@ -72,7 +72,7 @@ #define GYRO_OUT_Y_H 0x2B #define GYRO_OUT_Z_L 0x2C #define GYRO_OUT_Z_H 0x2D -#define GYRO_FIFO_CNTRL_REG 0x2E +#define GYRO_FIFO_CNTRL_REG 0x2E #define GYRO_FIFO_SRC_REG 0x2F #define GYRO_INT1_CFG 0x30 #define GYRO_INT1_SRC 0x31 @@ -110,6 +110,7 @@ uint16_t gyro_calibration_num_samples; const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1; const uint8_t accel_conf_len=5; uint8_t accel_conf_data[5]; +int32_t accel_data[3]; const uint8_t accel_data_reg=ACCEL_OUT_X_L; const uint8_t accel_data_len=6; @@ -254,13 +255,12 @@ void imu_accel_get_data(void) void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out) { uint8_t i; - int16_t value; for(i=0;i<3;i++) { - value=(data_in[i*2]+(data_in[i*2+1]<<8)); - data_out[i*2]=value%256; - data_out[i*2+1]=value/256; + accel_data[i]=(data_in[i*2]+(data_in[i*2+1]<<8)); + data_out[i*2]=accel_data[i]%256; + data_out[i*2+1]=accel_data[i]/256; } } @@ -603,6 +603,13 @@ void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z) *z=gyro_data[2]; } +void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z) +{ + *x=accel_data[0]; + *y=accel_data[1]; + *z=accel_data[2]; +} + // operation functions uint8_t imu_in_range(unsigned short int address,unsigned short int length) { diff --git a/src/motion_manager.c b/src/motion_manager.c index 6e7768a..b6bb218 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -12,6 +12,7 @@ #include "smart_charger.h" #include "dyn_battery.h" #include "gpio.h" +#include #define MANAGER_TIMER TIM5 #define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE() @@ -132,6 +133,7 @@ void manager_balance(void) { uint32_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain; int32_t gyro_x,gyro_y,gyro_z; + int32_t accel_x,accel_y,accel_z; if(manager_balance_enabled==0x01)// balance is enabled { @@ -152,6 +154,26 @@ void manager_balance(void) manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9); manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9); } + // fall detection (using accelerometer) + imu_get_accel_data(&accel_x,&accel_y,&accel_z); + if(abs(accel_y)>abs(accel_z)) + { + if(accel_y>0) + { + ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL); + ram_data[DARWIN_MM_CNTRL]|=MANAGER_BWD_FALL; + } + else + { + ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL); + ram_data[DARWIN_MM_CNTRL]|=MANAGER_FWD_FALL; + } + } + else + { + ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL); + ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL); + } } // interrupt handlers @@ -509,6 +531,24 @@ void manager_disable_balance(void) ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_BAL); } +uint8_t manager_has_fallen(void) +{ + if(ram_data[DARWIN_MM_CNTRL]&(MANAGER_FWD_FALL|MANAGER_BWD_FALL)) + return 0x01; + else + return 0x00; +} + +TFall manager_get_fallen_position(void) +{ + if(ram_data[DARWIN_MM_CNTRL]&MANAGER_FWD_FALL) + return MM_FWD_FALL; + else if(ram_data[DARWIN_MM_CNTRL]&MANAGER_BWD_FALL) + return MM_BWD_FALL; + else + return MM_STANDING; +} + inline uint8_t manager_get_num_servos(void) { return manager_num_servos; -- GitLab From 0210fa1dd083694a998dd31906d5aed3c8b039f0 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Wed, 28 Jun 2017 17:40:03 +0200 Subject: [PATCH 04/30] Added support for the gripper used for the smart charger application. --- include/darwin_registers.h | 36 +++++--- include/eeprom_init.h | 6 +- include/grippers.h | 4 +- include/motion_manager.h | 12 ++- src/eeprom.c | 6 +- src/grippers.c | 185 +++++++++++++++++++++++++------------ src/motion_manager.c | 2 +- src/ram.c | 12 ++- 8 files changed, 176 insertions(+), 87 deletions(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index c91a832..8b5c191 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -75,15 +75,17 @@ extern "C" { #define HEAD_TILT_I ((unsigned short int)0x004E) #define HEAD_TILT_D ((unsigned short int)0x0050) #define HEAD_TILT_I_CLAMP ((unsigned short int)0x0052) -#define GRIPPER_LEFT_ID ((unsigned short int)0x0054) -#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0055) -#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0057) -#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x0059) -#define GRIPPER_RIGHT_ID ((unsigned short int)0x005B) -#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005C) -#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x005E) -#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0060) -#define SMART_CHARGER_PERIOD ((unsigned short int)0x0062) +#define GRIPPER_LEFT_TOP_ID ((unsigned short int)0x0054) +#define GRIPPER_LEFT_BOT_ID ((unsigned short int)0x0055) +#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0056) +#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0058) +#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x005A) +#define GRIPPER_RIGHT_TOP_ID ((unsigned short int)0x005C) +#define GRIPPER_RIGHT_BOT_ID ((unsigned short int)0x005D) +#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005E) +#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x0060) +#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062) +#define SMART_CHARGER_PERIOD ((unsigned short int)0x0064) #define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) @@ -172,14 +174,16 @@ typedef enum { DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1, DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1, - DARWIN_GRIPPER_LEFT_ID = GRIPPER_LEFT_ID, + DARWIN_GRIPPER_LEFT_TOP_ID = GRIPPER_LEFT_TOP_ID, + DARWIN_GRIPPER_LEFT_BOT_ID = GRIPPER_LEFT_BOT_ID, DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE, DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1, DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE, DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1, DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE, DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1, - DARWIN_GRIPPER_RIGHT_ID = GRIPPER_RIGHT_ID, + DARWIN_GRIPPER_RIGHT_TOP_ID = GRIPPER_RIGHT_TOP_ID, + DARWIN_GRIPPER_RIGHT_BOT_ID = GRIPPER_RIGHT_BOT_ID, DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE, DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1, DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE, @@ -580,8 +584,8 @@ 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 - // left opened | right opened | | | close left | open left | close right | open right + 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_registers; #define GPIO_BASE_ADDRESS 0x0100 @@ -668,11 +672,15 @@ typedef enum { #define GRIPPER_BASE_ADDRESS 0x0251 #define GRIPPER_MEM_LENGTH 1 #define GRIPPER_EEPROM_ADDRESS 0x0054 -#define GRIPPER_EEPROM_LENGTH 14 +#define GRIPPER_EEPROM_LENGTH 16 #define GRIPPER_OPEN_RIGHT 0x01 #define GRIPPER_CLOSE_RIGHT 0x02 #define GRIPPER_OPEN_LEFT 0x04 #define GRIPPER_CLOSE_LEFT 0x08 +#define GRIPPER_MOVING_LEFT 0x20 +#define GRIPPER_MOVING_RIGHT 0x10 +#define GRIPPER_OPENED_LEFT 0x80 +#define GRIPPER_OPENED_RIGHT 0x40 #define SMART_CHARGER_BASE_ADDRESS 0x0247 #define SMART_CHARGER_MEM_LENGTH 11 diff --git a/include/eeprom_init.h b/include/eeprom_init.h index dafe223..e7ae5eb 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -73,11 +73,13 @@ extern "C" { #define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16 #define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16 #define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 -#define DEFAULT_GRIPPER_LEFT_ID 0x0017 // ID 23 for the left gripper +#define DEFAULT_GRIPPER_LEFT_TOP_ID 0x0015 // ID 21 for the left gripper +#define DEFAULT_GRIPPER_LEFT_BOT_ID 0x0016 // ID 22 for the left gripper #define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 #define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 #define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format -#define DEFAULT_GRIPPER_RIGHT_ID 0x0018 // ID 24 for the left gripper +#define DEFAULT_GRIPPER_RIGHT_TOP_ID 0x0017 // ID 23 for the left gripper +#define DEFAULT_GRIPPER_RIGHT_BOT_ID 0x0018 // ID 24 for the left gripper #define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 #define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 #define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format diff --git a/include/grippers.h b/include/grippers.h index 9df31bc..9a1aa5f 100644 --- a/include/grippers.h +++ b/include/grippers.h @@ -16,13 +16,15 @@ inline void grippers_set_period(uint16_t period_us); inline uint16_t grippers_get_period(void); void grippers_open(grippers_t gripper); void grippers_close(grippers_t gripper); +void grippers_stop(grippers_t gripper); uint8_t gripper_is_open(grippers_t gripper); +uint8_t gripper_is_close(grippers_t gripper); +uint8_t gripper_is_moving(grippers_t gripper); // operation functions uint8_t grippers_in_range(unsigned short int address, unsigned short int length); void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -// motion manager interface functions void grippers_process(void); #ifdef __cplusplus diff --git a/include/motion_manager.h b/include/motion_manager.h index 641c39d..d0f234c 100755 --- a/include/motion_manager.h +++ b/include/motion_manager.h @@ -29,7 +29,11 @@ typedef enum { R_ANKLE_ROLL = 17, L_ANKLE_ROLL = 18, L_PAN = 19, - L_TILT = 20} servo_id_t; + L_TILT = 20, + L_GRIPPER_TOP = 21, + L_GRIPPER_BOT = 22, + R_GRIPPER_TOP = 23, + R_GRIPPER_BOT = 24} servo_id_t; typedef enum {MM_NONE = 0, @@ -38,9 +42,9 @@ typedef enum {MM_NONE = 0, MM_JOINTS = 3, MM_HEAD = 4} TModules; -typedef enum {MM_FWD_FALL =0, - MM_BWD_FALL =1, - MM_STANDING =2} TFall; +typedef enum {MM_FWD_FALL = 0, + MM_BWD_FALL = 1, + MM_STANDING = 2} TFall; typedef struct { diff --git a/src/eeprom.c b/src/eeprom.c index 4fa6f8e..fbb5a1a 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -139,14 +139,16 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1, DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP, DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1, - DEFAULT_GRIPPER_LEFT_ID, GRIPPER_LEFT_ID, + DEFAULT_GRIPPER_LEFT_TOP_ID, GRIPPER_LEFT_TOP_ID, + DEFAULT_GRIPPER_LEFT_BOT_ID, GRIPPER_LEFT_BOT_ID, DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE, DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1, DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE, DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1, DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE, DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1, - DEFAULT_GRIPPER_RIGHT_ID, GRIPPER_RIGHT_ID, + DEFAULT_GRIPPER_RIGHT_TOP_ID, GRIPPER_RIGHT_TOP_ID, + DEFAULT_GRIPPER_RIGHT_BOT_ID, GRIPPER_RIGHT_BOT_ID, DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE, DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1, DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF, GRIPPER_RIGHT_MIN_ANGLE, diff --git a/src/grippers.c b/src/grippers.c index c5fae08..5544346 100644 --- a/src/grippers.c +++ b/src/grippers.c @@ -2,42 +2,48 @@ #include "dyn_servos.h" #include "grippers.h" #include "ram.h" +#include "joint_motion.h" // private variables int64_t grippers_period; int16_t grippers_period_us; -uint8_t grippers_left_opened; -uint8_t grippers_left_open; -uint8_t grippers_left_close; -uint8_t grippers_right_opened; -uint8_t grippers_right_open; -uint8_t grippers_right_close; +uint8_t gripper_left_target; +uint8_t gripper_right_target; // public functions void grippers_init(uint16_t period_us) { uint16_t force; + uint32_t servo_mask=0x00000000; /* initialize private variables */ - grippers_left_opened=0x00; - grippers_left_open=0x00; - grippers_left_close=0x00; - grippers_right_opened=0x00; - grippers_right_open=0x00; - grippers_right_close=0x00; + gripper_left_target=0x00;//close; + gripper_right_target=0x00;//close; grippers_period=(period_us<<16)/1000000; grippers_period_us=period_us; /* configure the maximum force of the servos */ - if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF) + servo_mask=0x00000000; + if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF) { force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8); - dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_ID],XL_TORQUE_LIMIT_L,force); + dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force); + dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force); + servo_mask|=1< Date: Fri, 30 Jun 2017 12:52:20 +0200 Subject: [PATCH 05/30] Set the stop signal to 0 when a new motion is started to avoid false cancelations. Added a new variable to hold the angle required by the deacceleration phase with the initial command data, instead of computing it each cycle. Changed the max force default value of the grippers. --- include/eeprom_init.h | 4 ++-- src/joint_motion.c | 8 +++++++- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/include/eeprom_init.h b/include/eeprom_init.h index e7ae5eb..cb4543c 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -77,12 +77,12 @@ extern "C" { #define DEFAULT_GRIPPER_LEFT_BOT_ID 0x0016 // ID 22 for the left gripper #define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 #define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 -#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format +#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x0080 // 1023 max force in binary format #define DEFAULT_GRIPPER_RIGHT_TOP_ID 0x0017 // ID 23 for the left gripper #define DEFAULT_GRIPPER_RIGHT_BOT_ID 0x0018 // ID 24 for the left gripper #define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 #define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 -#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format +#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x0080 // 1023 max force in binary format #define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms diff --git a/src/joint_motion.c b/src/joint_motion.c index 8e8147a..395bfc0 100644 --- a/src/joint_motion.c +++ b/src/joint_motion.c @@ -6,6 +6,7 @@ int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16 int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16 int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16 +int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16 int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16 int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16 @@ -120,6 +121,8 @@ void joint_motion_start(uint8_t group_id) ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16; } } + // stop angles + joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]); // the current angles, speeds and accelerations are in RAM if(joint_target_angles[i]>=joint_current_angles[i]) joint_dir[i]=1; @@ -129,6 +132,7 @@ void joint_motion_start(uint8_t group_id) } } joint_moving[group_id]=0x01; + joint_stop[group_id]=0x00; ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS; } @@ -236,7 +240,7 @@ void joint_motion_process(void) } else { - if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase + if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase { joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16); if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) @@ -256,6 +260,8 @@ void joint_motion_process(void) } } } + else + joint_current_speeds[i]=0; manager_current_angles[i]=joint_current_angles[i]; manager_current_slopes[i]=5; } -- GitLab From 637a115e3ced62157bd81668ba440ca8ab559b11 Mon Sep 17 00:00:00 2001 From: darwin Date: Fri, 25 Aug 2017 00:02:01 +0200 Subject: [PATCH 06/30] Solved a problem with the sign of the accelerometer sensor. Chnaged the sign of the X axis of the gyroscope sensor. --- src/imu.c | 2 +- src/motion_manager.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/imu.c b/src/imu.c index e792585..bbaed72 100755 --- a/src/imu.c +++ b/src/imu.c @@ -258,7 +258,7 @@ void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out) for(i=0;i<3;i++) { - accel_data[i]=(data_in[i*2]+(data_in[i*2+1]<<8)); + accel_data[i]=(int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)); data_out[i*2]=accel_data[i]%256; data_out[i*2+1]=accel_data[i]/256; } diff --git a/src/motion_manager.c b/src/motion_manager.c index 65f06bf..8faa546 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -149,10 +149,10 @@ void manager_balance(void) manager_balance_offset[R_ANKLE_PITCH]=((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9); manager_balance_offset[L_KNEE]=((((int64_t)gyro_y*(int64_t)knee_gain)/12000)>>9); manager_balance_offset[L_ANKLE_PITCH]=-((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9); - manager_balance_offset[R_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9); - manager_balance_offset[L_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9); - manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9); - manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9); + manager_balance_offset[R_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9); + manager_balance_offset[L_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9); + manager_balance_offset[R_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9); + manager_balance_offset[L_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9); } // fall detection (using accelerometer) imu_get_accel_data(&accel_x,&accel_y,&accel_z); -- GitLab From 54d8e70eac35249c54e6101b0d77ffe0ec573913 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Fri, 25 Aug 2017 12:52:45 +0200 Subject: [PATCH 07/30] Changed one analog port to a digital output to control the power on the second dynamixel bus. --- include/adc_dma.h | 2 +- include/darwin_dyn_master_v2.h | 2 ++ include/darwin_registers.h | 9 +++----- include/smart_charger.h | 2 +- src/adc_dma.c | 38 +++++++++------------------------- src/darwin_dyn_master_v2.c | 16 ++++++++++++++ src/motion_manager.c | 4 ++++ 7 files changed, 37 insertions(+), 36 deletions(-) diff --git a/include/adc_dma.h b/include/adc_dma.h index 0fd1dc4..db29c8d 100755 --- a/include/adc_dma.h +++ b/include/adc_dma.h @@ -10,7 +10,7 @@ extern "C" { #define ADC_NUM_CHANNELS 12 typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7, - ADC_CH9=8,ADC_CH10=9,ADC_CH12=10,ADC_CH14=11} adc_ch_t; + ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_ch_t; void adc_init(void); void adc_start(void); diff --git a/include/darwin_dyn_master_v2.h b/include/darwin_dyn_master_v2.h index b4a1b34..a7e5d19 100644 --- a/include/darwin_dyn_master_v2.h +++ b/include/darwin_dyn_master_v2.h @@ -12,6 +12,8 @@ extern "C" { extern TDynamixelMaster darwin_dyn_master_v2; void darwin_dyn_master_v2_init(void); +inline void darwin_dyn_master_v2_enable_power(void); +inline void darwin_dyn_master_v2_disable_power(void); #ifdef __cplusplus } diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 8b5c191..6cf97e2 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -254,10 +254,6 @@ typedef enum { DARWIN_ADC_TEMP_H = 0x012C, DARWIN_ADC_CH12_L = 0x012D, DARWIN_ADC_CH12_H = 0x012E, - DARWIN_ADC_VREF_L = 0x012F, - DARWIN_ADC_VREF_H = 0x0130, - DARWIN_ADC_CH14_L = 0x0131, - DARWIN_ADC_CH14_H = 0x0132, DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // accel_det | gyro_det | calibrating | running | | start_cal | stop | start DARWIN_IMU_CAL_SAMPLES_L = 0x013C, @@ -276,8 +272,8 @@ typedef enum { DARWIN_IMU_ACCEL_Z_H = 0x0149, DARWIN_MM_NUM_SERVOS = 0x014A, - DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // scanning | fwd fall | bwd fall | | | Enable power | Enable balance | Enable manager + DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // scanning | fwd fall | bwd fall | | Enable power v2 | Enable power | Enable balance | Enable manager DARWIN_MM_MODULE_EN0 = 0x014C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i // Enable servo 0 | assigned module | Enable servo 1 | assigned module // | 000 -> none | @@ -630,6 +626,7 @@ typedef enum { #define MANAGER_ENABLE 0x01 #define MANAGER_EN_BAL 0x02 #define MANAGER_EN_PWR 0x04 +#define MANAGER_EN_PWR_V2 0x08 #define MANAGER_SCANNING 0x80 #define MANAGER_FWD_FALL 0x40 #define MANAGER_BWD_FALL 0x20 diff --git a/include/smart_charger.h b/include/smart_charger.h index 5d3587c..42c3217 100644 --- a/include/smart_charger.h +++ b/include/smart_charger.h @@ -118,4 +118,4 @@ void smart_charger_process(void); } #endif -#endif \ No newline at end of file +#endif diff --git a/src/adc_dma.c b/src/adc_dma.c index d24f466..eb0f821 100755 --- a/src/adc_dma.c +++ b/src/adc_dma.c @@ -7,15 +7,13 @@ #define ADC1_CH4 ADC_CHANNEL_7 #define ADC1_CH5 ADC_CHANNEL_8 #define ADC1_CH6 ADC_CHANNEL_TEMPSENSOR -#define ADC1_CH7 ADC_CHANNEL_VREFINT #define ADC2_CH1 ADC_CHANNEL_9 #define ADC2_CH2 ADC_CHANNEL_10 #define ADC2_CH3 ADC_CHANNEL_11 #define ADC2_CH4 ADC_CHANNEL_12 -#define ADC2_CH5 ADC_CHANNEL_13 -#define ADC2_CH6 ADC_CHANNEL_14 -#define ADC2_CH7 ADC_CHANNEL_15 +#define ADC2_CH5 ADC_CHANNEL_14 +#define ADC2_CH6 ADC_CHANNEL_15 #define ADC1_CH1_PIN GPIO_PIN_0 #define ADC1_CH1_PORT GPIOA @@ -47,15 +45,12 @@ #define ADC2_CH4_PIN GPIO_PIN_2 #define ADC2_CH4_PORT GPIOC #define ADC2_CH4_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE() -#define ADC2_CH5_PIN GPIO_PIN_3 +#define ADC2_CH5_PIN GPIO_PIN_4 #define ADC2_CH5_PORT GPIOC #define ADC2_CH5_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE() -#define ADC2_CH6_PIN GPIO_PIN_4 +#define ADC2_CH6_PIN GPIO_PIN_5 #define ADC2_CH6_PORT GPIOC #define ADC2_CH6_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE() -#define ADC2_CH7_PIN GPIO_PIN_5 -#define ADC2_CH7_PORT GPIOC -#define ADC2_CH7_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE() #define ADC2_ENABLE_CLK __ADC2_CLK_ENABLE() @@ -82,7 +77,7 @@ ADC_HandleTypeDef hadc2; DMA_HandleTypeDef hdma_adc1; TIM_HandleTypeDef ADC_TIM_Handle; -uint32_t adc_data[9];// temporal buffer to store ADC data before conversion +uint32_t adc_data[6];// temporal buffer to store ADC data before conversion uint16_t adc_period_ms; // private functions @@ -153,7 +148,7 @@ void ADC_DMA_IRQHandler(void) if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TC) != RESET) { __HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1)); - for(i=0;i<7;i++) + for(i=0;i<6;i++) { if(i==5) { @@ -202,7 +197,6 @@ void adc_init(void) ADC2_CH4_ENABLE_PORT_CLK; ADC2_CH5_ENABLE_PORT_CLK; ADC2_CH6_ENABLE_PORT_CLK; - ADC2_CH7_ENABLE_PORT_CLK; ADC_ENABLE_DMA_CLK; @@ -214,7 +208,7 @@ void adc_init(void) hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc1.Init.NbrOfConversion = 7; + hadc1.Init.NbrOfConversion = 6; HAL_ADC_Init(&hadc1); multimode.Mode = ADC_DUALMODE_REGSIMULT; @@ -246,10 +240,6 @@ void adc_init(void) sConfig.Rank = 6; HAL_ADC_ConfigChannel(&hadc1, &sConfig); - sConfig.Channel = ADC1_CH7; - sConfig.Rank = 7; - HAL_ADC_ConfigChannel(&hadc1, &sConfig); - // configure GPIO GPIO_InitStruct.Pin = ADC1_CH1_PIN|ADC1_CH2_PIN|ADC1_CH3_PIN|ADC1_CH4_PIN; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; @@ -266,7 +256,7 @@ void adc_init(void) hadc2.Init.DiscontinuousConvMode = DISABLE; hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;//ADC_EXTERNALTRIGCONV_T1_CC1; hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc2.Init.NbrOfConversion = 7; + hadc2.Init.NbrOfConversion = 6; HAL_ADC_Init(&hadc2); // configure ADC2 channels @@ -295,13 +285,9 @@ void adc_init(void) sConfig.Rank = 6; HAL_ADC_ConfigChannel(&hadc2, &sConfig); - sConfig.Channel = ADC2_CH7; - sConfig.Rank = 7; - HAL_ADC_ConfigChannel(&hadc2, &sConfig); - // configure GPIO GPIO_InitStruct.Pin = ADC2_CH2_PIN|ADC2_CH3_PIN|ADC2_CH4_PIN|ADC2_CH5_PIN| - ADC2_CH6_PIN|ADC2_CH7_PIN; + ADC2_CH6_PIN; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); @@ -366,7 +352,7 @@ void adc_init(void) HAL_ADCEx_Calibration_Start(&hadc2); HAL_ADC_Start(&hadc2); - HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,7); + HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,6); } void adc_start(void) @@ -426,8 +412,6 @@ uint16_t adc_get_channel(adc_ch_t channel) break; case ADC_CH12: value=ram_data[DARWIN_ADC_CH12_L]+ram_data[DARWIN_ADC_CH12_H]*256; break; - case ADC_CH14: value=ram_data[DARWIN_ADC_CH14_L]+ram_data[DARWIN_ADC_CH14_H]*256; - break; default: value=0x0000; } @@ -462,8 +446,6 @@ uint16_t adc_get_channel_raw(adc_ch_t channel) break; case ADC_CH12: value=(adc_data[5]&0xFFFF0000)>>16; break; - case ADC_CH14: value=(adc_data[6]&0xFFFF0000)>>16; - break; default: value=0x0000; } diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c index 5dc5e5d..5c61157 100755 --- a/src/darwin_dyn_master_v2.c +++ b/src/darwin_dyn_master_v2.c @@ -11,6 +11,10 @@ #define TX_EN_PIN GPIO_PIN_1 #define TX_EN_GPIO_PORT GPIOA +#define POWER_GPIO_CLK __GPIOC_CLK_ENABLE() +#define POWER_PIN GPIO_PIN_3 +#define POWER_GPIO_PORT GPIOC + /* private variables */ TDynamixelMaster darwin_dyn_master_v2; TTime darwin_dyn_master_v2_timer; @@ -81,3 +85,15 @@ void darwin_dyn_master_v2_init(void) dyn_master_set_return_level(&darwin_dyn_master_v2,return_all); } +inline void darwin_dyn_master_v2_enable_power(void) +{ + HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET); + ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2; +} + +inline void darwin_dyn_master_v2_disable_power(void) +{ + HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET); + ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2); +} + diff --git a/src/motion_manager.c b/src/motion_manager.c index 65f06bf..4d61378 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -692,6 +692,10 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len darwin_dyn_master_enable_power(); else darwin_dyn_master_disable_power(); + if(data[DARWIN_MM_CNTRL-address]&MANAGER_EN_PWR_V2) + darwin_dyn_master_v2_enable_power(); + else + darwin_dyn_master_v2_disable_power(); } // balance gains for(i=MM_BAL_KNEE_GAIN_OFFSET;i<=MM_BAL_HIP_ROLL_GAIN_OFFSET+1;i++) -- GitLab From 9b9c4030feb28d594b3a6ee585bd778f4a755531 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Fri, 25 Aug 2017 14:56:02 +0200 Subject: [PATCH 08/30] Initial implementation of the stairs module. --- include/darwin_registers.h | 48 ++++++ include/eeprom_init.h | 22 ++- include/stairs.h | 30 ++++ src/eeprom.c | 29 +++- src/stairs.c | 339 +++++++++++++++++++++++++++++++++++++ 5 files changed, 464 insertions(+), 4 deletions(-) create mode 100755 include/stairs.h create mode 100755 src/stairs.c diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 6cf97e2..4a4684d 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -86,6 +86,24 @@ extern "C" { #define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x0060) #define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062) #define SMART_CHARGER_PERIOD ((unsigned short int)0x0064) +#define STAIRS_PHASE1_TIME ((unsigned short int)0x0066) +#define STAIRS_PHASE2_TIME ((unsigned short int)0x0068) +#define STAIRS_PHASE3_TIME ((unsigned short int)0x006A) +#define STAIRS_PHASE4_TIME ((unsigned short int)0x006C) +#define STAIRS_PHASE5_TIME ((unsigned short int)0x006E) +#define STAIRS_PHASE6_TIME ((unsigned short int)0x0070) +#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 LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) @@ -192,6 +210,34 @@ typedef enum { DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1, DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1, + DARWIN_STAIRS_PHASE1_TIME_L = STAIRS_PHASE1_TIME, + DARWIN_STAIRS_PHASE1_TIME_H = STAIRS_PHASE1_TIME+1, + DARWIN_STAIRS_PHASE2_TIME_L = STAIRS_PHASE2_TIME, + DARWIN_STAIRS_PHASE2_TIME_H = STAIRS_PHASE2_TIME+1, + DARWIN_STAIRS_PHASE3_TIME_L = STAIRS_PHASE3_TIME, + DARWIN_STAIRS_PHASE3_TIME_H = STAIRS_PHASE3_TIME+1, + DARWIN_STAIRS_PHASE4_TIME_L = STAIRS_PHASE4_TIME, + DARWIN_STAIRS_PHASE4_TIME_H = STAIRS_PHASE4_TIME+1, + DARWIN_STAIRS_PHASE5_TIME_L = STAIRS_PHASE5_TIME, + DARWIN_STAIRS_PHASE5_TIME_H = STAIRS_PHASE5_TIME+1, + DARWIN_STAIRS_PHASE6_TIME_L = STAIRS_PHASE6_TIME, + DARWIN_STAIRS_PHASE6_TIME_H = STAIRS_PHASE6_TIME+1, + DARWIN_STAIRS_PHASE7_TIME_L = STAIRS_PHASE7_TIME, + DARWIN_STAIRS_PHASE7_TIME_H = STAIRS_PHASE7_TIME+1, + DARWIN_STAIRS_PHASE8_TIME_L = STAIRS_PHASE8_TIME, + 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_X_OFFSET = STAIRS_X_OFFSET, + DARWIN_STAIRS_Y_OFFSET = STAIRS_Y_OFFSET, + DARWIN_STAIRS_Z_OFFSET = STAIRS_Z_OFFSET, + DARWIN_STAIRS_Y_SHIFT = STAIRS_Y_SHIFT, + DARWIN_STAIRS_X_LEFT_SHIFT = STAIRS_X_LEFT_SHIFT, + 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, + //RAM DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // | | | | blink | toggle | value | internally used @@ -582,6 +628,8 @@ typedef enum { // | detected | enable 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_registers; #define GPIO_BASE_ADDRESS 0x0100 diff --git a/include/eeprom_init.h b/include/eeprom_init.h index cb4543c..570db02 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -83,9 +83,25 @@ extern "C" { #define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7 #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_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_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_Z_HEIGHT 0x001E // 30 mm +#define DEFAULT_STAIRS_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10 #ifdef __cplusplus } diff --git a/include/stairs.h b/include/stairs.h new file mode 100755 index 0000000..6907e9f --- /dev/null +++ b/include/stairs.h @@ -0,0 +1,30 @@ +#ifndef _STAIRS_H +#define _STAIRS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f1xx.h" +#include "motion_manager.h" + +// public functions +void stairs_init(uint16_t period_us); +inline void stairs_set_period(uint16_t period_us); +inline uint16_t stairs_get_period(void); +void stairs_start(void); +void stairs_stop(void); +uint8_t is_climbing_stairs(void); + +// operation functions +uint8_t stairs_in_range(unsigned short int address, unsigned short int length); +void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); +void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); +// motion manager interface functions +void stairs_process(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/eeprom.c b/src/eeprom.c index fbb5a1a..b6eae9e 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -156,7 +156,34 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE, 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_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1, + DEFAULT_STAIRS_PHASE1_TIME&0xFF, STAIRS_PHASE1_TIME, + DEFAULT_STAIRS_PHASE1_TIME>>8, STAIRS_PHASE1_TIME+1, + DEFAULT_STAIRS_PHASE2_TIME&0xFF, STAIRS_PHASE2_TIME, + DEFAULT_STAIRS_PHASE2_TIME>>8, STAIRS_PHASE2_TIME+1, + DEFAULT_STAIRS_PHASE3_TIME&0xFF, STAIRS_PHASE3_TIME, + DEFAULT_STAIRS_PHASE3_TIME>>8, STAIRS_PHASE3_TIME+1, + DEFAULT_STAIRS_PHASE4_TIME&0xFF, STAIRS_PHASE4_TIME, + DEFAULT_STAIRS_PHASE4_TIME>>8, STAIRS_PHASE4_TIME+1, + DEFAULT_STAIRS_PHASE5_TIME&0xFF, STAIRS_PHASE5_TIME, + DEFAULT_STAIRS_PHASE5_TIME>>8, STAIRS_PHASE5_TIME+1, + DEFAULT_STAIRS_PHASE6_TIME&0xFF, STAIRS_PHASE6_TIME, + DEFAULT_STAIRS_PHASE6_TIME>>8, STAIRS_PHASE6_TIME+1, + DEFAULT_STAIRS_PHASE7_TIME&0xFF, STAIRS_PHASE7_TIME, + DEFAULT_STAIRS_PHASE7_TIME>>8, STAIRS_PHASE7_TIME+1, + DEFAULT_STAIRS_PHASE8_TIME&0xFF, STAIRS_PHASE8_TIME, + 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_X_OFFSET, STAIRS_X_OFFSET, + DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET, + DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_OFFSET, + DEFAULT_STAIRS_Y_SHIFT, STAIRS_Y_SHIFT, + DEFAULT_STAIRS_X_LEFT_SHIFT, STAIRS_X_LEFT_SHIFT, + DEFAULT_STAIRS_X_RIGHT_SHIFT, STAIRS_X_RIGHT_SHIFT, + DEFAULT_STAIRS_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT, + DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT, + DEFAULT_STAIRS_HIP_PITCH_OFF, STAIRS_HIP_PITCH_OFF}; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ diff --git a/src/stairs.c b/src/stairs.c new file mode 100755 index 0000000..dfa470a --- /dev/null +++ b/src/stairs.c @@ -0,0 +1,339 @@ +#include "stairs.h" +#include "darwin_kinematics.h" +#include "darwin_math.h" +#include "ram.h" +#include + +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; + +// 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; + +// internal offset variables +float m_Hip_Pitch_Offset; +float m_X_Offset; +float m_Y_Offset; +float m_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; + +// control variables +uint8_t m_Ctrl_Running; + +// private functions + +// public functions +void stairs_init(uint16_t period_us) +{ + // initialize the internal motion variables + + + // initialize internal control variables + m_Time=0; + stairs_period=((float)period_us)/1000.0; + m_Ctrl_Running=0x00; + + stairs_process(); +} + +inline void stairs_set_period(uint16_t period_us) +{ + stairs_period=((float)period_us)/1000.0; +} + +inline uint16_t stairs_get_period(void) +{ + return (uint16_t)(stairs_period*1000); +} + +void stairs_start(void) +{ + ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS; + m_Ctrl_Running=0x01; +} + +void stairs_stop(void) +{ + m_Ctrl_Running=0x00; +} + +uint8_t is_climbing_stairs(void) +{ + if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS) + return 0x01; + else + return 0x00; +} + +// 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]; + + if(m_Time==0) + { + update_param_time(); + ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE); + ram_data[DARWIN_WALK_CNTRL]|=PHASE0; + if(m_Ctrl_Running==0x00) + { + 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; + } + } + } + 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) + { + 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; + } + } + } + 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;*/ + + // 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); + } + + if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS) + { + m_Time += stairs_period; + if(m_Time >= m_PeriodTime) + m_Time = 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)) + return 0x01; + else + return 0x00; +} + +void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + +} + +void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + uint16_t i; + + // walk control + if(ram_in_range(DARWIN_WALK_CNTRL,address,length)) + { + if(data[DARWIN_WALK_CNTRL-address]&WALK_START) + stairs_start(); + if(data[DARWIN_WALK_CNTRL-address]&WALK_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++) + if(ram_in_range(i,address,length)) + ram_data[i]=data[i-address]; +} + -- GitLab From ba9140904e686f00654bf07cd2d3567a9f8f51e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= Date: Sun, 27 Aug 2017 21:54:58 +0200 Subject: [PATCH 09/30] 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>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 #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 #include -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 From 7d7b711d334084db6c9052e69700757c19b50ef3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= Date: Sun, 27 Aug 2017 23:21:07 +0200 Subject: [PATCH 10/30] Changed all the hardcoded values to registers in the dynamixel interface. Cleaned up the stairs code. --- include/darwin_registers.h | 33 ++++-- include/eeprom.h | 2 +- include/eeprom_init.h | 32 +++--- src/eeprom.c | 16 ++- src/stairs.c | 206 ++++++++++++++++++++----------------- 5 files changed, 167 insertions(+), 122 deletions(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index f12214f..04c6658 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -95,16 +95,22 @@ 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_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_X_OFFSET ((unsigned short int)0x0078) +#define STAIRS_Y_OFFSET ((unsigned short int)0x0079) +#define STAIRS_Z_OFFSET ((unsigned short int)0x007A) +#define STAIRS_R_OFFSET ((unsigned short int)0x007B) +#define STAIRS_P_OFFSET ((unsigned short int)0x007C) +#define STAIRS_A_OFFSET ((unsigned short int)0x007D) +#define STAIRS_Y_SHIFT ((unsigned short int)0x007E) +#define STAIRS_X_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 STAIRS_R_SHIFT ((unsigned short int)0x0084) +#define STAIRS_P_SHIFT ((unsigned short int)0x0085) +#define STAIRS_A_SHIFT ((unsigned short int)0x0086) +#define STAIRS_Y_SPREAD ((unsigned short int)0x0087) +#define STAIRS_X_SHIFT_BODY ((unsigned short int)0x0088) #define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) @@ -229,18 +235,23 @@ 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, DARWIN_STAIRS_Y_SHIFT = STAIRS_Y_SHIFT, - DARWIN_STAIRS_X_LEFT_SHIFT = STAIRS_X_LEFT_SHIFT, - DARWIN_STAIRS_X_RIGHT_SHIFT = STAIRS_X_RIGHT_SHIFT, + DARWIN_STAIRS_R_OFFSET = STAIRS_R_OFFSET, + DARWIN_STAIRS_P_OFFSET = STAIRS_P_OFFSET, + DARWIN_STAIRS_A_OFFSET = STAIRS_A_OFFSET, + DARWIN_STAIRS_X_SHIFT = STAIRS_X_SHIFT, DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT, DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT, DARWIN_STAIRS_HIP_PITCH_OFF_L = STAIRS_HIP_PITCH_OFF, DARWIN_STAIRS_HIP_PITCH_OFF_H = STAIRS_HIP_PITCH_OFF+1, + DARWIN_STAIRS_R_SHIFT = STAIRS_R_SHIFT, + DARWIN_STAIRS_P_SHIFT = STAIRS_P_SHIFT, + DARWIN_STAIRS_A_SHIFT = STAIRS_A_SHIFT, + DARWIN_STAIRS_Y_SPREAD = STAIRS_Y_SPREAD, + DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY, //RAM DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 diff --git a/include/eeprom.h b/include/eeprom.h index c9c2f12..5d22788 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)0x83) +#define NB_OF_VAR ((uint8_t)0x89) /* Exported types ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/include/eeprom_init.h b/include/eeprom_init.h index db1405a..adf5729 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -84,27 +84,33 @@ 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 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_PHASE1_TIME 0x0640 // 1600 ms +#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 3200 ms +#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 4800 ms +#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 6400 ms +#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 8000 ms +#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 9600 ms +#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 11200 ms +#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 12800 ms +#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 14400 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_R_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_STAIRS_P_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_STAIRS_A_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 #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_X_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 DEFAULT_STAIRS_R_SHIFT 0x0017 // 2.875 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_STAIRS_P_SHIFT 0x002E // 5.73 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_STAIRS_A_SHIFT 0x0045 // 17.19 degrees in fixed point format 6 (1+5) | 2 +#define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm +#define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm -#define EEPROM_SIZE 131 +#define EEPROM_SIZE 137 #ifdef __cplusplus } diff --git a/src/eeprom.c b/src/eeprom.c index 142ad7a..ad139df 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -175,17 +175,23 @@ 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, + DEFAULT_STAIRS_R_OFFSET, STAIRS_R_OFFSET, + DEFAULT_STAIRS_P_OFFSET, STAIRS_P_OFFSET, + DEFAULT_STAIRS_A_OFFSET, STAIRS_A_OFFSET, DEFAULT_STAIRS_Y_SHIFT, STAIRS_Y_SHIFT, - DEFAULT_STAIRS_X_LEFT_SHIFT, STAIRS_X_LEFT_SHIFT, - DEFAULT_STAIRS_X_RIGHT_SHIFT, STAIRS_X_RIGHT_SHIFT, + DEFAULT_STAIRS_X_SHIFT, STAIRS_X_SHIFT, DEFAULT_STAIRS_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT, DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT, - DEFAULT_STAIRS_HIP_PITCH_OFF, STAIRS_HIP_PITCH_OFF}; + DEFAULT_STAIRS_HIP_PITCH_OFF&0xFF, STAIRS_HIP_PITCH_OFF, + DEFAULT_STAIRS_HIP_PITCH_OFF>>8, STAIRS_HIP_PITCH_OFF+1, + DEFAULT_STAIRS_R_SHIFT, STAIRS_R_SHIFT, + DEFAULT_STAIRS_P_SHIFT, STAIRS_P_SHIFT, + DEFAULT_STAIRS_A_SHIFT, STAIRS_A_SHIFT, + DEFAULT_STAIRS_Y_SPREAD, STAIRS_Y_SPREAD, + DEFAULT_STAIRS_X_SHIFT_BODY, STAIRS_X_SHIFT_BODY}; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ diff --git a/src/stairs.c b/src/stairs.c index 5cefc8e..dd9668f 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -5,20 +5,27 @@ #include #include -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; +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} stairs_phase_t; // internal motion variables 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; +float stairs_X_shift_amplitude; +float stairs_R_shift_amplitude; +float stairs_P_shift_amplitude; +float stairs_A_shift_amplitude; +float stairs_Y_spread_amplitude; +float stairs_X_shift_body; // internal offset variables float stairs_Hip_Pitch_Offset; float stairs_X_Offset; float stairs_Y_Offset; float stairs_Z_Offset; +float stairs_R_Offset; +float stairs_P_Offset; +float stairs_A_Offset; // internal time variables float stairs_Time; @@ -32,7 +39,6 @@ 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 stairs_Ctrl_Running; @@ -73,16 +79,22 @@ void stairs_start(void) 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_Z_stair_height=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_HEIGHT])); + stairs_Z_overshoot=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OVERSHOOT])); + stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT])); + stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_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_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/1440.0;// (r_shift/8)*(pi/180) + stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/1440.0;// (p_shift/8)*(pi/180) + stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180) + stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD])); + stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY])); 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])); + stairs_R_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]))*PI/1440.0; + stairs_P_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]))*PI/1440.0; + stairs_A_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]))*PI/1440.0; // start operation ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS; stairs_Ctrl_Running=0x01; @@ -105,6 +117,7 @@ uint8_t is_climbing_stairs(void) void stairs_process(void) { float angle[14]={0},ep[12]={0}; + float delta; if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) { @@ -112,156 +125,165 @@ void stairs_process(void) if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) { //1 + delta=stairs_Time/stairs_shift_weight_left_time; ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; ep[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_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[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; ep[8]=stairs_Z_Offset; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; } else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) { //2 + delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time); 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[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; 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); + ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; } else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) { //3 + delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time); 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[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta; 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[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; 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); + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; 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 + delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time); ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.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; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; 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); + delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body*delta; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; 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[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; 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; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; 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; + delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); + ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.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; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; 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); + delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; 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[3]=stairs_R_Offset+stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; 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); + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; 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 + delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time); 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[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; 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; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; } else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) { //9 + delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time); 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[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; 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; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; 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); -- GitLab From 787cde6e330edbd489ff878bd9acc69ffd6a2851 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= Date: Sun, 27 Aug 2017 23:22:31 +0200 Subject: [PATCH 11/30] Removed the printf's functins used for debugging. --- src/stairs.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/stairs.c b/src/stairs.c index dd9668f..22ac2e4 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -2,7 +2,6 @@ #include "darwin_kinematics.h" #include "darwin_math.h" #include "ram.h" -#include #include 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} stairs_phase_t; @@ -289,7 +288,6 @@ void stairs_process(void) 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; @@ -299,10 +297,7 @@ void stairs_process(void) // 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 - } // Compute motor value if(manager_get_module(R_HIP_YAW)==MM_WALKING) -- GitLab From 4c6f428355ee47642524c75a6d2302c8c7755aab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= Date: Mon, 28 Aug 2017 00:31:07 +0200 Subject: [PATCH 12/30] Added a function to get the current phase of the stairs algorithm. --- include/stairs.h | 1 + src/stairs.c | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/include/stairs.h b/include/stairs.h index 6907e9f..08f4ba5 100755 --- a/include/stairs.h +++ b/include/stairs.h @@ -15,6 +15,7 @@ inline uint16_t stairs_get_period(void); void stairs_start(void); void stairs_stop(void); uint8_t is_climbing_stairs(void); +uint8_t stairs_get_phase(void); // operation functions uint8_t stairs_in_range(unsigned short int address, unsigned short int length); diff --git a/src/stairs.c b/src/stairs.c index 22ac2e4..cf09087 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -112,6 +112,11 @@ uint8_t is_climbing_stairs(void) return 0x00; } +uint8_t stairs_get_phase(void) +{ + return (int8_t)ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE; +} + // motion manager interface functions void stairs_process(void) { -- GitLab From 20897b8df7d7335ed685825863b31d25cf60afd9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= Date: Mon, 28 Aug 2017 23:12:06 +0200 Subject: [PATCH 13/30] Modified the stairs trajectory to avoid collisions with the stairs. --- src/stairs.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/stairs.c b/src/stairs.c index cf09087..6a2eaf3 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -228,7 +228,7 @@ void stairs_process(void) ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); - ep[8]=stairs_Z_Offset+stairs_Z_stair_height; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; @@ -242,13 +242,13 @@ void stairs_process(void) ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; + ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; - ep[8]=stairs_Z_Offset+stairs_Z_stair_height; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; + ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; } @@ -260,13 +260,13 @@ void stairs_process(void) 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*delta; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset; + ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[5]=stairs_A_Offset; 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[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot+stairs_Z_overshoot*delta; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset; + ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset; ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; } -- GitLab From 52a03466d40b695745d92a38482b64906e1b0572 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= Date: Tue, 29 Aug 2017 20:58:15 +0200 Subject: [PATCH 14/30] Added the algorithm to go down stairs. Added the necessary changes in the dynamixel interface to execute it. --- include/darwin_registers.h | 9 +- include/stairs.h | 2 +- src/stairs.c | 510 +++++++++++++++++++++++++------------ 3 files changed, 350 insertions(+), 171 deletions(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 04c6658..cfa36fc 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -643,8 +643,8 @@ typedef enum { // | detected | enable 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 down | start stairs up }darwin_registers; #define GPIO_BASE_ADDRESS 0x0100 @@ -753,8 +753,9 @@ typedef enum { #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_START_UP 0x01 +#define STAIRS_START_DOWN 0x02 +#define STAIRS_STOP 0x04 #define STAIRS_STATUS 0x08 #define STAIRS_PHASE 0xF0 diff --git a/include/stairs.h b/include/stairs.h index 08f4ba5..4cb0bac 100755 --- a/include/stairs.h +++ b/include/stairs.h @@ -12,7 +12,7 @@ extern "C" { void stairs_init(uint16_t period_us); inline void stairs_set_period(uint16_t period_us); inline uint16_t stairs_get_period(void); -void stairs_start(void); +void stairs_start(uint8_t up); void stairs_stop(void); uint8_t is_climbing_stairs(void); uint8_t stairs_get_phase(void); diff --git a/src/stairs.c b/src/stairs.c index 6a2eaf3..6ad7d3c 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -41,6 +41,7 @@ float stairs_center_weight_time; // control variables uint8_t stairs_Ctrl_Running; +uint8_t stairs_up; // private functions @@ -54,6 +55,7 @@ void stairs_init(uint16_t period_us) stairs_Time=0; stairs_period=((float)period_us)/1000.0; stairs_Ctrl_Running=0x00; + stairs_up=0x00; } inline void stairs_set_period(uint16_t period_us) @@ -66,7 +68,7 @@ inline uint16_t stairs_get_period(void) return (uint16_t)(stairs_period*1000); } -void stairs_start(void) +void stairs_start(uint8_t up) { // load all parameters stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8)); @@ -97,6 +99,7 @@ void stairs_start(void) // start operation ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS; stairs_Ctrl_Running=0x01; + stairs_up=up; } void stairs_stop(void) @@ -126,172 +129,345 @@ void stairs_process(void) if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) { ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE); - if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) + if(stairs_up) { - //1 - delta=stairs_Time/stairs_shift_weight_left_time; - ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; - ep[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset; - ep[4]=stairs_P_Offset; - ep[5]=stairs_A_Offset; - ep[6]=stairs_X_Offset; - ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; - ep[8]=stairs_Z_Offset; - ep[9]=stairs_R_Offset; - ep[10]=stairs_P_Offset; - ep[11]=stairs_A_Offset; - ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; - } - else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) - { - //2 - delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time); - ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; - ep[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset; - ep[5]=stairs_A_Offset; - 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)*delta; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset; - ep[11]=stairs_A_Offset; - ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; - } - else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) - { - //3 - delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time); - ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta; - ep[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; - ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; - ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; - ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; - ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; - ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; - 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 - delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time); - ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); - ep[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; - ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; - ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; - ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); - ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; - ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; - 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 - delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time); - ep[0]=stairs_X_Offset-stairs_X_shift_body*delta; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; - ep[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; - ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; - ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; - ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; - ep[8]=stairs_Z_Offset+stairs_Z_stair_height; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; - ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; - 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 - delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time); - ep[0]=stairs_X_Offset-stairs_X_shift_body; - ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); - ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; - ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; - ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; - ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); - ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta; - ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; - ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; - 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 - delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time); - ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta; - ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; - ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; - ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; - ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; - ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; - ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot; - ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; - ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; - 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 - delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time); - 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*delta; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; - ep[5]=stairs_A_Offset; - 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-stairs_Z_overshoot+stairs_Z_overshoot*delta; - ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; - ep[11]=stairs_A_Offset; - ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; - } - else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) - { - //9 - delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time); - ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; - ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; - ep[3]=stairs_R_Offset; - ep[4]=stairs_P_Offset; - ep[5]=stairs_A_Offset; - ep[6]=stairs_X_Offset; - ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; - ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; - ep[9]=stairs_R_Offset; - ep[10]=stairs_P_Offset; - ep[11]=stairs_A_Offset; - ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; + if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) + { + //1 + delta=stairs_Time/stairs_shift_weight_left_time; + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; + ep[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; + } + else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) + { + //2 + delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; + ep[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + 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)*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; + } + else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) + { + //3 + delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta; + ep[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + 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 + delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); + ep[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + 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 + delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body*delta; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; + ep[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + 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 + delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); + ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + 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 + delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; + ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; + 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 + delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time); + 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*delta; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset; + 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-stairs_Z_overshoot+stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; + } + else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) + { + //9 + delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; + } + else + { + ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); + stairs_Ctrl_Running=0x00; + } } else - { - ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); - stairs_Ctrl_Running=0x00; + { + if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) + { + //1 + delta=stairs_Time/stairs_shift_weight_left_time; + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; + ep[2]=stairs_Z_Offset+stairs_Z_stair_height*delta; + ep[3]=stairs_R_Offset; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height*delta; + ep[9]=stairs_R_Offset; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; + } + else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) + { + //2 + delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; + ep[2]=stairs_Z_Offset+stairs_Z_stair_height; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + 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+stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; + } + else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) + { + //3 + delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta; + ep[2]=stairs_Z_Offset+stairs_Z_stair_height; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; + 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 + delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); + ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_stair_height*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + 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 + delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body*delta; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; + ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; + ep[8]=stairs_Z_Offset+stairs_Z_overshoot; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + 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 + delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); + ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); + ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; + 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 + delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time); + ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; + ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*stairs_P_shift_amplitude*delta; + ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; + ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; + ep[8]=stairs_Z_Offset; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*stairs_P_shift_amplitude*delta; + ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; + 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 + delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time); + 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_stair_height+stairs_Z_overshoot)*delta; + ep[3]=stairs_R_Offset+2.0*stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude; + ep[8]=stairs_Z_Offset; + ep[9]=stairs_R_Offset+2.0*stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; + } + else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) + { + //9 + delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time); + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[2]=stairs_Z_Offset; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset; + ep[5]=stairs_A_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[8]=stairs_Z_Offset; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset; + ep[11]=stairs_A_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; + } + else + { + ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); + stairs_Ctrl_Running=0x00; + } } if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) @@ -358,8 +534,10 @@ void stairs_process_write_cmd(unsigned short int address,unsigned short int leng // walk control if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length)) { - if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START) - stairs_start(); + if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP) + stairs_start(0x01); + if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN) + stairs_start(0x00); if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP) stairs_stop(); } -- GitLab From 1d5bb0ca753662c072ff868c308dec2aca6058d2 Mon Sep 17 00:00:00 2001 From: ferranmafe Date: Thu, 31 Aug 2017 18:54:48 +0200 Subject: [PATCH 15/30] Added the enable and disable power functions for the second dynamixel bus. --- src/cm730_fw.c | 1 + src/darwin_dyn_master_v2.c | 1 + src/motion_manager.c | 4 +++- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/cm730_fw.c b/src/cm730_fw.c index 5c1040f..7693bac 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -7,6 +7,7 @@ #include "darwin_time.h" #include "darwin_dyn_slave.h" #include "darwin_dyn_master.h" +#include "darwin_dyn_master_v2.h" #include "motion_manager.h" #include "action.h" #include "action_id.h" diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c index 5c61157..6152125 100755 --- a/src/darwin_dyn_master_v2.c +++ b/src/darwin_dyn_master_v2.c @@ -83,6 +83,7 @@ void darwin_dyn_master_v2_init(void) /* configure dynamixel master module */ dyn_master_set_rx_timeout(&darwin_dyn_master_v2,50); dyn_master_set_return_level(&darwin_dyn_master_v2,return_all); + darwin_dyn_master_v2_disable_power(); } inline void darwin_dyn_master_v2_enable_power(void) diff --git a/src/motion_manager.c b/src/motion_manager.c index 2e72f34..805fc18 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -365,6 +365,7 @@ void manager_init(uint16_t period_us) darwin_dyn_master_disable_power(); + darwin_dyn_master_v2_enable_power(); // detect the servos connected on the v2 bus dyn_master_scan(&darwin_dyn_master_v2,&num,servo_ids); ram_data[DARWIN_MM_NUM_SERVOS]+=num; @@ -425,7 +426,8 @@ void manager_init(uint16_t period_us) default: break; } } - + + darwin_dyn_master_v2_disable_power(); ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF); ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8); -- GitLab From 3e658674e8fcf05018d5ffe7f49560c60f2d4b6e Mon Sep 17 00:00:00 2001 From: ferranmafe Date: Thu, 31 Aug 2017 20:55:10 +0200 Subject: [PATCH 16/30] Changed to the remmaped version of the USART1 module. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 6e95e7d..ff8179a 100755 --- a/Makefile +++ b/Makefile @@ -95,7 +95,7 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal.c TARGET_FILES+=$(USART_PATH)/src/usart3.c TARGET_FILES+=$(USART_PATH)/src/usart2.c -TARGET_FILES+=$(USART_PATH)/src/usart1.c +TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o)) DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP)) -- GitLab From fb1a3d8849bdf6e621cceaf0ece593333f691ef9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= Date: Sat, 2 Sep 2017 15:36:14 +0200 Subject: [PATCH 17/30] Modified the stairs up algorithm to make it more stable in the real robot. --- src/stairs.c | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/stairs.c b/src/stairs.c index 6ad7d3c..3947bd6 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -136,7 +136,7 @@ void stairs_process(void) //1 delta=stairs_Time/stairs_shift_weight_left_time; ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; + ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; ep[2]=stairs_Z_Offset; ep[3]=stairs_R_Offset; ep[4]=stairs_P_Offset; @@ -154,7 +154,7 @@ void stairs_process(void) //2 delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time); ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; + ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; ep[2]=stairs_Z_Offset; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta; ep[4]=stairs_P_Offset; @@ -172,7 +172,7 @@ void stairs_process(void) //3 delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time); ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta; + ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta; ep[2]=stairs_Z_Offset; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; @@ -190,7 +190,7 @@ void stairs_process(void) //4 delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time); ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); + ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); ep[2]=stairs_Z_Offset; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; @@ -208,16 +208,16 @@ void stairs_process(void) //5 delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time); ep[0]=stairs_X_Offset-stairs_X_shift_body*delta; - ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; + ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[2]=stairs_Z_Offset; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[8]=stairs_Z_Offset+stairs_Z_stair_height; ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; } @@ -226,16 +226,16 @@ void stairs_process(void) //6 delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time); ep[0]=stairs_X_Offset-stairs_X_shift_body; - ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); + ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; } @@ -244,16 +244,16 @@ void stairs_process(void) //7 delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time); ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta; - ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; + ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; + ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; } @@ -262,7 +262,7 @@ void stairs_process(void) //8 delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time); ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude; + 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*delta; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; @@ -280,7 +280,7 @@ void stairs_process(void) //9 delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time); ep[0]=stairs_X_Offset; - ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; + ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; ep[3]=stairs_R_Offset; ep[4]=stairs_P_Offset; -- GitLab From 2c9cf1d62bc394b271f036bac8ce2464d06aa127 Mon Sep 17 00:00:00 2001 From: ferranmafe Date: Sat, 2 Sep 2017 15:44:08 +0200 Subject: [PATCH 18/30] Changed the size of the EEPROM memory region for the stairs module. --- include/darwin_registers.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/darwin_registers.h b/include/darwin_registers.h index cfa36fc..b84afcc 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -752,7 +752,7 @@ typedef enum { #define STAIRS_BASE_ADDRESS 0x0252 #define STAIRS_MEM_LENGTH 1 #define STAIRS_EEPROM_ADDRESS 0x0066 -#define STAIRS_EEPROM_LENGTH 30 +#define STAIRS_EEPROM_LENGTH 35 #define STAIRS_START_UP 0x01 #define STAIRS_START_DOWN 0x02 #define STAIRS_STOP 0x04 -- GitLab From 05bea09c4b3655caaf29ca932c94041a7323eb6f Mon Sep 17 00:00:00 2001 From: shernand Date: Sat, 2 Sep 2017 19:16:54 +0200 Subject: [PATCH 19/30] Added the PID parameters values to the stairs algorithm. Changed the coding of the roll and pitch shift parameters. Modified the upstairs algorithm to work in the real robot. --- include/eeprom_init.h | 4 +-- src/stairs.c | 74 +++++++++++++++++++++++++++++++------------ 2 files changed, 55 insertions(+), 23 deletions(-) diff --git a/include/eeprom_init.h b/include/eeprom_init.h index adf5729..09eca5c 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -104,8 +104,8 @@ extern "C" { #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 DEFAULT_STAIRS_R_SHIFT 0x0017 // 2.875 degrees in fixed point format 5 (1+4) | 3 -#define DEFAULT_STAIRS_P_SHIFT 0x002E // 5.73 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_STAIRS_R_SHIFT 0x000B // 2.875 degrees in fixed point format 5 (1+5) | 2 +#define DEFAULT_STAIRS_P_SHIFT 0x0017 // 5.73 degrees in fixed point format 5 (1+5) | 2 #define DEFAULT_STAIRS_A_SHIFT 0x0045 // 17.19 degrees in fixed point format 6 (1+5) | 2 #define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm #define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm diff --git a/src/stairs.c b/src/stairs.c index 3947bd6..18adb7f 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -85,8 +85,8 @@ void stairs_start(uint8_t up) stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT])); stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_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_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/1440.0;// (r_shift/8)*(pi/180) - stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/1440.0;// (p_shift/8)*(pi/180) + stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/720.0;// (r_shift/4)*(pi/180) + stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(pi/180) stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180) stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD])); stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY])); @@ -210,14 +210,14 @@ void stairs_process(void) ep[0]=stairs_X_Offset-stairs_X_shift_body*delta; ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[2]=stairs_Z_Offset; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[8]=stairs_Z_Offset+stairs_Z_stair_height; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; } @@ -228,13 +228,13 @@ void stairs_process(void) ep[0]=stairs_X_Offset-stairs_X_shift_body; ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); - ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta; - ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot*delta; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; @@ -251,7 +251,7 @@ void stairs_process(void) ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; - ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; @@ -269,7 +269,7 @@ void stairs_process(void) ep[5]=stairs_A_Offset; 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-stairs_Z_overshoot+stairs_Z_overshoot*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot+stairs_Z_overshoot*delta; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset; @@ -381,13 +381,13 @@ void stairs_process(void) ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(4.0*stairs_P_shift_amplitude)*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[8]=stairs_Z_Offset+stairs_Z_overshoot; ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(3.0*stairs_P_shift_amplitude)*delta; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(4.0*stairs_P_shift_amplitude)*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; } @@ -399,13 +399,13 @@ void stairs_process(void) ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; + ep[4]=stairs_P_Offset-3.0*stairs_P_shift_amplitude; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; + ep[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; } @@ -417,13 +417,13 @@ void stairs_process(void) ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*stairs_P_shift_amplitude*delta; + ep[4]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.0*stairs_P_shift_amplitude*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; ep[8]=stairs_Z_Offset; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*stairs_P_shift_amplitude*delta; + ep[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.0*stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; } @@ -482,33 +482,65 @@ void stairs_process(void) // 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; + manager_current_slopes[R_HIP_YAW]=5; + } if(manager_get_module(R_HIP_ROLL)==MM_WALKING) + { manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0; + manager_current_slopes[R_HIP_ROLL]=5; + } 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; + manager_current_slopes[R_HIP_PITCH]=5; + } if(manager_get_module(R_KNEE)==MM_WALKING) + { manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0; + manager_current_slopes[R_KNEE]=5; + } if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING) + { manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0; + manager_current_slopes[R_ANKLE_PITCH]=5; + } if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING) + { manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0; + manager_current_slopes[R_ANKLE_ROLL]=5; + } if(manager_get_module(L_HIP_YAW)==MM_WALKING) + { manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0; + manager_current_slopes[L_HIP_YAW]=5; + } if(manager_get_module(L_HIP_ROLL)==MM_WALKING) + { manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0; + manager_current_slopes[L_HIP_ROLL]=5; + } 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; + manager_current_slopes[L_HIP_PITCH]=5; + } if(manager_get_module(L_KNEE)==MM_WALKING) + { manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0; + manager_current_slopes[L_KNEE]=5; + } if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING) + { manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0; + manager_current_slopes[L_ANKLE_PITCH]=5; + } 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; + manager_current_slopes[L_ANKLE_ROLL]=5; + } } } -- GitLab From c8bf191169e6b76f093cf979fb35efde8b88e7c2 Mon Sep 17 00:00:00 2001 From: shernand Date: Sat, 2 Sep 2017 20:53:32 +0200 Subject: [PATCH 20/30] Modified the downstairs algorithm to work in the real robot. --- src/stairs.c | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/stairs.c b/src/stairs.c index 18adb7f..68795d9 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -233,7 +233,7 @@ void stairs_process(void) ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); - ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; @@ -251,7 +251,7 @@ void stairs_process(void) ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; - ep[8]=stairs_Z_Offset+stairs_Z_stair_height;//-stairs_Z_overshoot; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; @@ -269,7 +269,7 @@ void stairs_process(void) ep[5]=stairs_A_Offset; 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;//-stairs_Z_overshoot+stairs_Z_overshoot*delta; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset; @@ -380,14 +380,14 @@ void stairs_process(void) ep[0]=stairs_X_Offset-stairs_X_shift_body*delta; ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot; - ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(4.0*stairs_P_shift_amplitude)*delta; + ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; ep[8]=stairs_Z_Offset+stairs_Z_overshoot; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(4.0*stairs_P_shift_amplitude)*delta; + ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; } @@ -398,14 +398,14 @@ void stairs_process(void) ep[0]=stairs_X_Offset-stairs_X_shift_body; ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset-3.0*stairs_P_shift_amplitude; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; + ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta; - ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; + ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; } @@ -416,14 +416,14 @@ void stairs_process(void) ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta; ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[4]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.0*stairs_P_shift_amplitude*delta; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta; + ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta; ep[8]=stairs_Z_Offset; - ep[9]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta; - ep[10]=stairs_P_Offset-3.0*stairs_P_shift_amplitude+3.0*stairs_P_shift_amplitude*delta; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta; + ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta; ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; } @@ -434,13 +434,13 @@ void stairs_process(void) 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_stair_height+stairs_Z_overshoot)*delta; - ep[3]=stairs_R_Offset+2.0*stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; ep[4]=stairs_P_Offset; ep[5]=stairs_A_Offset; ep[6]=stairs_X_Offset; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude; ep[8]=stairs_Z_Offset; - ep[9]=stairs_R_Offset+2.0*stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; + ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; ep[10]=stairs_P_Offset; ep[11]=stairs_A_Offset; ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; -- GitLab From 834104401f6de85348fc4a1c3f9697eae676d2f0 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Tue, 5 Sep 2017 01:11:10 +0200 Subject: [PATCH 21/30] Solved a problem with the EEPROM leveleing algorithm. --- src/eeprom.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/eeprom.c b/src/eeprom.c index ad139df..fd7d1f0 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -773,7 +773,7 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data) HAL_StatusTypeDef flashstatus = HAL_OK; uint32_t newpageaddress = EEPROM_START_ADDRESS; uint32_t oldpageid = 0; - uint16_t validpage = PAGE0, varidx = 0; + uint16_t validpage = PAGE0, varidx = 0, address=0; uint16_t eepromstatus = 0, readstatus = 0; uint32_t page_error = 0; FLASH_EraseInitTypeDef s_eraseinit; @@ -821,15 +821,16 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data) /* Transfer process: transfer variables from old to the new active page */ for (varidx = 0; varidx < NB_OF_VAR; varidx++) { - if (eeprom_data[varidx] != VirtAddress) /* Check each variable except the one passed as parameter */ + address=((__IO uint16_t*)oldpageid)[1+varidx*2]; + if (address != VirtAddress) /* Check each variable except the one passed as parameter */ { /* Read the other last variable updates */ - readstatus = EE_ReadVariable(eeprom_data[varidx], &DataVar); + readstatus = EE_ReadVariable(address, &DataVar); /* In case variable corresponding to the virtual address was found */ if (readstatus != 0x1) { /* Transfer the variable to the new active page */ - eepromstatus = EE_VerifyPageFullWriteVariable(eeprom_data[varidx], DataVar); + eepromstatus = EE_VerifyPageFullWriteVariable(address, DataVar); /* If program operation was failed, a Flash error code is returned */ if (eepromstatus != HAL_OK) { -- GitLab From 54f969633cc83ce9dd20322d5faf33d8ae013d42 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Thu, 15 Feb 2018 15:00:56 +0100 Subject: [PATCH 22/30] Chnaged the GPIO module to use the memory data structures. Added a general configuration file for all modules (darwin_conf.h). --- Makefile | 52 +++++++++------- include/darwin_conf.h | 10 ++++ include/darwin_registers.h | 51 ---------------- include/gpio.h | 8 +-- include/gpio_registers.h | 56 +++++++++++++++++ include/ram.h | 37 ------------ src/cm730_fw.c | 38 +++++++----- src/gpio.c | 118 +++++++++++++++++++----------------- src/ram.c | 119 ------------------------------------- 9 files changed, 188 insertions(+), 301 deletions(-) create mode 100644 include/darwin_conf.h create mode 100644 include/gpio_registers.h delete mode 100755 include/ram.h delete mode 100755 src/ram.c diff --git a/Makefile b/Makefile index ff8179a..c603cfd 100755 --- a/Makefile +++ b/Makefile @@ -10,26 +10,25 @@ TARGET_FILES=src/cm730_fw.c #TARGET_FILES+=src/test_charger.c TARGET_FILES+=src/system_stm32f1xx.c TARGET_FILES+=src/gpio.c -TARGET_FILES+=src/ram.c -TARGET_FILES+=src/darwin_time.c +#TARGET_FILES+=src/darwin_time.c TARGET_FILES+=src/eeprom.c -TARGET_FILES+=src/adc_dma.c -TARGET_FILES+=src/imu.c -TARGET_FILES+=src/darwin_dyn_slave.c -TARGET_FILES+=src/darwin_dyn_master.c -TARGET_FILES+=src/darwin_dyn_master_v2.c +#TARGET_FILES+=src/adc_dma.c +#TARGET_FILES+=src/imu.c +#TARGET_FILES+=src/darwin_dyn_slave.c +#TARGET_FILES+=src/darwin_dyn_master.c +#TARGET_FILES+=src/darwin_dyn_master_v2.c TARGET_FILES+=src/stm32f1xx_hal_msp.c -TARGET_FILES+=src/motion_manager.c -TARGET_FILES+=src/action.c -TARGET_FILES+=src/motion_pages.c -TARGET_FILES+=src/walking.c -TARGET_FILES+=src/darwin_math.c -TARGET_FILES+=src/darwin_kinematics.c -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_FILES+=src/motion_manager.c +#TARGET_FILES+=src/action.c +#TARGET_FILES+=src/motion_pages.c +#TARGET_FILES+=src/walking.c +#TARGET_FILES+=src/darwin_math.c +#TARGET_FILES+=src/darwin_kinematics.c +#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 @@ -41,6 +40,7 @@ STM32_STARTUP_FILES_PATH = $(HAL_PATH)/startup_code/ STM32_LINKER_SCRIPTS_PATH = ./linker_script UTILS_PATH=$(STM32_LIBRARIES_PATH)/utils COMM_PATH=$(STM32_LIBRARIES_PATH)/comm +MEMORY_PATH=$(STM32_LIBRARIES_PATH)/memory USART_PATH=$(STM32_LIBRARIES_PATH)/f1/usart DYNAMIXEL_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_base BUILD_PATH=build @@ -49,7 +49,7 @@ COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float -DUSE_HAL_DRIVER COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices -INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include +INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include DOC_DIR = ./doc @@ -63,7 +63,6 @@ ASFLAGS = $(COMPILE_OPTS) -c LD = $(TCHAIN_PREFIX)gcc LDFLAGS = -mthumb -mcpu=cortex-m3 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/darwin.ld --specs=nosys.specs -EXT_LIB = $(COMM_PATH)/lib/comm_m3.a $(UTILS_PATH)/lib/utils_m3.a $(DYNAMIXEL_PATH)/lib/dynamixel_m3.a OBJCP = $(TCHAIN_PREFIX)objcopy OBJCPFLAGS_HEX = -O ihex @@ -97,6 +96,11 @@ TARGET_FILES+=$(USART_PATH)/src/usart3.c TARGET_FILES+=$(USART_PATH)/src/usart2.c TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c +TARGET_FILES+=$(wildcard $(UTILS_PATH)/src/*.c) +TARGET_FILES+=$(wildcard $(COMM_PATH)/src/*.c) +TARGET_FILES+=$(wildcard $(MEMORY_PATH)/src/*.c) +TARGET_FILES+=$(wildcard $(DYNAMIXEL_PATH)/src/*.c) + DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o)) DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP)) @@ -113,6 +117,14 @@ $(BUILD_PATH)/%.o: $(HAL_PATH)/src/%.c $(CC) -c $(CFLAGS) -o $@ $< $(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c $(CC) -c $(CFLAGS) -o $@ $< +$(BUILD_PATH)/%.o: $(UTILS_PATH)/src/%.c + $(CC) -c $(CFLAGS) -o $@ $< +$(BUILD_PATH)/%.o: $(COMM_PATH)/src/%.c + $(CC) -c $(CFLAGS) -o $@ $< +$(BUILD_PATH)/%.o: $(MEMORY_PATH)/src/%.c + $(CC) -c $(CFLAGS) -o $@ $< +$(BUILD_PATH)/%.o: $(DYNAMIXEL_PATH)/src/%.c + $(CC) -c $(CFLAGS) -o $@ $< $(MAIN_OUT_ELF): mkdir_build $(DARWIN_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) $(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@ diff --git a/include/darwin_conf.h b/include/darwin_conf.h new file mode 100644 index 0000000..f88b914 --- /dev/null +++ b/include/darwin_conf.h @@ -0,0 +1,10 @@ +#ifndef _DARWIN_CONF_H +#define _DARWIN_CONF_H + +#define RAM_SIZE 1024 +#define EEPROM_SIZE 256 + +/* GPIO configuration */ +#define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100) + +#endif diff --git a/include/darwin_registers.h b/include/darwin_registers.h index b84afcc..bd2cc32 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -254,40 +254,6 @@ typedef enum { DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY, //RAM - DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | | blink | toggle | value | internally used - DARWIN_TX_LED_PERIOD_L = 0x0101, - DARWIN_TX_LED_PERIOD_H = 0x0102, - DARWIN_RX_LED_CNTRL = 0x0103, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | | blink | toggle | value | internally used - DARWIN_RX_LED_PERIOD_L = 0x0104, - DARWIN_RX_LED_PERIOD_H = 0x0105, - DARWIN_PLAY_LED_CNTRL = 0x0106, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | | blink | toggle | value | internally used - DARWIN_PLAY_LED_PERIOD_L = 0x0107, - DARWIN_PLAY_LED_PERIOD_H = 0x0108, - DARWIN_EDIT_LED_CNTRL = 0x0109, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | | blink | toggle | value | internally used - DARWIN_EDIT_LED_PERIOD_L = 0x010A, - DARWIN_EDIT_LED_PERIOD_H = 0x010B, - DARWIN_MNG_LED_CNTRL = 0x010C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | | | toggle | value | internally used - DARWIN_AUX1_LED_CNTRL = 0x010D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used - DARWIN_AUX1_LED_COLOR_L = 0x010E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // green value LSB | red value - DARWIN_AUX1_LED_COLOR_H = 0x010F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | blue value | green value MSB - DARWIN_AUX2_LED_CNTRL = 0x0110, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used - DARWIN_AUX2_LED_COLOR_L = 0x0111, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // green value LSB | red value - DARWIN_AUX2_LED_COLOR_H = 0x0112, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | blue value | green value MSB - DARWIN_START_PB_CNTRL = 0x0113, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | | | | value | internally used - DARWIN_MODE_PB_CNTRL = 0x0114, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | | | | value | internally used DARWIN_ADC_CNTRL = 0x0115, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // | | | running | | | start | stop DARWIN_ADC_PERIOD = 0x0116, @@ -647,23 +613,6 @@ typedef enum { // current phase | climbing | stop stairs | start stairs down | start stairs up }darwin_registers; -#define GPIO_BASE_ADDRESS 0x0100 -#define GPIO_MEM_LENGTH 21 -#define GPIO_INT_USED 0x01 -#define GPIO_VALUE 0x02 -#define GPIO_TOGGLE 0x04 -#define GPIO_VALUE_R 0x02 -#define GPIO_TOGGLE_R 0x04 -#define GPIO_VALUE_G 0x08 -#define GPIO_TOGGLE_G 0x10 -#define GPIO_VALUE_B 0x20 -#define GPIO_TOGGLE_B 0x40 -#define GPIO_BLINK 0x08 -#define GPIO_COLOR 0x80 -#define GPIO_RED_COLOR 0x001F -#define GPIO_GREEN_COLOR 0x03E0 -#define GPIO_BLUE_COLOR 0x7C00 - #define ADC_BASE_ADDRESS 0x0115 #define ADC_MEM_LENGTH 30 #define ADC_START 0x01 diff --git a/include/gpio.h b/include/gpio.h index 902deed..cd2d43f 100755 --- a/include/gpio.h +++ b/include/gpio.h @@ -6,6 +6,8 @@ extern "C" { #endif #include "stm32f1xx.h" +#include "gpio_registers.h" +#include "memory.h" #define GPIO_RGB(R,G,B) ((R&0x1F)+((G&0x1F)<<5)+((B&0x1F)<<10)) @@ -30,7 +32,7 @@ typedef struct typedef enum {START_PB=0,MODE_PB=1} pushbutton_t; -void gpio_init(void); +uint8_t gpio_init(TMemory *memory); // LED functions void gpio_set_led(led_t led_id); void gpio_clear_led(led_t led_id); @@ -40,10 +42,6 @@ void gpio_set_color(led_t led_id, uint16_t value); // Pushbuttons functions uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id); void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void)); -// operation functions -uint8_t gpio_in_range(unsigned short int address,unsigned short int length); -void gpio_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -void gpio_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); #ifdef __cplusplus } diff --git a/include/gpio_registers.h b/include/gpio_registers.h new file mode 100644 index 0000000..c5376e3 --- /dev/null +++ b/include/gpio_registers.h @@ -0,0 +1,56 @@ +#ifndef _GPIO_REGISTERS_H +#define _GPIO_REGISTERS_H + +#ifndef RAM_GPIO_BASE_ADDRESS + #define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0000) +#endif + +#define RAM_GPIO_LENGTH 21 + +#define TX_LED_CNTRL RAM_GPIO_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | blink | toggle | value | internally used +#define TX_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+1) +#define RX_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+3)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | blink | toggle | value | internally used +#define RX_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+4) +#define PLAY_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+6)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | blink | toggle | value | internally used +#define PLAY_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+7) +#define EDIT_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+9)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | blink | toggle | value | internally used +#define EDIT_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+10) +#define MNG_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+12)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | toggle | value | internally used +#define AUX1_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+13)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used +#define AUX1_LED_COLOR (RAM_GPIO_BASE_ADDRESS+14)// bit 15 | bit 14 | bit 13 | bit 12 | bit 11 | bit 10 | bit 9 | bit 8 + // green value LSB | red value + // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | blue value | green value MSB +#define AUX2_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+16)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used +#define AUX2_LED_COLOR (RAM_GPIO_BASE_ADDRESS+17)// bit 15 | bit 14 | bit 13 | bit 12 | bit 11 | bit 10 | bit 9 | bit 8 + // green value LSB | red value + // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | blue value | green value MSB +#define START_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+19)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | | value | internally used +#define MODE_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+20)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + +#define GPIO_INT_USED 0x01 +#define GPIO_VALUE 0x02 +#define GPIO_TOGGLE 0x04 +#define GPIO_VALUE_R 0x02 +#define GPIO_TOGGLE_R 0x04 +#define GPIO_VALUE_G 0x08 +#define GPIO_TOGGLE_G 0x10 +#define GPIO_VALUE_B 0x20 +#define GPIO_TOGGLE_B 0x40 +#define GPIO_BLINK 0x08 +#define GPIO_COLOR 0x80 +#define GPIO_RED_COLOR 0x001F +#define GPIO_GREEN_COLOR 0x03E0 +#define GPIO_BLUE_COLOR 0x7C00 + +#endif + diff --git a/include/ram.h b/include/ram.h deleted file mode 100755 index d920367..0000000 --- a/include/ram.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef _RAM_H -#define _RAM_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" -#include "darwin_registers.h" - -#define RAM_SUCCESS 0 -#define RAM_BAD_ADDRESS -1 -#define RAM_WRITE_ERROR -2 -#define RAM_BAD_BIT -3 -#define RAM_BAD_ACCESS -4 - -#define RAM_SIZE 1024 - -extern uint8_t ram_data[RAM_SIZE]; - -void ram_init(void); -inline void ram_read_byte(uint16_t address, uint8_t *data); -inline void ram_read_word(uint16_t address, uint16_t *data); -uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data); -uint8_t ram_set_bit(uint16_t address, uint8_t bit); -uint8_t ram_clear_bit(uint16_t address, uint8_t bit); -uint8_t ram_write_byte(uint16_t address, uint8_t data); -uint8_t ram_write_word(uint16_t address, uint16_t data); -uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data); -inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length); -uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/src/cm730_fw.c b/src/cm730_fw.c index 7693bac..6514ca2 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -1,7 +1,9 @@ #include "stm32f1xx_hal.h" +#include "darwin_conf.h" +#include "darwin_registers.h" +#include "memory.h" #include "gpio.h" #include "eeprom.h" -#include "ram.h" #include "adc_dma.h" #include "imu.h" #include "darwin_time.h" @@ -12,6 +14,8 @@ #include "action.h" #include "action_id.h" +TMemory darwin_memory; + int main(void) { uint16_t eeprom_data,period; @@ -20,26 +24,30 @@ int main(void) HAL_Init(); /* initialize EEPROM */ EE_Init(); - // initialize the Dynamixel RAM memory space - ram_init(); + /* initialize memory structure */ + mem_init(&darwin_memory); + darwin_memory.eeprom_write_data=EE_WriteVariable; + darwin_memory.eeprom_read_data=EE_ReadVariable; /* initialize the GPIO module */ - gpio_init(); + gpio_init(&darwin_memory); // initialize adc - adc_init(); +// adc_init(); // initialize imu - imu_init(); +// imu_init(); // initialize time module - darwin_time_init(); +// darwin_time_init(); /* initialize the dynamixel slave interface */ - darwin_dyn_slave_init(); - darwin_dyn_slave_start(); +// darwin_dyn_slave_init(); +// darwin_dyn_slave_start(); /* initialize motion manager module */ - EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data); - period=eeprom_data&0x00FF; - EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data); - period+=((eeprom_data&0x00FF)<<8); - manager_init(period); - gpio_set_led(LED_4); +// EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data); +// period=eeprom_data&0x00FF; +// EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data); +// period+=((eeprom_data&0x00FF)<<8); +// manager_init(period); +// gpio_set_led(LED_4); + /* initialize the ram module */ + ram_init(&darwin_memory); while(1);/* main function does not return */ } diff --git a/src/gpio.c b/src/gpio.c index b86a6ee..35315b2 100755 --- a/src/gpio.c +++ b/src/gpio.c @@ -75,21 +75,24 @@ TIM_HandleTypeDef GPO_TIM1Handle; TIM_HandleTypeDef GPO_TIM2Handle; TIM_HandleTypeDef GPO_TIM3Handle; +TMemModule gpio_mem_module; + // Pushbuttons callbacks void (*start_pb_callback)(void); void (*mode_pb_callback)(void); + // LED's info structures -const led_info_t leds[GPIO_NUM_LEDS]={{LED_TX_GPIO_PORT,LED_TX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_1,DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, - {LED_RX_GPIO_PORT,LED_RX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_2,DARWIN_RX_LED_CNTRL,DARWIN_RX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, - {LED_2_GPIO_PORT,LED_2_PIN,&GPO_TIM1Handle,TIM_CHANNEL_3,DARWIN_PLAY_LED_CNTRL,DARWIN_PLAY_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, - {LED_3_GPIO_PORT,LED_3_PIN,&GPO_TIM1Handle,TIM_CHANNEL_4,DARWIN_EDIT_LED_CNTRL,DARWIN_EDIT_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, - {LED_4_GPIO_PORT,LED_4_PIN,0,0,DARWIN_MNG_LED_CNTRL,0x0000,GPIO_VALUE,GPIO_TOGGLE,0x00}, - {LED_5_R_GPIO_PORT,LED_5_R_PIN,&GPO_TIM2Handle,TIM_CHANNEL_1,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00}, - {LED_5_G_GPIO_PORT,LED_5_G_PIN,&GPO_TIM2Handle,TIM_CHANNEL_2,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00}, - {LED_5_B_GPIO_PORT,LED_5_B_PIN,&GPO_TIM2Handle,TIM_CHANNEL_3,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}, - {LED_6_R_GPIO_PORT,LED_6_R_PIN,&GPO_TIM3Handle,TIM_CHANNEL_1,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00}, - {LED_6_G_GPIO_PORT,LED_6_G_PIN,&GPO_TIM3Handle,TIM_CHANNEL_2,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00}, - {LED_6_B_GPIO_PORT,LED_6_B_PIN,&GPO_TIM3Handle,TIM_CHANNEL_3,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}}; +const led_info_t leds[GPIO_NUM_LEDS]={{LED_TX_GPIO_PORT,LED_TX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_1,TX_LED_CNTRL,TX_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, + {LED_RX_GPIO_PORT,LED_RX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_2,RX_LED_CNTRL,RX_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, + {LED_2_GPIO_PORT,LED_2_PIN,&GPO_TIM1Handle,TIM_CHANNEL_3,PLAY_LED_CNTRL,PLAY_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, + {LED_3_GPIO_PORT,LED_3_PIN,&GPO_TIM1Handle,TIM_CHANNEL_4,EDIT_LED_CNTRL,EDIT_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK}, + {LED_4_GPIO_PORT,LED_4_PIN,0,0,MNG_LED_CNTRL,0x0000,GPIO_VALUE,GPIO_TOGGLE,0x00}, + {LED_5_R_GPIO_PORT,LED_5_R_PIN,&GPO_TIM2Handle,TIM_CHANNEL_1,AUX1_LED_CNTRL,AUX1_LED_COLOR,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00}, + {LED_5_G_GPIO_PORT,LED_5_G_PIN,&GPO_TIM2Handle,TIM_CHANNEL_2,AUX1_LED_CNTRL,AUX1_LED_COLOR,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00}, + {LED_5_B_GPIO_PORT,LED_5_B_PIN,&GPO_TIM2Handle,TIM_CHANNEL_3,AUX1_LED_CNTRL,AUX1_LED_COLOR,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}, + {LED_6_R_GPIO_PORT,LED_6_R_PIN,&GPO_TIM3Handle,TIM_CHANNEL_1,AUX2_LED_CNTRL,AUX2_LED_COLOR,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00}, + {LED_6_G_GPIO_PORT,LED_6_G_PIN,&GPO_TIM3Handle,TIM_CHANNEL_2,AUX2_LED_CNTRL,AUX2_LED_COLOR,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00}, + {LED_6_B_GPIO_PORT,LED_6_B_PIN,&GPO_TIM3Handle,TIM_CHANNEL_3,AUX2_LED_CNTRL,AUX2_LED_COLOR,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}}; uint16_t leds_data[GPIO_NUM_LEDS]; // IRQ handler functions @@ -98,7 +101,7 @@ void GPI_EXTI1_IRQHandler(void) if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET) { __HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN); - if(ram_data[DARWIN_START_PB_CNTRL]&GPIO_INT_USED) + if(ram_data[START_PB_CNTRL]&GPIO_INT_USED) { if(start_pb_callback!=0) start_pb_callback(); @@ -107,7 +110,7 @@ void GPI_EXTI1_IRQHandler(void) if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET) { __HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN); - if(ram_data[DARWIN_MODE_PB_CNTRL]&GPIO_INT_USED) + if(ram_data[MODE_PB_CNTRL]&GPIO_INT_USED) { if(mode_pb_callback!=0) mode_pb_callback(); @@ -397,8 +400,44 @@ void gpio_process_led(led_t led_id,uint16_t address,uint16_t length,uint8_t *dat } } +void gpio_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + if(ram_in_range(START_PB_CNTRL,address,length)) + if(!(ram_data[START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */ + { + if(gpio_is_pushbutton_pressed(START_PB)) + ram_data[START_PB_CNTRL]|=GPIO_VALUE; + else + ram_data[START_PB_CNTRL]&=(~GPIO_VALUE); + } + if(ram_in_range(MODE_PB_CNTRL,address,length)) + if(!(ram_data[MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */ + { + if(gpio_is_pushbutton_pressed(MODE_PB)) + ram_data[MODE_PB_CNTRL]|=GPIO_VALUE; + else + ram_data[MODE_PB_CNTRL]&=(~GPIO_VALUE); + } +} + +void gpio_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + /* process standard leds */ + gpio_process_led(LED_TX,address,length,data); + gpio_process_led(LED_RX,address,length,data); + gpio_process_led(LED_2,address,length,data); + gpio_process_led(LED_3,address,length,data); + gpio_process_led(LED_4,address,length,data); + gpio_process_led(LED_5_R,address,length,data); + gpio_process_led(LED_5_G,address,length,data); + gpio_process_led(LED_5_B,address,length,data); + gpio_process_led(LED_6_R,address,length,data); + gpio_process_led(LED_6_G,address,length,data); + gpio_process_led(LED_6_B,address,length,data); +} + // public functions -void gpio_init(void) +uint8_t gpio_init(TMemory *memory) { GPIO_InitTypeDef GPIO_InitStructure; TIM_ClockConfigTypeDef sClockSourceConfig; @@ -547,6 +586,17 @@ void gpio_init(void) gpio_clear_led(LED_6_R); gpio_clear_led(LED_6_G); gpio_clear_led(LED_6_B); + + /* initialize memory module */ + mem_module_init(&gpio_mem_module); + gpio_mem_module.write_cmd=gpio_write_cmd; + gpio_mem_module.read_cmd=gpio_read_cmd; + if(!mem_module_add_ram_segment(&gpio_mem_module,RAM_GPIO_BASE_ADDRESS,RAM_GPIO_LENGTH)) + return 0x00; + if(!mem_add_module(memory,&gpio_mem_module)) + return 0x00; + + return 0x01; } void gpio_set_led(led_t led_id) @@ -658,44 +708,4 @@ void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void)) } } -uint8_t gpio_in_range(unsigned short int address,unsigned short int length) -{ - return ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length); -} - -void gpio_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - if(ram_in_range(DARWIN_START_PB_CNTRL,address,length)) - if(!(ram_data[DARWIN_START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */ - { - if(gpio_is_pushbutton_pressed(START_PB)) - ram_data[DARWIN_START_PB_CNTRL]|=GPIO_VALUE; - else - ram_data[DARWIN_START_PB_CNTRL]&=(~GPIO_VALUE); - } - if(ram_in_range(DARWIN_MODE_PB_CNTRL,address,length)) - if(!(ram_data[DARWIN_MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */ - { - if(gpio_is_pushbutton_pressed(MODE_PB)) - ram_data[DARWIN_MODE_PB_CNTRL]|=GPIO_VALUE; - else - ram_data[DARWIN_MODE_PB_CNTRL]&=(~GPIO_VALUE); - } -} - -void gpio_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - /* process standard leds */ - gpio_process_led(LED_TX,address,length,data); - gpio_process_led(LED_RX,address,length,data); - gpio_process_led(LED_2,address,length,data); - gpio_process_led(LED_3,address,length,data); - gpio_process_led(LED_4,address,length,data); - gpio_process_led(LED_5_R,address,length,data); - gpio_process_led(LED_5_G,address,length,data); - gpio_process_led(LED_5_B,address,length,data); - gpio_process_led(LED_6_R,address,length,data); - gpio_process_led(LED_6_G,address,length,data); - gpio_process_led(LED_6_B,address,length,data); -} diff --git a/src/ram.c b/src/ram.c deleted file mode 100755 index be8aa9d..0000000 --- a/src/ram.c +++ /dev/null @@ -1,119 +0,0 @@ -#include "ram.h" -#include "eeprom.h" - -uint8_t ram_data[RAM_SIZE]; - -void ram_init(void) -{ - uint16_t eeprom_data,i; - - for(i=0;i=0 && bit<8) - { - ram_data[address]|=(0x01<=0 && bit<8) - { - ram_data[address]&=(~(0x01<=address && reg<(address+length)) - return 0x01; - else - return 0x00; -} - -uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length) -{ - uint16_t end_reg=start_reg+reg_length; - uint16_t end_address=start_address+address_length; - - if((start_reg>=start_address && start_reg=start_address && end_reg=start_reg && start_address=start_reg && end_address Date: Thu, 15 Feb 2018 17:27:12 +0100 Subject: [PATCH 23/30] Modified the ADC module to use the TMemory data structures. --- Makefile | 2 +- include/adc_dma.h | 7 ++- include/adc_dma_registers.h | 30 +++++++++++ include/darwin_conf.h | 3 ++ include/darwin_registers.h | 33 ------------ src/adc_dma.c | 103 ++++++++++++++++++++---------------- src/cm730_fw.c | 3 +- 7 files changed, 96 insertions(+), 85 deletions(-) create mode 100644 include/adc_dma_registers.h diff --git a/Makefile b/Makefile index c603cfd..cfd15ca 100755 --- a/Makefile +++ b/Makefile @@ -12,7 +12,7 @@ TARGET_FILES+=src/system_stm32f1xx.c TARGET_FILES+=src/gpio.c #TARGET_FILES+=src/darwin_time.c TARGET_FILES+=src/eeprom.c -#TARGET_FILES+=src/adc_dma.c +TARGET_FILES+=src/adc_dma.c #TARGET_FILES+=src/imu.c #TARGET_FILES+=src/darwin_dyn_slave.c #TARGET_FILES+=src/darwin_dyn_master.c diff --git a/include/adc_dma.h b/include/adc_dma.h index db29c8d..674171f 100755 --- a/include/adc_dma.h +++ b/include/adc_dma.h @@ -6,13 +6,15 @@ extern "C" { #endif #include "stm32f1xx.h" +#include "adc_dma_registers.h" +#include "memory.h" #define ADC_NUM_CHANNELS 12 typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7, ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_ch_t; -void adc_init(void); +uint8_t adc_init(TMemory *memory); void adc_start(void); void adc_set_period(uint8_t period_ms); inline uint8_t adc_get_period(void); @@ -20,9 +22,6 @@ unsigned short adc_get_channel(adc_ch_t channel); unsigned short adc_get_channel_raw(adc_ch_t channel); unsigned short adc_get_temperature(void); void adc_stop(void); -// operation functions -uint8_t adc_in_range(unsigned short int address,unsigned short int length); -void adc_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); #ifdef __cplusplus } diff --git a/include/adc_dma_registers.h b/include/adc_dma_registers.h new file mode 100644 index 0000000..a0a6366 --- /dev/null +++ b/include/adc_dma_registers.h @@ -0,0 +1,30 @@ +#ifndef _ADC_DMA_REGISTERS_H +#define _ADC_DMA_REGISTERS_H + +#ifndef RAM_ADC_DMA_BASE_ADDRESS + #define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0000) +#endif + +#define RAM_ADC_DMA_LENGTH 26 + +#define ADC_CNTRL RAM_ADC_DMA_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | | | Enable +#define ADC_PERIOD (RAM_ADC_DMA_BASE_ADDRESS+1) +#define ADC_CH1_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+2) +#define ADC_CH2_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+4) +#define ADC_CH3_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+6) +#define ADC_CH4_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+8) +#define ADC_CH5_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+10) +#define ADC_CH6_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+12) +#define ADC_CH7_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+14) +#define ADC_CH8_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+16) +#define ADC_CH9_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+18) +#define ADC_CH10_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+20) +#define ADC_TEMP (RAM_ADC_DMA_BASE_ADDRESS+22) +#define ADC_CH12_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+24) + +#define ADC_START 0x01 +#define ADC_STOP 0x02 +#define ADC_RUNNING 0x10 + +#endif diff --git a/include/darwin_conf.h b/include/darwin_conf.h index f88b914..e0a776b 100644 --- a/include/darwin_conf.h +++ b/include/darwin_conf.h @@ -7,4 +7,7 @@ /* GPIO configuration */ #define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100) +/* ADC configuration */ +#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115) + #endif diff --git a/include/darwin_registers.h b/include/darwin_registers.h index bd2cc32..7ae3534 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -254,33 +254,6 @@ typedef enum { DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY, //RAM - DARWIN_ADC_CNTRL = 0x0115, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | | | running | | | start | stop - DARWIN_ADC_PERIOD = 0x0116, - DARWIN_ADC_CH1_L = 0x0117, - DARWIN_ADC_CH1_H = 0x0118, - DARWIN_ADC_CH2_L = 0x0119, - DARWIN_ADC_CH2_H = 0x011A, - DARWIN_ADC_CH3_L = 0x011B, - DARWIN_ADC_CH3_H = 0x011C, - DARWIN_ADC_CH4_L = 0x011D, - DARWIN_ADC_CH4_H = 0x011E, - DARWIN_ADC_CH5_L = 0x011F, - DARWIN_ADC_CH5_H = 0x0120, - DARWIN_ADC_CH6_L = 0x0121, - DARWIN_ADC_CH6_H = 0x0122, - DARWIN_ADC_CH7_L = 0x0123, - DARWIN_ADC_CH7_H = 0x0124, - DARWIN_ADC_CH8_L = 0x0125, - DARWIN_ADC_CH8_H = 0x0126, - DARWIN_ADC_CH9_L = 0x0127, - DARWIN_ADC_CH9_H = 0x0128, - DARWIN_ADC_CH10_L = 0x0129, - DARWIN_ADC_CH10_H = 0x012A, - DARWIN_ADC_TEMP_L = 0x012B, - DARWIN_ADC_TEMP_H = 0x012C, - DARWIN_ADC_CH12_L = 0x012D, - DARWIN_ADC_CH12_H = 0x012E, DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // accel_det | gyro_det | calibrating | running | | start_cal | stop | start DARWIN_IMU_CAL_SAMPLES_L = 0x013C, @@ -613,12 +586,6 @@ typedef enum { // current phase | climbing | stop stairs | start stairs down | start stairs up }darwin_registers; -#define ADC_BASE_ADDRESS 0x0115 -#define ADC_MEM_LENGTH 30 -#define ADC_START 0x01 -#define ADC_STOP 0x02 -#define ADC_RUNNING 0x10 - #define IMU_BASE_ADDRESS 0x013B #define IMU_MEM_LENGTH 15 #define IMU_START 0x01 diff --git a/src/adc_dma.c b/src/adc_dma.c index eb0f821..cef91c3 100755 --- a/src/adc_dma.c +++ b/src/adc_dma.c @@ -77,6 +77,9 @@ ADC_HandleTypeDef hadc2; DMA_HandleTypeDef hdma_adc1; TIM_HandleTypeDef ADC_TIM_Handle; +/* memory module */ +TMemModule adc_mem_module; + uint32_t adc_data[6];// temporal buffer to store ADC data before conversion uint16_t adc_period_ms; @@ -99,6 +102,24 @@ uint16_t adc_convert_voltage(uint16_t value) return conv_value*(1<<12); } +void adc_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + if(ram_in_range(ADC_CNTRL,address,length)) + { + if(data[ADC_CNTRL-address]&ADC_START) + adc_start(); + else if(data[ADC_CNTRL-address]&ADC_STOP) + adc_stop(); + } + if(ram_in_range(ADC_PERIOD,address,length)) + adc_set_period(data[ADC_PERIOD-address]); +} + +void adc_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + ram_read_table(address,length,data); +} + // interrupt handlers void ADC_TIMER_IRQHandler(void) { @@ -153,24 +174,24 @@ void ADC_DMA_IRQHandler(void) if(i==5) { value=adc_convert_temperature(adc_data[i]&0x0000FFFF); - ram_data[DARWIN_ADC_TEMP_L]=value%256; - ram_data[DARWIN_ADC_TEMP_H]=value/256; + ram_data[ADC_TEMP]=value%256; + ram_data[ADC_TEMP+1]=value/256; } else { value=adc_convert_voltage(adc_data[i]&0x0000FFFF); - ram_data[DARWIN_ADC_CH1_L+i*4]=value%256; - ram_data[DARWIN_ADC_CH1_H+i*4]=value/256; + ram_data[ADC_CH1_VOLTAGE+i*4]=value%256; + ram_data[ADC_CH1_VOLTAGE+1+i*4]=value/256; } value=adc_convert_voltage((adc_data[i]&0xFFFF0000)>>16); - ram_data[DARWIN_ADC_CH2_L+i*4]=value%256; - ram_data[DARWIN_ADC_CH2_H+i*4]=value/256; + ram_data[ADC_CH2_VOLTAGE+i*4]=value%256; + ram_data[ADC_CH2_VOLTAGE+1+i*4]=value/256; } } } // public functions -void adc_init(void) +uint8_t adc_init(TMemory *memory) { GPIO_InitTypeDef GPIO_InitStruct; ADC_MultiModeTypeDef multimode; @@ -183,7 +204,7 @@ void adc_init(void) /* initialize the internal variables */ adc_period_ms=360;// equivalent to 10 ms - ram_data[DARWIN_ADC_PERIOD]=10; + ram_data[ADC_PERIOD]=10; /* enable clocks */ ADC1_CH1_ENABLE_PORT_CLK; @@ -353,22 +374,33 @@ void adc_init(void) HAL_ADC_Start(&hadc2); HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,6); + + /* initialize memory module */ + mem_module_init(&adc_mem_module); + adc_mem_module.write_cmd=adc_write_cmd; + adc_mem_module.read_cmd=adc_read_cmd; + if(!mem_module_add_ram_segment(&adc_mem_module,RAM_ADC_DMA_BASE_ADDRESS,RAM_ADC_DMA_LENGTH)) + return 0x00; + if(!mem_add_module(memory,&adc_mem_module)) + return 0x00; + + return 0x01; } void adc_start(void) { - if((ram_data[DARWIN_ADC_CNTRL]&ADC_RUNNING)==0x00) + if((ram_data[ADC_CNTRL]&ADC_RUNNING)==0x00) { - ram_data[DARWIN_ADC_CNTRL]|=ADC_RUNNING; + ram_data[ADC_CNTRL]|=ADC_RUNNING; HAL_TIM_OC_Start_IT(&ADC_TIM_Handle, TIM_CHANNEL_1); } } void adc_stop(void) { - if(ram_data[DARWIN_ADC_CNTRL]&ADC_RUNNING) + if(ram_data[ADC_CNTRL]&ADC_RUNNING) { - ram_data[DARWIN_ADC_CNTRL]&=(~ADC_RUNNING); + ram_data[ADC_CNTRL]&=(~ADC_RUNNING); HAL_TIM_OC_Stop_IT(&ADC_TIM_Handle, TIM_CHANNEL_1); } } @@ -376,12 +408,12 @@ void adc_stop(void) void adc_set_period(uint8_t period_ms) { adc_period_ms=period_ms*36; - ram_data[DARWIN_ADC_PERIOD]=period_ms; + ram_data[ADC_PERIOD]=period_ms; } inline uint8_t adc_get_period(void) { - return ram_data[DARWIN_ADC_PERIOD]; + return ram_data[ADC_PERIOD]; } uint16_t adc_get_channel(adc_ch_t channel) @@ -390,27 +422,27 @@ uint16_t adc_get_channel(adc_ch_t channel) switch(channel) { - case ADC_CH1: value=ram_data[DARWIN_ADC_CH1_L]+ram_data[DARWIN_ADC_CH1_H]*256; + case ADC_CH1: value=ram_data[ADC_CH1_VOLTAGE]+ram_data[ADC_CH1_VOLTAGE+1]*256; break; - case ADC_CH2: value=ram_data[DARWIN_ADC_CH2_L]+ram_data[DARWIN_ADC_CH2_H]*256; + case ADC_CH2: value=ram_data[ADC_CH2_VOLTAGE]+ram_data[ADC_CH2_VOLTAGE+1]*256; break; - case ADC_CH3: value=ram_data[DARWIN_ADC_CH3_L]+ram_data[DARWIN_ADC_CH3_H]*256; + case ADC_CH3: value=ram_data[ADC_CH3_VOLTAGE]+ram_data[ADC_CH3_VOLTAGE+1]*256; break; - case ADC_CH4: value=ram_data[DARWIN_ADC_CH4_L]+ram_data[DARWIN_ADC_CH4_H]*256; + case ADC_CH4: value=ram_data[ADC_CH4_VOLTAGE]+ram_data[ADC_CH4_VOLTAGE+1]*256; break; - case ADC_CH5: value=ram_data[DARWIN_ADC_CH5_L]+ram_data[DARWIN_ADC_CH5_H]*256; + case ADC_CH5: value=ram_data[ADC_CH5_VOLTAGE]+ram_data[ADC_CH5_VOLTAGE+1]*256; break; - case ADC_CH6: value=ram_data[DARWIN_ADC_CH6_L]+ram_data[DARWIN_ADC_CH6_H]*256; + case ADC_CH6: value=ram_data[ADC_CH6_VOLTAGE]+ram_data[ADC_CH6_VOLTAGE+1]*256; break; - case ADC_CH7: value=ram_data[DARWIN_ADC_CH7_L]+ram_data[DARWIN_ADC_CH7_H]*256; + case ADC_CH7: value=ram_data[ADC_CH7_VOLTAGE]+ram_data[ADC_CH7_VOLTAGE+1]*256; break; - case ADC_CH8: value=ram_data[DARWIN_ADC_CH8_L]+ram_data[DARWIN_ADC_CH8_H]*256; + case ADC_CH8: value=ram_data[ADC_CH8_VOLTAGE]+ram_data[ADC_CH8_VOLTAGE+1]*256; break; - case ADC_CH9: value=ram_data[DARWIN_ADC_CH9_L]+ram_data[DARWIN_ADC_CH9_H]*256; + case ADC_CH9: value=ram_data[ADC_CH9_VOLTAGE]+ram_data[ADC_CH9_VOLTAGE+1]*256; break; - case ADC_CH10: value=ram_data[DARWIN_ADC_CH10_L]+ram_data[DARWIN_ADC_CH10_H]*256; + case ADC_CH10: value=ram_data[ADC_CH10_VOLTAGE]+ram_data[ADC_CH10_VOLTAGE+1]*256; break; - case ADC_CH12: value=ram_data[DARWIN_ADC_CH12_L]+ram_data[DARWIN_ADC_CH12_H]*256; + case ADC_CH12: value=ram_data[ADC_CH12_VOLTAGE]+ram_data[ADC_CH12_VOLTAGE+1]*256; break; default: value=0x0000; } @@ -454,25 +486,6 @@ uint16_t adc_get_channel_raw(adc_ch_t channel) uint16_t adc_get_temperature(void) { - return ram_data[DARWIN_ADC_TEMP_L]+ram_data[DARWIN_ADC_TEMP_H]*256; -} - -// operation functions -uint8_t adc_in_range(unsigned short int address,unsigned short int length) -{ - return ram_in_window(ADC_BASE_ADDRESS,ADC_MEM_LENGTH,address,length); -} - -void adc_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - if(ram_in_range(DARWIN_ADC_CNTRL,address,length)) - { - if(data[DARWIN_ADC_CNTRL-address]&ADC_START) - adc_start(); - else if(data[DARWIN_ADC_CNTRL-address]&ADC_STOP) - adc_stop(); - } - if(ram_in_range(DARWIN_ADC_PERIOD,address,length)) - adc_set_period(data[DARWIN_ADC_PERIOD-address]); + return ram_data[ADC_TEMP]+ram_data[ADC_TEMP+1]*256; } diff --git a/src/cm730_fw.c b/src/cm730_fw.c index 6514ca2..f8ba5f2 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -1,6 +1,5 @@ #include "stm32f1xx_hal.h" #include "darwin_conf.h" -#include "darwin_registers.h" #include "memory.h" #include "gpio.h" #include "eeprom.h" @@ -31,7 +30,7 @@ int main(void) /* initialize the GPIO module */ gpio_init(&darwin_memory); // initialize adc -// adc_init(); + adc_init(&darwin_memory); // initialize imu // imu_init(); // initialize time module -- GitLab From 896a83c8b026a3bd8b34ef720d2a691cc2a93810 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Thu, 15 Feb 2018 17:42:21 +0100 Subject: [PATCH 24/30] Updated the IMU module to use the TMemory data structures. --- Makefile | 2 +- include/darwin_conf.h | 3 + include/darwin_registers.h | 24 -------- include/imu.h | 7 +-- include/imu_registers.h | 28 ++++++++++ src/cm730_fw.c | 2 +- src/imu.c | 110 ++++++++++++++++++++----------------- 7 files changed, 97 insertions(+), 79 deletions(-) create mode 100644 include/imu_registers.h diff --git a/Makefile b/Makefile index cfd15ca..a114da2 100755 --- a/Makefile +++ b/Makefile @@ -13,7 +13,7 @@ TARGET_FILES+=src/gpio.c #TARGET_FILES+=src/darwin_time.c TARGET_FILES+=src/eeprom.c TARGET_FILES+=src/adc_dma.c -#TARGET_FILES+=src/imu.c +TARGET_FILES+=src/imu.c #TARGET_FILES+=src/darwin_dyn_slave.c #TARGET_FILES+=src/darwin_dyn_master.c #TARGET_FILES+=src/darwin_dyn_master_v2.c diff --git a/include/darwin_conf.h b/include/darwin_conf.h index e0a776b..97ee25a 100644 --- a/include/darwin_conf.h +++ b/include/darwin_conf.h @@ -10,4 +10,7 @@ /* ADC configuration */ #define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115) +/* ADC configuration */ +#define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x012F) + #endif diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 7ae3534..59ae35e 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -254,23 +254,6 @@ typedef enum { DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY, //RAM - DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // accel_det | gyro_det | calibrating | running | | start_cal | stop | start - DARWIN_IMU_CAL_SAMPLES_L = 0x013C, - DARWIN_IMU_CAL_SAMPLES_H = 0x013D, - DARWIN_IMU_GYRO_X_L = 0x013E, - DARWIN_IMU_GYRO_X_H = 0x013F, - DARWIN_IMU_GYRO_Y_L = 0x0140, - DARWIN_IMU_GYRO_Y_H = 0x0141, - DARWIN_IMU_GYRO_Z_L = 0x0142, - DARWIN_IMU_GYRO_Z_H = 0x0143, - DARWIN_IMU_ACCEL_X_L = 0x0144, - DARWIN_IMU_ACCEL_X_H = 0x0145, - DARWIN_IMU_ACCEL_Y_L = 0x0146, - DARWIN_IMU_ACCEL_Y_H = 0x0147, - DARWIN_IMU_ACCEL_Z_L = 0x0148, - DARWIN_IMU_ACCEL_Z_H = 0x0149, - DARWIN_MM_NUM_SERVOS = 0x014A, DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // scanning | fwd fall | bwd fall | | Enable power v2 | Enable power | Enable balance | Enable manager @@ -588,13 +571,6 @@ typedef enum { #define IMU_BASE_ADDRESS 0x013B #define IMU_MEM_LENGTH 15 -#define IMU_START 0x01 -#define IMU_STOP 0x02 -#define IMU_START_CAL 0x04 -#define IMU_RUNNING 0x10 -#define IMU_CALIBRATING 0x20 -#define IMU_ACCEL_DET 0x40 -#define IMU_GYRO_DET 0x80 #define MANAGER_BASE_ADDRESS 0x014A #define MANAGER_MEM_LENGTH 86 diff --git a/include/imu.h b/include/imu.h index f3e1c3d..b74bf3e 100755 --- a/include/imu.h +++ b/include/imu.h @@ -6,9 +6,11 @@ extern "C" { #endif #include "stm32f1xx.h" +#include "imu_registers.h" +#include "memory.h" //public functions -void imu_init(void); +uint8_t imu_init(TMemory *memory); void imu_start(void); void imu_stop(void); void imu_set_calibration_samples(uint16_t num_samples); @@ -16,9 +18,6 @@ void imu_start_gyro_cal(void); void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z); void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z); uint8_t imu_is_gyro_calibrated(void); -// operation functions -uint8_t imu_in_range(unsigned short int address,unsigned short int length); -void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); #ifdef __cplusplus } diff --git a/include/imu_registers.h b/include/imu_registers.h new file mode 100644 index 0000000..92c38a8 --- /dev/null +++ b/include/imu_registers.h @@ -0,0 +1,28 @@ +#ifndef _IMU_REGISTERS_H +#define _IMU_REGISTERS_H + +#ifndef RAM_IMU_BASE_ADDRESS + #define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x0000) +#endif + +#define RAM_IMU_LENGTH 15 + +#define IMU_CNTRL RAM_IMU_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // accel_det | gyro_det | calibrating | running | | start_cal | stop | start +#define IMU_CAL_SAMPLES (RAM_IMU_BASE_ADDRESS+1) +#define IMU_GYRO_X (RAM_IMU_BASE_ADDRESS+3) +#define IMU_GYRO_Y (RAM_IMU_BASE_ADDRESS+5) +#define IMU_GYRO_Z (RAM_IMU_BASE_ADDRESS+7) +#define IMU_ACCEL_X (RAM_IMU_BASE_ADDRESS+9) +#define IMU_ACCEL_Y (RAM_IMU_BASE_ADDRESS+11) +#define IMU_ACCEL_Z (RAM_IMU_BASE_ADDRESS+13) + +#define IMU_START 0x01 +#define IMU_STOP 0x02 +#define IMU_START_CAL 0x04 +#define IMU_RUNNING 0x10 +#define IMU_CALIBRATING 0x20 +#define IMU_ACCEL_DET 0x40 +#define IMU_GYRO_DET 0x80 + +#endif diff --git a/src/cm730_fw.c b/src/cm730_fw.c index f8ba5f2..859d26a 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -32,7 +32,7 @@ int main(void) // initialize adc adc_init(&darwin_memory); // initialize imu -// imu_init(); + imu_init(&darwin_memory); // initialize time module // darwin_time_init(); /* initialize the dynamixel slave interface */ diff --git a/src/imu.c b/src/imu.c index bbaed72..95c834c 100755 --- a/src/imu.c +++ b/src/imu.c @@ -117,6 +117,8 @@ const uint8_t accel_data_len=6; SPI_HandleTypeDef hspi2; TIM_HandleTypeDef htim; +TMemModule imu_mem_module; + // private functions inline void imu_wait_op_done(void) { @@ -208,12 +210,12 @@ uint8_t imu_accel_detect(void) imu_spi_get_data(&data); if(data==ACCEL_ID) { - ram_data[DARWIN_IMU_CNTRL]|=IMU_ACCEL_DET; + ram_data[IMU_CNTRL]|=IMU_ACCEL_DET; return 0x01; } else { - ram_data[DARWIN_IMU_CNTRL]&=(~IMU_ACCEL_DET); + ram_data[IMU_CNTRL]&=(~IMU_ACCEL_DET); return 0x00; } } @@ -275,12 +277,12 @@ uint8_t imu_gyro_detect(void) imu_spi_get_data(&data); if(data==GYRO_ID) { - ram_data[DARWIN_IMU_CNTRL]|=IMU_GYRO_DET; + ram_data[IMU_CNTRL]|=IMU_GYRO_DET; return 0x01; } else { - ram_data[DARWIN_IMU_CNTRL]&=(~IMU_GYRO_DET); + ram_data[IMU_CNTRL]&=(~IMU_GYRO_DET); return 0x00; } } @@ -331,6 +333,32 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out) } } +void imu_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + uint16_t num_samples; + + if(ram_in_range(IMU_CNTRL,address,length)) + { + if(data[IMU_CNTRL-address]&IMU_START) + imu_start(); + else if(data[IMU_CNTRL-address]&IMU_STOP) + imu_stop(); + else if(data[IMU_CNTRL-address]&IMU_START_CAL) + imu_start_gyro_cal(); + } + ram_read_word(IMU_CAL_SAMPLES,&num_samples); + if(ram_in_range(IMU_CAL_SAMPLES,address,length)) + num_samples=(num_samples&0xFF00)+data[IMU_CAL_SAMPLES-address]; + if(ram_in_range(IMU_CAL_SAMPLES+1,address,length)) + num_samples=(num_samples&0x00FF)+(data[IMU_CAL_SAMPLES+1-address]<<8); + imu_set_calibration_samples(num_samples); +} + +void imu_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data) +{ + ram_read_table(address,length,data); +} + // interrupt handlers void IMU_SPI_IRQHANDLER(void) { @@ -390,10 +418,10 @@ void IMU_TIMER_IRQHandler(void) { switch(state) { - case IMU_GET_ACCEL: if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET) + case IMU_GET_ACCEL: if(ram_data[IMU_CNTRL]&IMU_GYRO_DET) { imu_spi_get_data(data); - imu_gyro_convert_data(data,&ram_data[DARWIN_IMU_GYRO_X_L]); + imu_gyro_convert_data(data,&ram_data[IMU_GYRO_X]); if(gyro_calibration) { gyro_accum[0]+=gyro_data[0]; @@ -409,23 +437,23 @@ void IMU_TIMER_IRQHandler(void) gyro_accum[0]=0; gyro_accum[1]=0; gyro_accum[2]=0; - ram_data[DARWIN_IMU_CNTRL]&=(~IMU_CALIBRATING); - if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00) + ram_data[IMU_CNTRL]&=(~IMU_CALIBRATING); + if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00) imu_stop_flag=0x01; gyro_calibration=0x00; } } } - if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET) + if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET) imu_accel_get_data(); state=IMU_GET_GYRO; break; - case IMU_GET_GYRO: if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET) + case IMU_GET_GYRO: if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET) { imu_spi_get_data(data); - imu_accel_convert_data(data,&ram_data[DARWIN_IMU_ACCEL_X_L]); + imu_accel_convert_data(data,&ram_data[IMU_ACCEL_X]); } - if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET) + if(ram_data[IMU_CNTRL]&IMU_GYRO_DET) imu_gyro_get_data(); state=IMU_GET_ACCEL; break; @@ -438,7 +466,7 @@ void IMU_TIMER_IRQHandler(void) // public functions -void imu_init(void) +uint8_t imu_init(TMemory *memory) { GPIO_InitTypeDef GPIO_InitStructure; uint8_t i; @@ -534,11 +562,22 @@ void imu_init(void) imu_accel_get_config(); imu_accel_set_config(); } + + /* initialize memory module */ + mem_module_init(&imu_mem_module); + imu_mem_module.write_cmd=imu_write_cmd; + imu_mem_module.read_cmd=imu_read_cmd; + if(!mem_module_add_ram_segment(&imu_mem_module,RAM_IMU_BASE_ADDRESS,RAM_IMU_LENGTH)) + return 0x00; + if(!mem_add_module(memory,&imu_mem_module)) + return 0x00; + + return 0x01; } void imu_start(void) { - if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running + if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running { // start the accelerometer imu_accel_start(); @@ -546,33 +585,33 @@ void imu_start(void) // start the timer to get the imu data HAL_TIM_Base_Start_IT(&htim); - ram_data[DARWIN_IMU_CNTRL]|=IMU_RUNNING; + ram_data[IMU_CNTRL]|=IMU_RUNNING; } } void imu_stop(void) { - if(ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING) + if(ram_data[IMU_CNTRL]&IMU_RUNNING) { imu_stop_flag=0x01; while(!imu_stopped); imu_stopped=0x00; imu_accel_stop(); imu_gyro_stop(); - ram_data[DARWIN_IMU_CNTRL]&=(~IMU_RUNNING); + ram_data[IMU_CNTRL]&=(~IMU_RUNNING); } } void imu_set_calibration_samples(uint16_t num_samples) { gyro_calibration_num_samples=num_samples; - ram_data[DARWIN_IMU_CAL_SAMPLES_L]=gyro_calibration_num_samples%256; - ram_data[DARWIN_IMU_CAL_SAMPLES_H]=gyro_calibration_num_samples/256; + ram_data[IMU_CAL_SAMPLES]=gyro_calibration_num_samples%256; + ram_data[IMU_CAL_SAMPLES+1]=gyro_calibration_num_samples/256; } void imu_start_gyro_cal(void) { - if((ram_data[DARWIN_IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running + if((ram_data[IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running { // start the accelerometer imu_accel_start(); @@ -581,7 +620,7 @@ void imu_start_gyro_cal(void) // start the timer to get the imu data HAL_TIM_Base_Start_IT(&htim); } - ram_data[DARWIN_IMU_CNTRL]|=IMU_CALIBRATING; + ram_data[IMU_CNTRL]|=IMU_CALIBRATING; gyro_center[0]=0; gyro_center[1]=0; gyro_center[2]=0; @@ -590,7 +629,7 @@ void imu_start_gyro_cal(void) uint8_t imu_is_gyro_calibrated(void) { - if(ram_data[DARWIN_IMU_CNTRL]) + if(ram_data[IMU_CNTRL]) return 0x00; else return 0x01; @@ -610,30 +649,3 @@ void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z) *z=accel_data[2]; } -// operation functions -uint8_t imu_in_range(unsigned short int address,unsigned short int length) -{ - return ram_in_window(IMU_BASE_ADDRESS,IMU_MEM_LENGTH,address,length); -} - -void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - uint16_t num_samples; - - if(ram_in_range(DARWIN_IMU_CNTRL,address,length)) - { - if(data[DARWIN_IMU_CNTRL-address]&IMU_START) - imu_start(); - else if(data[DARWIN_IMU_CNTRL-address]&IMU_STOP) - imu_stop(); - else if(data[DARWIN_IMU_CNTRL-address]&IMU_START_CAL) - imu_start_gyro_cal(); - } - ram_read_word(DARWIN_IMU_CAL_SAMPLES_L,&num_samples); - if(ram_in_range(DARWIN_IMU_CAL_SAMPLES_L,address,length)) - num_samples=(num_samples&0xFF00)+data[DARWIN_IMU_CAL_SAMPLES_L-address]; - if(ram_in_range(DARWIN_IMU_CAL_SAMPLES_H,address,length)) - num_samples=(num_samples&0x00FF)+(data[DARWIN_IMU_CAL_SAMPLES_H-address]<<8); - imu_set_calibration_samples(num_samples); -} - -- GitLab From cfee3b230e1c026791714daf7264ab21412a3cc9 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Thu, 15 Feb 2018 18:06:01 +0100 Subject: [PATCH 25/30] Updated the dynamixel slave module to use the TMemory data structures. Removed all the items in the eeprom_data variable except for the VALID_PAGE. Now, each module declares their own EEPROM variables. --- Makefile | 7 +- include/darwin_conf.h | 15 ++++ include/darwin_dyn_slave.h | 4 +- include/darwin_registers.h | 13 ---- src/darwin_dyn_slave.c | 99 ++++---------------------- src/eeprom.c | 140 +------------------------------------ 6 files changed, 37 insertions(+), 241 deletions(-) diff --git a/Makefile b/Makefile index a114da2..eeb471a 100755 --- a/Makefile +++ b/Makefile @@ -7,14 +7,13 @@ STM32_LIBRARIES_PATH=$(HOME)/humanoids/stm32_libraries PROJECT_NAME=darwin_firmware #TARGET_FILES=$(wildcard src/*.c) TARGET_FILES=src/cm730_fw.c -#TARGET_FILES+=src/test_charger.c +TARGET_FILES+=src/eeprom.c TARGET_FILES+=src/system_stm32f1xx.c TARGET_FILES+=src/gpio.c -#TARGET_FILES+=src/darwin_time.c -TARGET_FILES+=src/eeprom.c +TARGET_FILES+=src/darwin_time.c TARGET_FILES+=src/adc_dma.c TARGET_FILES+=src/imu.c -#TARGET_FILES+=src/darwin_dyn_slave.c +TARGET_FILES+=src/darwin_dyn_slave.c #TARGET_FILES+=src/darwin_dyn_master.c #TARGET_FILES+=src/darwin_dyn_master_v2.c TARGET_FILES+=src/stm32f1xx_hal_msp.c diff --git a/include/darwin_conf.h b/include/darwin_conf.h index 97ee25a..577308c 100644 --- a/include/darwin_conf.h +++ b/include/darwin_conf.h @@ -4,6 +4,21 @@ #define RAM_SIZE 1024 #define EEPROM_SIZE 256 +/* Dynamixel slave configuration */ +#define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000) +#define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010) + +#define DEFAULT_DEVICE_MODEL 0x7300 +#define DEFAULT_FIRMWARE_VERSION 0x0001 +#define DEFAULT_DEVICE_ID 0x0001 +#define DEFAULT_BAUDRATE 0x0010 +#define DEFAULT_RETURN_DELAY 0x0000 +#define DEFAULT_RETURN_LEVEL 0x0002 + +#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024 +#define MAX_DYN_SLAVE_RX_BUFFER_LEN 1024 +#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024 + /* GPIO configuration */ #define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100) diff --git a/include/darwin_dyn_slave.h b/include/darwin_dyn_slave.h index b3a38c9..49437d6 100644 --- a/include/darwin_dyn_slave.h +++ b/include/darwin_dyn_slave.h @@ -7,8 +7,10 @@ extern "C" { #include "stm32f1xx.h" #include "dynamixel_slave.h" +#include "dynamixel_slave_registers.h" +#include "memory.h" -void darwin_dyn_slave_init(void); +uint8_t darwin_dyn_slave_init(TMemory *memory); void darwin_dyn_slave_start(void); void darwin_dyn_slave_stop(void); diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 59ae35e..c431cb9 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -7,17 +7,11 @@ extern "C" { #endif -#define DEVICE_MODEL_OFFSET ((unsigned short int)0x0000) -#define FIRMWARE_VERSION_OFFSET ((unsigned short int)0x0002) -#define DEVICE_ID_OFFSET ((unsigned short int)0x0003) -#define BAUDRATE_OFFSET ((unsigned short int)0x0004) -#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005) #define MM_PERIOD_OFFSET ((unsigned short int)0x0006) #define MM_BAL_KNEE_GAIN_OFFSET ((unsigned short int)0x0008) #define MM_BAL_ANKLE_ROLL_GAIN_OFFSET ((unsigned short int)0x000A) #define MM_BAL_ANKLE_PITCH_GAIN_OFFSET ((unsigned short int)0x000C) #define MM_BAL_HIP_ROLL_GAIN_OFFSET ((unsigned short int)0x000E) -#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010) #define MM_SERVO0_OFFSET ((unsigned short int)0x0011) #define MM_SERVO1_OFFSET ((unsigned short int)0x0012) #define MM_SERVO2_OFFSET ((unsigned short int)0x0013) @@ -115,12 +109,6 @@ extern "C" { #define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) typedef enum { - DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET, - DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1, - DARWIN_VERSION = FIRMWARE_VERSION_OFFSET, - DARWIN_ID = DEVICE_ID_OFFSET, - DARWIN_BAUD_RATE = BAUDRATE_OFFSET, - DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET, DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET, DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1, DARWIN_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16 @@ -131,7 +119,6 @@ typedef enum { DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1, DARWIN_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16 DARWIN_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1, - DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET, DARWIN_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4 DARWIN_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4 DARWIN_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4 diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c index b287d06..eb680db 100755 --- a/src/darwin_dyn_slave.c +++ b/src/darwin_dyn_slave.c @@ -2,18 +2,6 @@ #include "darwin_time.h" #include "usart3.h" #include "ram.h" -#include "eeprom.h" -#include "gpio.h" -#include "adc_dma.h" -#include "imu.h" -#include "motion_manager.h" -#include "action.h" -#include "walking.h" -#include "joint_motion.h" -#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 @@ -28,80 +16,20 @@ TComm darwin_dyn_slave_comm; UART_InitTypeDef darwin_comm_init; TIM_HandleTypeDef darwin_dyn_slave_tim_handle; +/* memory module */ +TMemory *darwin_dyn_slave_memory; + // private functions unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned char error; - - if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length)) - gpio_process_read_cmd(address,length,data); - /* dynamixel slave internal operation registers */ - error=ram_read_table(address,length,data); - - return error; + mem_do_read(darwin_dyn_slave_memory,address,length,data); + + return 0x00; } unsigned char darwin_on_write(unsigned short int address,unsigned short int length,unsigned char *data) { - unsigned short int i,j; - - /* dynamixel slave internal operation registers */ - if(ram_in_range(DARWIN_ID,address,length)) - { - dyn_slave_set_address(&darwin_dyn_slave,data[DARWIN_ID-address]); - ram_data[DARWIN_ID]=data[DARWIN_ID-address]; - } - if(ram_in_range(DARWIN_BAUD_RATE,address,length)) - { - darwin_comm_init.BaudRate=2000000/(data[DARWIN_BAUD_RATE-address]+1); - usart3_config(&darwin_dyn_slave_comm,&darwin_comm_init); - ram_data[DARWIN_BAUD_RATE]=data[DARWIN_BAUD_RATE-address]; - } - if(ram_in_range(DARWIN_RETURN_DELAY_TIME,address,length)) - { - dyn_slave_set_return_delay(&darwin_dyn_slave,data[DARWIN_RETURN_DELAY_TIME-address]); - ram_data[DARWIN_RETURN_DELAY_TIME]=data[DARWIN_RETURN_DELAY_TIME-address]; - } - if(ram_in_range(DARWIN_RETURN_LEVEL,address,length)) - { - dyn_slave_set_return_level(&darwin_dyn_slave,data[DARWIN_RETURN_LEVEL-address]); - ram_data[DARWIN_RETURN_LEVEL]=data[DARWIN_RETURN_LEVEL-address]; - } - // GPIO commands - if(gpio_in_range(address,length)) - gpio_process_write_cmd(address,length,data); - // ADC commands - if(adc_in_range(address,length)) - adc_process_write_cmd(address,length,data); - // IMU commands - if(imu_in_range(address,length)) - imu_process_write_cmd(address,length,data); - // 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); - // walk commands - if(walking_in_range(address,length)) - walking_process_write_cmd(address,length,data); - // joint motion commands - if(joint_motion_in_range(address,length)) - joint_motion_process_write_cmd(address,length,data); - // head_tracking commands - if(head_tracking_in_range(address,length)) - head_tracking_process_write_cmd(address,length,data); - // gripper commands - if(grippers_in_range(address,length)) - grippers_process_write_cmd(address,length,data); - // 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>8, DEVICE_MODEL_OFFSET+1,// model number MSB - DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version - DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id - DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate - DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time - DEFAULT_MM_PERIOD&0xFF, 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,// return level - DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET, - DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET, - DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET, - DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET, - DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET, - DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET, - DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET, - DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET, - DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET, - DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET, - DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET, - DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET, - DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET, - DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET, - DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET, - DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET, - DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET, - DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET, - DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET, - DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET, - DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET, - DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET, - DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET, - DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET, - DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET, - DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET, - DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET, - DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET, - DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET, - DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET, - DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET, - DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET, - DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET, - DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET, - DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET, - DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET, - DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET, - DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET, - DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF, - DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1, - DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME, - DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1, - DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO, - DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO, - DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT, - DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT, - DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN, - DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET, - DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN, - DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL, - DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL, - DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P, - DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1, - DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I, - DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1, - DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D, - DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, - DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP, - DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1, - DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, - DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P, - DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1, - DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I, - DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1, - DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D, - DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1, - DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP, - DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1, - DEFAULT_GRIPPER_LEFT_TOP_ID, GRIPPER_LEFT_TOP_ID, - DEFAULT_GRIPPER_LEFT_BOT_ID, GRIPPER_LEFT_BOT_ID, - DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE, - DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1, - DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE, - DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1, - DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE, - DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1, - DEFAULT_GRIPPER_RIGHT_TOP_ID, GRIPPER_RIGHT_TOP_ID, - DEFAULT_GRIPPER_RIGHT_BOT_ID, GRIPPER_RIGHT_BOT_ID, - DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE, - DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1, - 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_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD, - DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1, - DEFAULT_STAIRS_PHASE1_TIME&0xFF, STAIRS_PHASE1_TIME, - DEFAULT_STAIRS_PHASE1_TIME>>8, STAIRS_PHASE1_TIME+1, - DEFAULT_STAIRS_PHASE2_TIME&0xFF, STAIRS_PHASE2_TIME, - DEFAULT_STAIRS_PHASE2_TIME>>8, STAIRS_PHASE2_TIME+1, - DEFAULT_STAIRS_PHASE3_TIME&0xFF, STAIRS_PHASE3_TIME, - DEFAULT_STAIRS_PHASE3_TIME>>8, STAIRS_PHASE3_TIME+1, - DEFAULT_STAIRS_PHASE4_TIME&0xFF, STAIRS_PHASE4_TIME, - DEFAULT_STAIRS_PHASE4_TIME>>8, STAIRS_PHASE4_TIME+1, - DEFAULT_STAIRS_PHASE5_TIME&0xFF, STAIRS_PHASE5_TIME, - DEFAULT_STAIRS_PHASE5_TIME>>8, STAIRS_PHASE5_TIME+1, - DEFAULT_STAIRS_PHASE6_TIME&0xFF, STAIRS_PHASE6_TIME, - DEFAULT_STAIRS_PHASE6_TIME>>8, STAIRS_PHASE6_TIME+1, - DEFAULT_STAIRS_PHASE7_TIME&0xFF, STAIRS_PHASE7_TIME, - DEFAULT_STAIRS_PHASE7_TIME>>8, STAIRS_PHASE7_TIME+1, - DEFAULT_STAIRS_PHASE8_TIME&0xFF, STAIRS_PHASE8_TIME, - 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_X_OFFSET, STAIRS_X_OFFSET, - DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET, - DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_OFFSET, - DEFAULT_STAIRS_R_OFFSET, STAIRS_R_OFFSET, - DEFAULT_STAIRS_P_OFFSET, STAIRS_P_OFFSET, - DEFAULT_STAIRS_A_OFFSET, STAIRS_A_OFFSET, - DEFAULT_STAIRS_Y_SHIFT, STAIRS_Y_SHIFT, - DEFAULT_STAIRS_X_SHIFT, STAIRS_X_SHIFT, - DEFAULT_STAIRS_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT, - DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT, - DEFAULT_STAIRS_HIP_PITCH_OFF&0xFF, STAIRS_HIP_PITCH_OFF, - DEFAULT_STAIRS_HIP_PITCH_OFF>>8, STAIRS_HIP_PITCH_OFF+1, - DEFAULT_STAIRS_R_SHIFT, STAIRS_R_SHIFT, - DEFAULT_STAIRS_P_SHIFT, STAIRS_P_SHIFT, - DEFAULT_STAIRS_A_SHIFT, STAIRS_A_SHIFT, - DEFAULT_STAIRS_Y_SPREAD, STAIRS_Y_SPREAD, - DEFAULT_STAIRS_X_SHIFT_BODY, STAIRS_X_SHIFT_BODY}; +uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF}; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ -- GitLab From 349a73bff970eefcc15d97a5ff2e65e815500a44 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Tue, 10 Jul 2018 10:08:48 +0200 Subject: [PATCH 26/30] Removed the inline modifiers. --- include/action.h | 4 ++-- include/adc_dma.h | 2 +- include/darwin_dyn_master.h | 4 ++-- include/darwin_dyn_master_v2.h | 4 ++-- include/grippers.h | 4 ++-- include/head_tracking.h | 4 ++-- include/joint_motion.h | 4 ++-- include/motion_manager.h | 28 ++++++++++++++-------------- include/ram.h | 6 +++--- include/smart_charger.h | 4 ++-- include/stairs.h | 4 ++-- include/walking.h | 4 ++-- src/action.c | 4 ++-- src/adc_dma.c | 2 +- src/darwin_dyn_master.c | 6 +++--- src/darwin_dyn_master_v2.c | 6 +++--- src/darwin_dyn_slave.c | 30 +++++++++++++++--------------- src/grippers.c | 4 ++-- src/head_tracking.c | 4 ++-- src/imu.c | 2 +- src/joint_motion.c | 4 ++-- src/motion_manager.c | 26 +++++++++++++------------- src/ram.c | 6 +++--- src/smart_charger.c | 4 ++-- src/stairs.c | 4 ++-- src/walking.c | 6 +++--- 26 files changed, 90 insertions(+), 90 deletions(-) diff --git a/include/action.h b/include/action.h index 98626f1..6a3817a 100755 --- a/include/action.h +++ b/include/action.h @@ -10,8 +10,8 @@ extern "C" { // public functions void action_init(uint16_t period_us); -inline void action_set_period(uint16_t period_us); -inline uint16_t action_get_period(void); +void action_set_period(uint16_t period_us); +uint16_t action_get_period(void); uint8_t action_load_page(uint8_t page_id); void action_start_page(void); void action_stop_page(void); diff --git a/include/adc_dma.h b/include/adc_dma.h index db29c8d..f846f42 100755 --- a/include/adc_dma.h +++ b/include/adc_dma.h @@ -15,7 +15,7 @@ typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH void adc_init(void); void adc_start(void); void adc_set_period(uint8_t period_ms); -inline uint8_t adc_get_period(void); +uint8_t adc_get_period(void); unsigned short adc_get_channel(adc_ch_t channel); unsigned short adc_get_channel_raw(adc_ch_t channel); unsigned short adc_get_temperature(void); diff --git a/include/darwin_dyn_master.h b/include/darwin_dyn_master.h index 8031f12..cca8fc1 100644 --- a/include/darwin_dyn_master.h +++ b/include/darwin_dyn_master.h @@ -12,8 +12,8 @@ extern "C" { extern TDynamixelMaster darwin_dyn_master; void darwin_dyn_master_init(void); -inline void darwin_dyn_master_enable_power(void); -inline void darwin_dyn_master_disable_power(void); +void darwin_dyn_master_enable_power(void); +void darwin_dyn_master_disable_power(void); #ifdef __cplusplus } diff --git a/include/darwin_dyn_master_v2.h b/include/darwin_dyn_master_v2.h index a7e5d19..2f27508 100644 --- a/include/darwin_dyn_master_v2.h +++ b/include/darwin_dyn_master_v2.h @@ -12,8 +12,8 @@ extern "C" { extern TDynamixelMaster darwin_dyn_master_v2; void darwin_dyn_master_v2_init(void); -inline void darwin_dyn_master_v2_enable_power(void); -inline void darwin_dyn_master_v2_disable_power(void); +void darwin_dyn_master_v2_enable_power(void); +void darwin_dyn_master_v2_disable_power(void); #ifdef __cplusplus } diff --git a/include/grippers.h b/include/grippers.h index 9a1aa5f..b8818bd 100644 --- a/include/grippers.h +++ b/include/grippers.h @@ -12,8 +12,8 @@ typedef enum {GRIPPER_LEFT,GRIPPER_RIGHT} grippers_t; // public functions void grippers_init(uint16_t period_us); -inline void grippers_set_period(uint16_t period_us); -inline uint16_t grippers_get_period(void); +void grippers_set_period(uint16_t period_us); +uint16_t grippers_get_period(void); void grippers_open(grippers_t gripper); void grippers_close(grippers_t gripper); void grippers_stop(grippers_t gripper); diff --git a/include/head_tracking.h b/include/head_tracking.h index 31d3a53..f556264 100644 --- a/include/head_tracking.h +++ b/include/head_tracking.h @@ -10,8 +10,8 @@ extern "C" { // public functions void head_tracking_init(uint16_t period_us); -inline void head_tracking_set_period(uint16_t period_us); -inline uint16_t head_tracking_get_period(void); +void head_tracking_set_period(uint16_t period_us); +uint16_t head_tracking_get_period(void); void head_tracking_start(void); void head_tracking_stop(void); uint8_t head_is_tracking(void); diff --git a/include/joint_motion.h b/include/joint_motion.h index 70587b3..f2f0e16 100644 --- a/include/joint_motion.h +++ b/include/joint_motion.h @@ -12,8 +12,8 @@ extern "C" { // public functions void joint_motion_init(uint16_t period_us); -inline void joint_motion_set_period(uint16_t period_us); -inline uint16_t joint_motion_get_period(void); +void joint_motion_set_period(uint16_t period_us); +uint16_t joint_motion_get_period(void); void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos); uint32_t joint_motion_get_group_servos(uint8_t group_id); void joint_motion_start(uint8_t group_id); diff --git a/include/motion_manager.h b/include/motion_manager.h index d0f234c..7f10bd7 100755 --- a/include/motion_manager.h +++ b/include/motion_manager.h @@ -7,7 +7,7 @@ extern "C" { #endif #include "stm32f1xx.h" -#include "dynamixel_common.h" +#include "dyn_common.h" typedef enum { R_SHOULDER_PITCH = 1, @@ -86,25 +86,25 @@ extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS]; /** \brief Initialize motion manager module */ void manager_init(uint16_t period_us); -inline uint16_t manager_get_period(void); -inline uint16_t manager_get_period_us(void); -inline void manager_set_period(uint16_t period_us); -inline void manager_enable(void); -inline void manager_disable(void); -inline uint8_t manager_is_enabled(void); +uint16_t manager_get_period(void); +uint16_t manager_get_period_us(void); +void manager_set_period(uint16_t period_us); +void manager_enable(void); +void manager_disable(void); +uint8_t manager_is_enabled(void); void manager_enable_balance(void); void manager_disable_balance(void); -inline uint8_t manager_has_fallen(void); +uint8_t manager_has_fallen(void); TFall manager_get_fallen_position(void); -inline uint8_t manager_get_num_servos(void); +uint8_t manager_get_num_servos(void); void manager_set_module(uint8_t servo_id,TModules module); -inline TModules manager_get_module(uint8_t servo_id); +TModules manager_get_module(uint8_t servo_id); void manager_enable_servo(uint8_t servo_id); -inline void manager_disable_servo(uint8_t servo_id); -inline uint8_t manager_is_servo_enabled(uint8_t servo_id); +void manager_disable_servo(uint8_t servo_id); +uint8_t manager_is_servo_enabled(uint8_t servo_id); void manager_set_offset(uint8_t servo_id,int8_t offset); -inline int16_t manager_get_cw_angle_limit(uint8_t servo_id); -inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id); +int16_t manager_get_cw_angle_limit(uint8_t servo_id); +int16_t manager_get_ccw_angle_limit(uint8_t servo_id); // motion modules handling // operation functions uint8_t manager_in_range(unsigned short int address, unsigned short int length); diff --git a/include/ram.h b/include/ram.h index d920367..6256085 100755 --- a/include/ram.h +++ b/include/ram.h @@ -19,15 +19,15 @@ extern "C" { extern uint8_t ram_data[RAM_SIZE]; void ram_init(void); -inline void ram_read_byte(uint16_t address, uint8_t *data); -inline void ram_read_word(uint16_t address, uint16_t *data); +void ram_read_byte(uint16_t address, uint8_t *data); +void ram_read_word(uint16_t address, uint16_t *data); uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data); uint8_t ram_set_bit(uint16_t address, uint8_t bit); uint8_t ram_clear_bit(uint16_t address, uint8_t bit); uint8_t ram_write_byte(uint16_t address, uint8_t data); uint8_t ram_write_word(uint16_t address, uint16_t data); uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data); -inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length); +uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length); uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length); #ifdef __cplusplus diff --git a/include/smart_charger.h b/include/smart_charger.h index 42c3217..0ad973b 100644 --- a/include/smart_charger.h +++ b/include/smart_charger.h @@ -37,13 +37,13 @@ void smart_charger_init(uint16_t period_us); * * \param master pointer to a TDynamixelMaster structure ((darwin_dyn_master or darwin_dyn_master_v2). */ -inline void smart_charger_set_master(TDynamixelMaster *master); +void smart_charger_set_master(TDynamixelMaster *master); /** * \brief Function to get the master version of the Dynamixel protocol set to the module * \return the Dynamixel master structure associated to the smart charger module (darwin_dyn_master or darwin_dyn_master_v2). */ -inline TDynamixelMaster* smart_charger_get_master(void); +TDynamixelMaster* smart_charger_get_master(void); /** * \brief Function to set smart charger's period * diff --git a/include/stairs.h b/include/stairs.h index 4cb0bac..a34b4dc 100755 --- a/include/stairs.h +++ b/include/stairs.h @@ -10,8 +10,8 @@ extern "C" { // public functions void stairs_init(uint16_t period_us); -inline void stairs_set_period(uint16_t period_us); -inline uint16_t stairs_get_period(void); +void stairs_set_period(uint16_t period_us); +uint16_t stairs_get_period(void); void stairs_start(uint8_t up); void stairs_stop(void); uint8_t is_climbing_stairs(void); diff --git a/include/walking.h b/include/walking.h index 4e09755..30bc04e 100755 --- a/include/walking.h +++ b/include/walking.h @@ -10,8 +10,8 @@ extern "C" { // public functions void walking_init(uint16_t period_us); -inline void walking_set_period(uint16_t period_us); -inline uint16_t walking_get_period(void); +void walking_set_period(uint16_t period_us); +uint16_t walking_get_period(void); void walking_start(void); void walking_stop(void); uint8_t is_walking(void); diff --git a/src/action.c b/src/action.c index 47c264f..bfe36ed 100755 --- a/src/action.c +++ b/src/action.c @@ -273,13 +273,13 @@ void action_init(uint16_t period_us) action_period_us=period_us; } -inline void action_set_period(uint16_t period_us) +void action_set_period(uint16_t period_us) { action_period=(period_us<<16)/1000000; action_period_us=period_us; } -inline uint16_t action_get_period(void) +uint16_t action_get_period(void) { return action_period_us; } diff --git a/src/adc_dma.c b/src/adc_dma.c index eb0f821..532a58a 100755 --- a/src/adc_dma.c +++ b/src/adc_dma.c @@ -379,7 +379,7 @@ void adc_set_period(uint8_t period_ms) ram_data[DARWIN_ADC_PERIOD]=period_ms; } -inline uint8_t adc_get_period(void) +uint8_t adc_get_period(void) { return ram_data[DARWIN_ADC_PERIOD]; } diff --git a/src/darwin_dyn_master.c b/src/darwin_dyn_master.c index 7b3c0e9..9762cf6 100755 --- a/src/darwin_dyn_master.c +++ b/src/darwin_dyn_master.c @@ -89,16 +89,16 @@ void darwin_dyn_master_init(void) /* configure dynamixel master module */ dyn_master_set_rx_timeout(&darwin_dyn_master,50); - dyn_master_set_return_level(&darwin_dyn_master,return_all); + dyn_master_set_return_level(&darwin_dyn_master,RETURN_ALL); } -inline void darwin_dyn_master_enable_power(void) +void darwin_dyn_master_enable_power(void) { HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET); ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR; } -inline void darwin_dyn_master_disable_power(void) +void darwin_dyn_master_disable_power(void) { HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET); ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR); diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c index 6152125..732bd56 100755 --- a/src/darwin_dyn_master_v2.c +++ b/src/darwin_dyn_master_v2.c @@ -82,17 +82,17 @@ void darwin_dyn_master_v2_init(void) /* configure dynamixel master module */ dyn_master_set_rx_timeout(&darwin_dyn_master_v2,50); - dyn_master_set_return_level(&darwin_dyn_master_v2,return_all); + dyn_master_set_return_level(&darwin_dyn_master_v2,RETURN_ALL); darwin_dyn_master_v2_disable_power(); } -inline void darwin_dyn_master_v2_enable_power(void) +void darwin_dyn_master_v2_enable_power(void) { HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET); ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2; } -inline void darwin_dyn_master_v2_disable_power(void) +void darwin_dyn_master_v2_disable_power(void) { HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET); ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2); diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c index b287d06..a1fbe97 100755 --- a/src/darwin_dyn_slave.c +++ b/src/darwin_dyn_slave.c @@ -28,11 +28,24 @@ TComm darwin_dyn_slave_comm; UART_InitTypeDef darwin_comm_init; TIM_HandleTypeDef darwin_dyn_slave_tim_handle; +/* interrupt service routines */ +void DYN_SLAVE_TIMER_IRQHandler(void) +{ + if(__HAL_TIM_GET_FLAG(&darwin_dyn_slave_tim_handle, TIM_FLAG_UPDATE) != RESET) + { + if(__HAL_TIM_GET_IT_SOURCE(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE) !=RESET) + { + __HAL_TIM_CLEAR_IT(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE); + dyn_slave_loop(&darwin_dyn_slave); + } + } +} + // private functions unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data) { unsigned char error; - + if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length)) gpio_process_read_cmd(address,length,data); /* dynamixel slave internal operation registers */ @@ -109,20 +122,7 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng void darwin_on_ping(void) { } - -/* interrupt service routines */ -void DYN_SLAVE_TIMER_IRQHandler(void) -{ - if(__HAL_TIM_GET_FLAG(&darwin_dyn_slave_tim_handle, TIM_FLAG_UPDATE) != RESET) - { - if(__HAL_TIM_GET_IT_SOURCE(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE) !=RESET) - { - __HAL_TIM_CLEAR_IT(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE); - dyn_slave_loop(&darwin_dyn_slave); - } - } -} - + // public functions void darwin_dyn_slave_init(void) { diff --git a/src/grippers.c b/src/grippers.c index 5544346..14ab402 100644 --- a/src/grippers.c +++ b/src/grippers.c @@ -46,13 +46,13 @@ void grippers_init(uint16_t period_us) grippers_close(GRIPPER_RIGHT); } -inline void grippers_set_period(uint16_t period_us) +void grippers_set_period(uint16_t period_us) { grippers_period=(period_us<<16)/1000000; grippers_period_us=period_us; } -inline uint16_t grippers_get_period(void) +uint16_t grippers_get_period(void) { return grippers_period_us; } diff --git a/src/head_tracking.c b/src/head_tracking.c index 6226a9a..3850168 100644 --- a/src/head_tracking.c +++ b/src/head_tracking.c @@ -55,13 +55,13 @@ void head_tracking_init(uint16_t period_us) head_tracking_enabled=0x00; } -inline void head_tracking_set_period(uint16_t period_us) +void head_tracking_set_period(uint16_t period_us) { head_tracking_period=(period_us<<16)/1000000; head_tracking_period_us=period_us; } -inline uint16_t head_tracking_get_period(void) +uint16_t head_tracking_get_period(void) { return head_tracking_period_us; } diff --git a/src/imu.c b/src/imu.c index bbaed72..f911915 100755 --- a/src/imu.c +++ b/src/imu.c @@ -118,7 +118,7 @@ SPI_HandleTypeDef hspi2; TIM_HandleTypeDef htim; // private functions -inline void imu_wait_op_done(void) +void imu_wait_op_done(void) { while(imu_op_state!=IMU_DONE); } diff --git a/src/joint_motion.c b/src/joint_motion.c index 395bfc0..f4153fe 100644 --- a/src/joint_motion.c +++ b/src/joint_motion.c @@ -48,13 +48,13 @@ void joint_motion_init(uint16_t period_us) joint_motion_period_us=period_us; } -inline void joint_motion_set_period(uint16_t period_us) +void joint_motion_set_period(uint16_t period_us) { joint_motion_period=(period_us<<16)/1000000; joint_motion_period_us=period_us; } -inline uint16_t joint_motion_get_period(void) +uint16_t joint_motion_get_period(void) { return joint_motion_period_us; } diff --git a/src/motion_manager.c b/src/motion_manager.c index 805fc18..a5476c5 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -71,24 +71,24 @@ void manager_send_motion_command(void) dyn_master_sync_write(&darwin_dyn_master_v2,num,servo_ids,XL_P_GAIN,3,write_data); } -inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle) +uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle) { return ((angle+manager_servos[servo_id].center_angle)*manager_servos[servo_id].encoder_resolution)/manager_servos[servo_id].max_angle; } -inline int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value) +int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value) { return (((int16_t)((value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution))-manager_servos[servo_id].center_angle); } -inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed) +uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed) { if(speed>manager_servos[servo_id].max_speed) speed=manager_servos[servo_id].max_speed; return (speed*3)>>1; } -inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value) +uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value) { return (value*2)/3; } @@ -498,7 +498,7 @@ void manager_set_period(uint16_t period_us) head_tracking_init(period_us); } -inline void manager_enable(void) +void manager_enable(void) { TIM_OC_InitTypeDef TIM_OCInitStructure; uint16_t capture; @@ -513,13 +513,13 @@ inline void manager_enable(void) ram_data[DARWIN_MM_CNTRL]|=MANAGER_ENABLE; } -inline void manager_disable(void) +void manager_disable(void) { HAL_TIM_OC_Stop_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1); ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_ENABLE); } -inline uint8_t manager_is_enabled(void) +uint8_t manager_is_enabled(void) { return ram_data[DARWIN_MM_CNTRL]&MANAGER_ENABLE; } @@ -554,7 +554,7 @@ TFall manager_get_fallen_position(void) return MM_STANDING; } -inline uint8_t manager_get_num_servos(void) +uint8_t manager_get_num_servos(void) { return manager_num_servos; } @@ -575,7 +575,7 @@ void manager_set_module(uint8_t servo_id,TModules module) } } -inline TModules manager_get_module(uint8_t servo_id) +TModules manager_get_module(uint8_t servo_id) { if(servo_id>=0 && servo_id=0 && servo_id=address && reg<(address+length)) return 0x01; diff --git a/src/smart_charger.c b/src/smart_charger.c index 03a9ed8..c31f13e 100644 --- a/src/smart_charger.c +++ b/src/smart_charger.c @@ -29,12 +29,12 @@ void smart_charger_init(uint16_t period_us) ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs); } -inline void smart_charger_set_master(TDynamixelMaster *master) +void smart_charger_set_master(TDynamixelMaster *master) { dyn_battery_master=master; } -inline TDynamixelMaster* smart_charger_get_master(void) +TDynamixelMaster* smart_charger_get_master(void) { return dyn_battery_master; } diff --git a/src/stairs.c b/src/stairs.c index 68795d9..4255587 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -58,12 +58,12 @@ void stairs_init(uint16_t period_us) stairs_up=0x00; } -inline void stairs_set_period(uint16_t period_us) +void stairs_set_period(uint16_t period_us) { stairs_period=((float)period_us)/1000.0; } -inline uint16_t stairs_get_period(void) +uint16_t stairs_get_period(void) { return (uint16_t)(stairs_period*1000); } diff --git a/src/walking.c b/src/walking.c index e261ad9..57277de 100755 --- a/src/walking.c +++ b/src/walking.c @@ -67,7 +67,7 @@ uint8_t m_Ctrl_Running; float walking_period; // private functions -inline float wsin(float time, float period, float period_shift, float mag, float mag_shift) +float wsin(float time, float period, float period_shift, float mag, float mag_shift) { return mag*sin(((2.0*PI*time)/period)-period_shift)+mag_shift; } @@ -228,12 +228,12 @@ void walking_init(uint16_t period_us) walking_process(); } -inline void walking_set_period(uint16_t period_us) +void walking_set_period(uint16_t period_us) { walking_period=((float)period_us)/1000.0; } -inline uint16_t walking_get_period(void) +uint16_t walking_get_period(void) { return (uint16_t)(walking_period*1000); } -- GitLab From 3853bcede9c0e8eb4a49e70374249cba1b7dbe8a Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Fri, 10 Aug 2018 10:27:18 +0200 Subject: [PATCH 27/30] Changed the way to compute the velocity profile to improve accuracy and avoid errors. --- src/joint_motion.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/joint_motion.c b/src/joint_motion.c index f4153fe..bd537d0 100644 --- a/src/joint_motion.c +++ b/src/joint_motion.c @@ -220,6 +220,7 @@ void joint_motion_process(void) moving=0x01; if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed { + joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17); if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) { joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed @@ -232,7 +233,7 @@ void joint_motion_process(void) if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; } - joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16); + joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17); if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) joint_current_angles[i]=joint_target_angles[i]; if(joint_dir[i]==-1 && joint_current_angles[i]>17); joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16); + if(joint_target_speeds[i]<0) + joint_target_speeds[i]=0; joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; - joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16); + joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17); if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) joint_current_angles[i]=joint_target_angles[i]; if(joint_dir[i]==-1 && joint_current_angles[i] Date: Wed, 10 Jul 2019 09:53:47 +0200 Subject: [PATCH 28/30] Updated the head status flag in the head control register. --- src/head_tracking.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/head_tracking.c b/src/head_tracking.c index 3850168..12ee27c 100644 --- a/src/head_tracking.c +++ b/src/head_tracking.c @@ -69,11 +69,13 @@ uint16_t head_tracking_get_period(void) void head_tracking_start(void) { head_tracking_enabled=0x01; + ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS; } void head_tracking_stop(void) { head_tracking_enabled=0x00; + ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS); } uint8_t head_is_tracking(void) -- GitLab From 055e4fbbaffa8083991dd4f5a42f93dea92bc21d Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Wed, 18 Sep 2019 12:52:19 +0200 Subject: [PATCH 29/30] Used the scheduler from the library. --- include/darwin_sch.h | 10 ++++ include/scheduler.h | 23 -------- src/darwin_sch.c | 128 +++++++++++++++++++++++++++++++++++++++++++ src/scheduler.c | 58 -------------------- 4 files changed, 138 insertions(+), 81 deletions(-) create mode 100644 include/darwin_sch.h delete mode 100644 include/scheduler.h create mode 100644 src/darwin_sch.c delete mode 100644 src/scheduler.c diff --git a/include/darwin_sch.h b/include/darwin_sch.h new file mode 100644 index 0000000..21ce2fe --- /dev/null +++ b/include/darwin_sch.h @@ -0,0 +1,10 @@ +#ifndef _DARWIN_SCH_H +#define _DARWIN_SCH_H + +#include "stm32f1xx.h" +#include "scheduler.h" + +TScheduler *darwin_sch_init(void); + +#endif + diff --git a/include/scheduler.h b/include/scheduler.h deleted file mode 100644 index 2970a43..0000000 --- a/include/scheduler.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef _SCHEDULER_H -#define _SCHEDULER_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" - -typedef enum {SCH_CH1=0,SCH_CH2=1,SCH_CH3=2,SCH_CH4=3} sch_ch_t; - -void scheduler_init(void); -void scheduler_set_period(sch_ch_t channel_id, uint16_t period_ms); -void scheduler_set_one_shot(sch_ch_t channel_id, uint16_t time_ms); -void scheduler_set_function(sch_ch_t channel_id, void (*function)(void)); -void scheduler_start(sch_ch_t channel_id); -void scheduler_stop(sch_ch_t channel_id); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/src/darwin_sch.c b/src/darwin_sch.c new file mode 100644 index 0000000..f27bee6 --- /dev/null +++ b/src/darwin_sch.c @@ -0,0 +1,128 @@ +#include "darwin_sch.h" + +/* private variables */ +TIM_HandleTypeDef darwin_sch_timer_handle; +TScheduler darwin_scheduler; + +/* interrupt handlers */ +void TIM1_CC_IRQHandler(void) +{ + /* Capture compare 1 event */ + if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC1) != RESET) + { + if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC1) !=RESET) + { + __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC1); + scheduler_interrupt(&darwin_scheduler,SCHED_CH1); + } + } + /* Capture compare 2 event */ + if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC2) != RESET) + { + if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC2) !=RESET) + { + __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC2); + scheduler_interrupt(&darwin_scheduler,SCHED_CH2); + } + } + /* Capture compare 3 event */ + if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC3) != RESET) + { + if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC3) !=RESET) + { + __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC3); + scheduler_interrupt(&darwin_scheduler,SCHED_CH3); + } + } + /* Capture compare 4 event */ + if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC4) != RESET) + { + if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC4) !=RESET) + { + __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC4); + scheduler_interrupt(&darwin_scheduler,SCHED_CH4); + } + } +} + +/* private functions */ +void darwin_sch_start(unsigned short int channel_id) +{ + HAL_TIM_OC_Start_IT(&darwin_sch_timer_handle,channel_id); +} + +void darwin_sch_stop(unsigned short int channel_id) +{ + HAL_TIM_OC_Stop_IT(&darwin_sch_timer_handle,channel_id); +} + +void darwin_sch_set_pulse(unsigned short int channel_id,unsigned short int pulse, unsigned char running) +{ + TIM_OC_InitTypeDef out_comp_config; + unsigned short int capture; + + if(running==0x00) + { + if(darwin_scheduler.channels[channel_id].enabled==0x00) + { + out_comp_config.OCMode = TIM_OCMODE_TIMING; + capture = HAL_TIM_ReadCapturedValue(&darwin_sch_timer_handle,channel_id); + out_comp_config.Pulse = capture+pulse; + out_comp_config.OCPolarity = TIM_OCPOLARITY_HIGH; + out_comp_config.OCFastMode = TIM_OCFAST_DISABLE; + HAL_TIM_OC_ConfigChannel(&darwin_sch_timer_handle, &out_comp_config, channel_id); + } + } + else + { + capture = HAL_TIM_ReadCapturedValue(&darwin_sch_timer_handle, channel_id); + __HAL_TIM_SetCompare(&darwin_sch_timer_handle,channel_id, capture+pulse); + } +} + +/* public functions */ +TScheduler *darwin_sch_init(void) +{ + TIM_ClockConfigTypeDef sClockSourceConfig; + TIM_MasterConfigTypeDef sMasterConfig; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig; + + __HAL_RCC_TIM1_CLK_ENABLE(); + + darwin_sch_timer_handle.Instance = TIM1; + darwin_sch_timer_handle.Init.Prescaler = 168; + darwin_sch_timer_handle.Init.CounterMode = TIM_COUNTERMODE_UP; + darwin_sch_timer_handle.Init.Period = 0xFFFF; + darwin_sch_timer_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + darwin_sch_timer_handle.Init.RepetitionCounter = 0; + HAL_TIM_Base_Init(&darwin_sch_timer_handle); + + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + HAL_TIM_ConfigClockSource(&darwin_sch_timer_handle, &sClockSourceConfig); + + HAL_TIM_OC_Init(&darwin_sch_timer_handle); + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + HAL_TIMEx_MasterConfigSynchronization(&darwin_sch_timer_handle, &sMasterConfig); + + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = 0; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + HAL_TIMEx_ConfigBreakDeadTime(&darwin_sch_timer_handle, &sBreakDeadTimeConfig); + + /* Peripheral interrupt init */ + HAL_NVIC_SetPriority(TIM1_CC_IRQn, 2, 0); + HAL_NVIC_EnableIRQ(TIM1_CC_IRQn); + + scheduler_init(&darwin_scheduler,4,168); + darwin_scheduler.start=darwin_sch_start; + darwin_scheduler.stop=darwin_sch_stop; + darwin_scheduler.set_pulse=darwin_sch_set_pulse; + + return &darwin_scheduler; +} diff --git a/src/scheduler.c b/src/scheduler.c deleted file mode 100644 index b8e0d99..0000000 --- a/src/scheduler.c +++ /dev/null @@ -1,58 +0,0 @@ -#include "scheduler.h" - -#define SCHEDULER_TIMER TIM1 -#define SCHEDULER_TIMER_IRQn TIM1_IRQn -#define SCHEDULER_TIMER_IRQHandler TIM1_IRQHandler -#define SCHEDULER_TIMER_CLK __HAL_RCC_TIM1_CLK_ENABLE() - -typedef void (*scheduler_function)(void); - -scheduler_function sch_ch1_fnct; -scheduler_function sch_ch2_fnct; -scheduler_function sch_ch3_fnct; -scheduler_function sch_ch4_fnct; -uint16_t sch_ch1_period; -uint16_t sch_ch2_period; -uint16_t sch_ch3_period; -uint16_t sch_ch4_period; - -void scheduler_init(void) -{ - /* initialize internal variables */ - sch_ch1_fnct=0; - sch_ch2_fnct=0; - sch_ch3_fnct=0; - sch_ch4_fnct=0; - sch_ch1_period=0; - sch_ch2_period=0; - sch_ch3_period=0; - sch_ch4_period=0; - /* initialize timer 1 */ - -} - -void scheduler_set_period(sch_ch_t channel_id, uint16_t period_ms) -{ - -} - -void scheduler_set_one_shot(sch_ch_t channel_id, uint16_t time_ms) -{ - -} - -void scheduler_set_function(sch_ch_t channel_id, void (*function)(void)) -{ - -} - -void scheduler_start(sch_ch_t channel_id) -{ - -} - -void scheduler_stop(sch_ch_t channel_id) -{ - -} - -- GitLab From 4128130b9198131c875677b766b39b1d4548b38d Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan Date: Wed, 18 Sep 2019 12:52:45 +0200 Subject: [PATCH 30/30] Changes to use the new implementation of the dynamixel slave interface. --- Makefile | 20 ++++++++---- include/darwin_conf.h | 11 ++++--- include/darwin_dyn_slave.h | 4 ++- include/eeprom_init.h | 8 ----- src/cm730_fw.c | 49 ++++++++++++++-------------- src/darwin_dyn_slave.c | 65 +++++++++++++------------------------- 6 files changed, 71 insertions(+), 86 deletions(-) diff --git a/Makefile b/Makefile index eeb471a..d9f8ca6 100755 --- a/Makefile +++ b/Makefile @@ -9,10 +9,11 @@ PROJECT_NAME=darwin_firmware TARGET_FILES=src/cm730_fw.c TARGET_FILES+=src/eeprom.c TARGET_FILES+=src/system_stm32f1xx.c -TARGET_FILES+=src/gpio.c +#TARGET_FILES+=src/gpio.c +TARGET_FILES+=src/darwin_sch.c TARGET_FILES+=src/darwin_time.c -TARGET_FILES+=src/adc_dma.c -TARGET_FILES+=src/imu.c +#TARGET_FILES+=src/adc_dma.c +#TARGET_FILES+=src/imu.c TARGET_FILES+=src/darwin_dyn_slave.c #TARGET_FILES+=src/darwin_dyn_master.c #TARGET_FILES+=src/darwin_dyn_master_v2.c @@ -42,13 +43,14 @@ COMM_PATH=$(STM32_LIBRARIES_PATH)/comm MEMORY_PATH=$(STM32_LIBRARIES_PATH)/memory USART_PATH=$(STM32_LIBRARIES_PATH)/f1/usart DYNAMIXEL_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_base +SCHEDULER_PATH=$(STM32_LIBRARIES_PATH)/scheduler BUILD_PATH=build COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float -DUSE_HAL_DRIVER -COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) +COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DHSE_VALUE=8000000 -imacros ./include/darwin_conf.h INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices -INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include +INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I$(SCHEDULER_PATH)/include -I./include DOC_DIR = ./doc @@ -98,7 +100,11 @@ TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c TARGET_FILES+=$(wildcard $(UTILS_PATH)/src/*.c) TARGET_FILES+=$(wildcard $(COMM_PATH)/src/*.c) TARGET_FILES+=$(wildcard $(MEMORY_PATH)/src/*.c) -TARGET_FILES+=$(wildcard $(DYNAMIXEL_PATH)/src/*.c) +TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel2.c +TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel.c +TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave.c +TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave_device.c +TARGET_FILES+=$(wildcard $(SCHEDULER_PATH)/src/*.c) DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o)) DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP)) @@ -124,6 +130,8 @@ $(BUILD_PATH)/%.o: $(MEMORY_PATH)/src/%.c $(CC) -c $(CFLAGS) -o $@ $< $(BUILD_PATH)/%.o: $(DYNAMIXEL_PATH)/src/%.c $(CC) -c $(CFLAGS) -o $@ $< +$(BUILD_PATH)/%.o: $(SCHEDULER_PATH)/src/%.c + $(CC) -c $(CFLAGS) -o $@ $< $(MAIN_OUT_ELF): mkdir_build $(DARWIN_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) $(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@ diff --git a/include/darwin_conf.h b/include/darwin_conf.h index 577308c..44ebbf8 100644 --- a/include/darwin_conf.h +++ b/include/darwin_conf.h @@ -1,23 +1,24 @@ #ifndef _DARWIN_CONF_H #define _DARWIN_CONF_H -#define RAM_SIZE 1024 -#define EEPROM_SIZE 256 +#define RAM_SIZE 1024 +#define EEPROM_SIZE 256 /* Dynamixel slave configuration */ #define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000) #define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010) -#define DEFAULT_DEVICE_MODEL 0x7300 +#define DEFAULT_DEVICE_MODEL 0x001D #define DEFAULT_FIRMWARE_VERSION 0x0001 -#define DEFAULT_DEVICE_ID 0x0001 -#define DEFAULT_BAUDRATE 0x0010 +#define DEFAULT_DEVICE_ID 0x0003 +#define DEFAULT_BAUDRATE 0x0022 #define DEFAULT_RETURN_DELAY 0x0000 #define DEFAULT_RETURN_LEVEL 0x0002 #define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024 #define MAX_DYN_SLAVE_RX_BUFFER_LEN 1024 #define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024 +#define MAX_NUM_SLAVE_DEVICES 4 /* GPIO configuration */ #define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100) diff --git a/include/darwin_dyn_slave.h b/include/darwin_dyn_slave.h index 49437d6..493b395 100644 --- a/include/darwin_dyn_slave.h +++ b/include/darwin_dyn_slave.h @@ -7,10 +7,12 @@ extern "C" { #include "stm32f1xx.h" #include "dynamixel_slave.h" +#include "dynamixel_slave_device.h" #include "dynamixel_slave_registers.h" +#include "scheduler.h" #include "memory.h" -uint8_t darwin_dyn_slave_init(TMemory *memory); +uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler); void darwin_dyn_slave_start(void); void darwin_dyn_slave_stop(void); diff --git a/include/eeprom_init.h b/include/eeprom_init.h index 09eca5c..0f1baf3 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -5,17 +5,11 @@ extern "C" { #endif -#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 0x1E78 //7800us #define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16 #define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999 #define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9 #define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5 -#define DEFAULT_RETURN_LEVEL 0x0002 #define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 #define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 #define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 @@ -110,8 +104,6 @@ extern "C" { #define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm #define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm -#define EEPROM_SIZE 137 - #ifdef __cplusplus } #endif diff --git a/src/cm730_fw.c b/src/cm730_fw.c index 859d26a..326cbe9 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -1,43 +1,44 @@ #include "stm32f1xx_hal.h" #include "darwin_conf.h" #include "memory.h" -#include "gpio.h" -#include "eeprom.h" -#include "adc_dma.h" -#include "imu.h" -#include "darwin_time.h" +#include "darwin_sch.h" #include "darwin_dyn_slave.h" -#include "darwin_dyn_master.h" -#include "darwin_dyn_master_v2.h" -#include "motion_manager.h" -#include "action.h" -#include "action_id.h" +#include "darwin_time.h" +#include "eeprom.h" +//#include "gpio.h" +//#include "adc_dma.h" +//#include "imu.h" +//#include "darwin_dyn_master.h" +//#include "darwin_dyn_master_v2.h" +//#include "motion_manager.h" +//#include "action.h" +//#include "action_id.h" -TMemory darwin_memory; +TMemory *darwin_memory; +TScheduler *scheduler; int main(void) { - uint16_t eeprom_data,period; +// uint16_t eeprom_data,period; /* initialize the HAL module */ HAL_Init(); /* initialize EEPROM */ EE_Init(); - /* initialize memory structure */ - mem_init(&darwin_memory); - darwin_memory.eeprom_write_data=EE_WriteVariable; - darwin_memory.eeprom_read_data=EE_ReadVariable; + /* initialize the scheduler */ + scheduler=darwin_sch_init(); /* initialize the GPIO module */ - gpio_init(&darwin_memory); +// gpio_init(&darwin_memory); // initialize adc - adc_init(&darwin_memory); +// adc_init(&darwin_memory); // initialize imu - imu_init(&darwin_memory); +// imu_init(&darwin_memory); // initialize time module -// darwin_time_init(); + darwin_time_init(); /* initialize the dynamixel slave interface */ -// darwin_dyn_slave_init(); -// darwin_dyn_slave_start(); + darwin_dyn_slave_init(&darwin_memory,scheduler); + darwin_memory->eeprom_write_data=EE_WriteVariable; + darwin_memory->eeprom_read_data=EE_ReadVariable; /* initialize motion manager module */ // EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data); // period=eeprom_data&0x00FF; @@ -46,7 +47,9 @@ int main(void) // manager_init(period); // gpio_set_led(LED_4); /* initialize the ram module */ - ram_init(&darwin_memory); + ram_init(darwin_memory); + + darwin_dyn_slave_start(); while(1);/* main function does not return */ } diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c index eb680db..09c22bc 100755 --- a/src/darwin_dyn_slave.c +++ b/src/darwin_dyn_slave.c @@ -3,22 +3,20 @@ #include "usart3.h" #include "ram.h" -/* timer for the execution of the dynamixel slave loop */ -#define DYN_SLAVE_TIMER TIM7 -#define DYN_SLAVE_TIMER_IRQn TIM7_IRQn -#define DYN_SLAVE_TIMER_IRQHandler TIM7_IRQHandler -#define DYN_SLAVE_TIMER_ENABLE_CLK __HAL_RCC_TIM7_CLK_ENABLE() - /* private variables */ TDynamixelSlave darwin_dyn_slave; +TDynamixelSlaveDevice darwin_dyn_slave_device; TTime darwin_dyn_slave_timer; TComm darwin_dyn_slave_comm; UART_InitTypeDef darwin_comm_init; -TIM_HandleTypeDef darwin_dyn_slave_tim_handle; +TScheduler *darwin_dyn_slave_sch; /* memory module */ TMemory *darwin_dyn_slave_memory; +/* eeprom data */ +dyn_slave_control_eeprom_data(darwin_dyn_slave,".eeprom",EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2); + // private functions unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data) { @@ -38,21 +36,8 @@ void darwin_on_ping(void) { } -/* interrupt service routines */ -void DYN_SLAVE_TIMER_IRQHandler(void) -{ - if(__HAL_TIM_GET_FLAG(&darwin_dyn_slave_tim_handle, TIM_FLAG_UPDATE) != RESET) - { - if(__HAL_TIM_GET_IT_SOURCE(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE) !=RESET) - { - __HAL_TIM_CLEAR_IT(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE); - dyn_slave_loop(&darwin_dyn_slave); - } - } -} - // public functions -uint8_t darwin_dyn_slave_init(TMemory *memory) +uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler) { TUSART_IRQ_Priorities priorities; uint8_t status; @@ -62,7 +47,7 @@ uint8_t darwin_dyn_slave_init(TMemory *memory) /* initialize the comm object */ comm_init(&darwin_dyn_slave_comm,0x01,&darwin_dyn_slave_timer); - darwin_comm_init.BaudRate = 2000000/(ram_data[BAUDRATE]+1); + darwin_comm_init.BaudRate = 2000000/(ram_data[DEFAULT_BAUDRATE]+1); darwin_comm_init.WordLength = UART_WORDLENGTH_8B; darwin_comm_init.StopBits = UART_STOPBITS_1; darwin_comm_init.Parity = UART_PARITY_NONE; @@ -78,36 +63,30 @@ uint8_t darwin_dyn_slave_init(TMemory *memory) priorities.dma_tx_subpriority=0; usart3_init(&darwin_dyn_slave_comm,&darwin_comm_init,&priorities); - status=dyn_slave_init(&darwin_dyn_slave,memory,&darwin_dyn_slave_comm,ram_data[DEVICE_ID],DYN_VER2); - darwin_dyn_slave_memory=memory; - darwin_dyn_slave.on_read=darwin_on_read; - darwin_dyn_slave.on_write=darwin_on_write; - darwin_dyn_slave.on_ping=darwin_on_ping; - dyn_slave_set_return_delay(&darwin_dyn_slave,ram_data[RETURN_DELAY]); - dyn_slave_set_return_level(&darwin_dyn_slave,ram_data[RETURN_LEVEL]); - - /* initialize timer for the execution of the dynamixel slave loop */ - DYN_SLAVE_TIMER_ENABLE_CLK; - darwin_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER; - darwin_dyn_slave_tim_handle.Init.Period = 1000; - darwin_dyn_slave_tim_handle.Init.Prescaler = 72; - darwin_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - darwin_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP; - HAL_TIM_Base_Init(&darwin_dyn_slave_tim_handle); - // initialize the timer interrupts - HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 2); - HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn); + // initialize slave device + dyn_slave_device_init(&darwin_dyn_slave_device,DEFAULT_DEVICE_ID,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2); + darwin_dyn_slave_device.on_read=darwin_on_read; + darwin_dyn_slave_device.on_write=darwin_on_write; + darwin_dyn_slave_device.on_ping=darwin_on_ping; + dyn_slave_device_set_return_delay(&darwin_dyn_slave_device,DEFAULT_RETURN_DELAY); + dyn_slave_device_set_return_level(&darwin_dyn_slave_device,DEFAULT_RETURN_LEVEL); + (*memory)=dyn_slave_device_get_memory(&darwin_dyn_slave_device); + darwin_dyn_slave_memory=dyn_slave_device_get_memory(&darwin_dyn_slave_device); + + status=dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,scheduler,SCHED_CH1,DYN_VER2); + darwin_dyn_slave.set_baudrate=usart3_set_baudrate; + dyn_slave_add_device(&darwin_dyn_slave,&darwin_dyn_slave_device); return status; } void darwin_dyn_slave_start(void) { - HAL_TIM_Base_Start_IT(&darwin_dyn_slave_tim_handle); + dyn_slave_start(&darwin_dyn_slave); } void darwin_dyn_slave_stop(void) { - HAL_TIM_Base_Stop_IT(&darwin_dyn_slave_tim_handle); + dyn_slave_stop(&darwin_dyn_slave); } -- GitLab