From 9a06538b4f79329dce51f3f4d48e41702a2180d5 Mon Sep 17 00:00:00 2001 From: Irene Garcia Camacho <igarcia@iri.upc.edu> Date: Mon, 22 May 2017 16:33:38 +0200 Subject: [PATCH 01/21] 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<num; i++){ + for(i=current; i<num; i++) + { dyn_master_read_word(&darwin_dyn_master,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model); switch(model) { 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_version(&darwin_dyn_master); //set bus Dynamixel master version - // Read smart charger's memory map - //dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]); - //Read charger status - dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]); - break; + ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID + 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]); + break; default: break; - } + } } darwin_dyn_master_disable_power(); @@ -384,20 +384,20 @@ void manager_init(uint16_t period_us) } //Other devices detected - smart charger - for(i=current; i<num; i++){ + for(i=current; i<num; i++) + { dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model); switch(model) { 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_version(&darwin_dyn_master_v2); //Set bus Dynamixel master version - // Read smart charger's memory map - //dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]); - //Read charger status - dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]); + 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]); break; default: break; - } + } } diff --git a/src/smart_charger.c b/src/smart_charger.c index 70de2e7..3812797 100644 --- a/src/smart_charger.c +++ b/src/smart_charger.c @@ -8,17 +8,17 @@ 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 -uint16_t counter; //for read access operations +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 +#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<fifosize) //free space in fifo + if(numdata<FIFOSIZE) //free space in fifo { for(i=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;i<=DARWIN_SMART_CHARGER_LIMIT_CURRENT_H;i++) { @@ -144,17 +143,19 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i // motion manager interface function void smart_charger_process(void) -{ - gpio_set_led(LED_TX);//amarillo - counter++; +{ uint8_t error; + static uint16_t counter; + + counter++; + //Write smart_charger - Battery limit current (EEPROM) if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count) { error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2, current); if(error==DYN_SUCCESS){ /* dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo); - if(outfifo==&write_fifo[fifosize-1]) + if(outfifo==&write_fifo[FIFOSIZE-1]) outfifo=&write_fifo[0]; //go to first position of fifo else outfifo=outfifo+2; diff --git a/src/test_charger.c b/src/test_charger.c index 5a547fc..23cb9b8 100644 --- a/src/test_charger.c +++ b/src/test_charger.c @@ -74,7 +74,7 @@ int main(void) gpio_set_led(LED_3);//Si existen dispositivos conectados -> 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 <igarcia@iri.upc.edu> Date: Wed, 24 May 2017 11:12:03 +0200 Subject: [PATCH 02/21] 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<FIFOSIZE) //free space in fifo - { - for(i=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;i<=DARWIN_SMART_CHARGER_LIMIT_CURRENT_H;i++) - { - *infifo=data[i-address]; - infifo++; - numdata++; - } - */ - /* *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address]; //copy first byte to fifo - numdata++; - infifo++; - *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; //copy second byte to fifo - numdata++; - infifo++; */ - // } + ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address]; + ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; smart_charger_write = 0x01; } } @@ -144,38 +113,27 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i // motion manager interface function void smart_charger_process(void) { + //gpio_set_led(LED_3); uint8_t error; - static uint16_t counter; + static uint16_t counter=0; counter++; //Write smart_charger - Battery limit current (EEPROM) if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count) { - error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2, current); - if(error==DYN_SUCCESS){ -/* dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo); - if(outfifo==&write_fifo[FIFOSIZE-1]) - outfifo=&write_fifo[0]; //go to first position of fifo - else - outfifo=outfifo+2; - numdata=numdata-2; - if(numdata==0) //empty fifo - */ + error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]); + if(error==DYN_SUCCESS) smart_charger_write = 0x00; - } } //Read smart charger - time to empty, time to full and battery_status if(smart_charger_detected && smart_charger_enabled && counter==smart_charger_count) { - error=dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]); if(error!=DYN_SUCCESS){ + // gpio_set_led(LED_TX); ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs); - gpio_set_led(LED_RX); //naranja - }else{ - gpio_set_led(LED_3);//verde } counter = 0; } -- GitLab From f2be8d2926c9a7d84b89e96cce65f7a8f527e3a9 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Wed, 31 May 2017 14:36:05 +0200 Subject: [PATCH 03/21] 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 <stdlib.h> #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 <shernand@iri.upc.edu> Date: Wed, 28 Jun 2017 17:40:03 +0200 Subject: [PATCH 04/21] 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<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]; + servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]; + joint_motion_set_group_servos(1,servo_mask); } - if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF) + servo_mask=0x00000000; + if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF) { force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8); - dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_ID],XL_TORQUE_LIMIT_L,force); + dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force); + dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force); + servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]; + servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]; + joint_motion_set_group_servos(2,servo_mask); } + grippers_close(GRIPPER_LEFT); + grippers_close(GRIPPER_RIGHT); } inline void grippers_set_period(uint16_t period_us) @@ -54,27 +60,101 @@ inline uint16_t grippers_get_period(void) void grippers_open(grippers_t gripper) { if(gripper==GRIPPER_LEFT) - grippers_left_open=0x01; + { + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60; + joint_motion_start(1); + } else if(gripper==GRIPPER_RIGHT) - grippers_right_open=0x01; + { + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60; + joint_motion_start(2); + } } void grippers_close(grippers_t gripper) { if(gripper==GRIPPER_LEFT) - grippers_left_close=0x01; + { + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60; + joint_motion_start(1); + } else if(gripper==GRIPPER_RIGHT) - grippers_right_close=0x01; + { + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; + ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60; + joint_motion_start(2); + } +} + +void grippers_stop(grippers_t gripper) +{ + if(gripper==GRIPPER_LEFT) + joint_motion_stop(1); + else + joint_motion_stop(2); } uint8_t gripper_is_open(grippers_t gripper) { if(gripper==GRIPPER_LEFT) - return grippers_left_opened; - else if(gripper==GRIPPER_RIGHT) - return grippers_right_opened; + { + if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT) + return 0x01; + else + return 0x00; + } else - return 0x00; + { + if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT) + return 0x01; + else + return 0x00; + } +} + +uint8_t gripper_is_close(grippers_t gripper) +{ + if(gripper==GRIPPER_LEFT) + { + if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT) + return 0x00; + else + return 0x01; + } + else + { + if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT) + return 0x00; + else + return 0x01; + } +} + +uint8_t gripper_is_moving(grippers_t gripper) +{ + if(gripper==GRIPPER_LEFT) + return are_joints_moving(1); + else + return are_joints_moving(2); } // operation functions @@ -107,45 +187,32 @@ void grippers_process_write_cmd(unsigned short int address,unsigned short int le } } -// motion manager interface functions void grippers_process(void) { - uint16_t angle; - - if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF) + if(!are_joints_moving(1)) + { + ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT; + if(gripper_left_target==0x01) + ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT; + else + ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT; + } + else { - if(grippers_left_opened==0x01) - { - if(grippers_left_close==0x01) - { - grippers_left_close=0x00; - angle=ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_H]<<8); - manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9; - } - else if(grippers_left_open==0x01) - { - grippers_left_open=0x00; - angle=ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_H]<<8); - manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9; - } - } + ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT; + ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT; } - if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF) + if(!are_joints_moving(2)) + { + ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT; + if(gripper_right_target==0x01) + ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT; + else + ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT; + } + else { - if(grippers_right_opened==0x01) - { - if(grippers_right_close==0x01) - { - grippers_right_close=0x00; - angle=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]<<8); - manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9; - } - else if(grippers_right_open==0x01) - { - grippers_right_open=0x00; - angle=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]<<8); - manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9; - } - } + ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT; + ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT; } } diff --git a/src/motion_manager.c b/src/motion_manager.c index b6bb218..65f06bf 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -196,7 +196,7 @@ void MANAGER_TIMER_IRQHandler(void) head_tracking_process(); // call the walking process walking_process(); - // call the grippers process + // call the gripper process grippers_process(); // balance the robot manager_balance(); diff --git a/src/ram.c b/src/ram.c index e061c05..e708a46 100755 --- a/src/ram.c +++ b/src/ram.c @@ -119,8 +119,10 @@ void ram_init(void) ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0) ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_ID,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_ID]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(GRIPPER_LEFT_TOP_ID,&eeprom_data)==0) + ram_data[GRIPPER_LEFT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(GRIPPER_LEFT_BOT_ID,&eeprom_data)==0) + ram_data[GRIPPER_LEFT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0) ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0) @@ -133,8 +135,10 @@ void ram_init(void) ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0) ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_ID,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_ID]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(GRIPPER_RIGHT_TOP_ID,&eeprom_data)==0) + ram_data[GRIPPER_RIGHT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(GRIPPER_RIGHT_BOT_ID,&eeprom_data)==0) + ram_data[GRIPPER_RIGHT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0) ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0) -- GitLab From 58aa3e4e745a0f91a20537dd282d993c46352a67 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Fri, 30 Jun 2017 12:52:20 +0200 Subject: [PATCH 05/21] 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 <darwin@darwin.users.iri.prv> Date: Fri, 25 Aug 2017 00:02:01 +0200 Subject: [PATCH 06/21] 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 <shernand@iri.upc.edu> Date: Fri, 25 Aug 2017 12:52:45 +0200 Subject: [PATCH 07/21] 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 <shernand@iri.upc.edu> Date: Fri, 25 Aug 2017 14:56:02 +0200 Subject: [PATCH 08/21] 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 <math.h> + +typedef enum {SHIFT_WEIGHT_LEFT,RISE_RIGHT_FOOT,ADVANCE_RIGHT_FOOT,CONTACT_RIGHT_FOOT,SHIFT_WEIGHT_RIGHT,RISE_LEFT_FOOT,ADVANCE_LEFT_FOOT,CONTACT_LEFT_FOOT,CENTER_WEIGHT} stairs_phase_t; + +// 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?= <shernand@iri.upc.edu> Date: Sun, 27 Aug 2017 21:54:58 +0200 Subject: [PATCH 09/21] First working version of the climb stairs algorithm. Integrated the stairs algorithm to the motion manager structure. Tuned the stairs algorithm parameters to work in simulation. --- Makefile | 1 + include/darwin_registers.h | 39 +-- include/eeprom.h | 2 +- include/eeprom_init.h | 29 ++- src/darwin_dyn_slave.c | 3 + src/eeprom.c | 4 +- src/motion_manager.c | 5 +- src/ram.c | 147 +---------- src/stairs.c | 503 +++++++++++++++++++------------------ 9 files changed, 312 insertions(+), 421 deletions(-) diff --git a/Makefile b/Makefile index c31048f..6e95e7d 100755 --- a/Makefile +++ b/Makefile @@ -29,6 +29,7 @@ TARGET_FILES+=src/joint_motion.c TARGET_FILES+=src/head_tracking.c TARGET_FILES+=src/grippers.c TARGET_FILES+=src/smart_charger.c +TARGET_FILES+=src/stairs.c TARGET_PROCESSOR=STM32F103RE diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 4a4684d..f12214f 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -95,15 +95,16 @@ extern "C" { #define STAIRS_PHASE7_TIME ((unsigned short int)0x0072) #define STAIRS_PHASE8_TIME ((unsigned short int)0x0074) #define STAIRS_PHASE9_TIME ((unsigned short int)0x0076) -#define STAIRS_X_OFFSET ((unsigned short int)0x0078) -#define STAIRS_Y_OFFSET ((unsigned short int)0x0079) -#define STAIRS_Z_OFFSET ((unsigned short int)0x007A) -#define STAIRS_Y_SHIFT ((unsigned short int)0x007B) -#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007C) -#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007D) -#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x007E) -#define STAIRS_Z_HEIGHT ((unsigned short int)0x007F) -#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0080) +#define STAIRS_PHASE10_TIME ((unsigned short int)0x0078) +#define STAIRS_X_OFFSET ((unsigned short int)0x007A) +#define STAIRS_Y_OFFSET ((unsigned short int)0x007B) +#define STAIRS_Z_OFFSET ((unsigned short int)0x007C) +#define STAIRS_Y_SHIFT ((unsigned short int)0x007D) +#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007E) +#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007F) +#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x0080) +#define STAIRS_Z_HEIGHT ((unsigned short int)0x0081) +#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0082) #define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) @@ -228,6 +229,8 @@ typedef enum { DARWIN_STAIRS_PHASE8_TIME_H = STAIRS_PHASE8_TIME+1, DARWIN_STAIRS_PHASE9_TIME_L = STAIRS_PHASE9_TIME, DARWIN_STAIRS_PHASE9_TIME_H = STAIRS_PHASE9_TIME+1, + DARWIN_STAIRS_PHASE10_TIME_L = STAIRS_PHASE10_TIME, + DARWIN_STAIRS_PHASE10_TIME_H = STAIRS_PHASE10_TIME+1, DARWIN_STAIRS_X_OFFSET = STAIRS_X_OFFSET, DARWIN_STAIRS_Y_OFFSET = STAIRS_Y_OFFSET, DARWIN_STAIRS_Z_OFFSET = STAIRS_Z_OFFSET, @@ -236,7 +239,8 @@ typedef enum { DARWIN_STAIRS_X_RIGHT_SHIFT = STAIRS_X_RIGHT_SHIFT, DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT, DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT, - DARWIN_STAIRS_HIP_PITCH_OFF = STAIRS_HIP_PITCH_OFF, + DARWIN_STAIRS_HIP_PITCH_OFF_L = STAIRS_HIP_PITCH_OFF, + DARWIN_STAIRS_HIP_PITCH_OFF_H = STAIRS_HIP_PITCH_OFF+1, //RAM DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 @@ -626,10 +630,10 @@ typedef enum { DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x024F, DARWIN_SMART_CHARGER_CNTRL = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // | detected | enable - DARWIN_GRIPPER_CNTRL = 0x0251 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + DARWIN_GRIPPER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // left opened | right opened | left moving | right moving | close left | open left | close right | open right - DARWIN_STAIRS_CNTRL = 0x0252, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // current phase | climbing | | | stop stairs | start stairs + DARWIN_STAIRS_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // current phase | climbing | | stop stairs | start stairs }darwin_registers; #define GPIO_BASE_ADDRESS 0x0100 @@ -734,7 +738,14 @@ typedef enum { #define SMART_CHARGER_DET 0x01 #define SMART_CHARGER_EN 0x02 - +#define STAIRS_BASE_ADDRESS 0x0252 +#define STAIRS_MEM_LENGTH 1 +#define STAIRS_EEPROM_ADDRESS 0x0066 +#define STAIRS_EEPROM_LENGTH 30 +#define STAIRS_START 0x01 +#define STAIRS_STOP 0x02 +#define STAIRS_STATUS 0x08 +#define STAIRS_PHASE 0xF0 #ifdef __cplusplus } diff --git a/include/eeprom.h b/include/eeprom.h index c200358..c9c2f12 100755 --- a/include/eeprom.h +++ b/include/eeprom.h @@ -83,7 +83,7 @@ extern "C" { #define PAGE_FULL ((uint8_t)0x80) /* Variables' number */ -#define NB_OF_VAR ((uint8_t)0x60) +#define NB_OF_VAR ((uint8_t)0x83) /* Exported types ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/include/eeprom_init.h b/include/eeprom_init.h index 570db02..db1405a 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -84,25 +84,28 @@ extern "C" { #define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7 #define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x0080 // 1023 max force in binary format #define DEFAULT_SMART_CHARGER_PERIOD 0x05DC // 1500 ms -#define DEFAULT_STAIRS_PHASE1_TIME 0x0064 // 100 ms -#define DEFAULT_STAIRS_PHASE2_TIME 0x00C8 // 200 ms -#define DEFAULT_STAIRS_PHASE3_TIME 0x012C // 300 ms -#define DEFAULT_STAIRS_PHASE4_TIME 0x0190 // 400 ms -#define DEFAULT_STAIRS_PHASE5_TIME 0x01F4 // 500 ms -#define DEFAULT_STAIRS_PHASE6_TIME 0x0258 // 600 ms -#define DEFAULT_STAIRS_PHASE7_TIME 0x02BC // 700 ms -#define DEFAULT_STAIRS_PHASE8_TIME 0x0320 // 800 ms -#define DEFAULT_STAIRS_PHASE9_TIME 0x0384 // 900 ms +#define DEFAULT_STAIRS_PHASE1_TIME 0x0640 // 100 ms +#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 200 ms +#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 300 ms +#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 400 ms +#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 500 ms +#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 600 ms +#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 700 ms +#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 800 ms +#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 900 ms +#define DEFAULT_STAIRS_PHASE10_TIME 0x3E80 // 1000 ms #define DEFAULT_STAIRS_X_OFFSET 0xFFF6 // -10 mm #define DEFAULT_STAIRS_Y_OFFSET 0x0005 // 5mm #define DEFAULT_STAIRS_Z_OFFSET 0x0014 // 20 mm -#define DEFAULT_STAIRS_Y_SHIFT 0x0014 // 20 mm -#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x000A // 10 mm -#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0032 // 50 mm -#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000A // 10 mm +#define DEFAULT_STAIRS_Y_SHIFT 0x0028 // 40 mm +#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x0000 // 0 mm +#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0050 // 80 mm +#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000F // 15 mm #define DEFAULT_STAIRS_Z_HEIGHT 0x001E // 30 mm #define DEFAULT_STAIRS_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10 +#define EEPROM_SIZE 131 + #ifdef __cplusplus } #endif diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c index dd7f12f..b287d06 100755 --- a/src/darwin_dyn_slave.c +++ b/src/darwin_dyn_slave.c @@ -13,6 +13,7 @@ #include "head_tracking.h" #include "grippers.h" #include "smart_charger.h" +#include "stairs.h" /* timer for the execution of the dynamixel slave loop */ #define DYN_SLAVE_TIMER TIM7 @@ -96,6 +97,8 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng // smart charger commands if(smart_charger_in_range(address,length)) smart_charger_process_write_cmd(address,length,data); + if(stairs_in_range(address,length)) + stairs_process_write_cmd(address,length,data); // write eeprom for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++) EE_WriteVariable(i,data[j]); diff --git a/src/eeprom.c b/src/eeprom.c index b6eae9e..142ad7a 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -154,7 +154,7 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF, GRIPPER_RIGHT_MIN_ANGLE, DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8, GRIPPER_RIGHT_MIN_ANGLE+1, DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE, - DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1, + DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1, DEFAULT_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD, DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1, DEFAULT_STAIRS_PHASE1_TIME&0xFF, STAIRS_PHASE1_TIME, @@ -175,6 +175,8 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, DEFAULT_STAIRS_PHASE8_TIME>>8, STAIRS_PHASE8_TIME+1, DEFAULT_STAIRS_PHASE9_TIME&0xFF, STAIRS_PHASE9_TIME, DEFAULT_STAIRS_PHASE9_TIME>>8, STAIRS_PHASE9_TIME+1, + DEFAULT_STAIRS_PHASE10_TIME&0xFF, STAIRS_PHASE10_TIME, + DEFAULT_STAIRS_PHASE10_TIME>>8, STAIRS_PHASE10_TIME+1, DEFAULT_STAIRS_X_OFFSET, STAIRS_X_OFFSET, DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET, DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_OFFSET, diff --git a/src/motion_manager.c b/src/motion_manager.c index 709777c..2e72f34 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -11,7 +11,7 @@ #include "imu.h" #include "smart_charger.h" #include "dyn_battery.h" -#include "gpio.h" +#include "stairs.h" #include <stdlib.h> #define MANAGER_TIMER TIM5 @@ -198,6 +198,8 @@ void MANAGER_TIMER_IRQHandler(void) walking_process(); // call the gripper process grippers_process(); + // call the stairs process + stairs_process(); // balance the robot manager_balance(); // access to smart charger @@ -469,6 +471,7 @@ void manager_init(uint16_t period_us) head_tracking_init(period_us); grippers_init(period_us); smart_charger_init(period_us); + stairs_init(period_us); } uint16_t manager_get_period(void) diff --git a/src/ram.c b/src/ram.c index e708a46..be8aa9d 100755 --- a/src/ram.c +++ b/src/ram.c @@ -10,152 +10,11 @@ void ram_init(void) for(i=0;i<RAM_SIZE;i++) ram_data[i]=0x00; // read contents from EEPROM to RAM - if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0) - ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0) - ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0) - ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0) - ram_data[DEVICE_ID_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0) - ram_data[BAUDRATE_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0) - ram_data[RETURN_DELAY_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0) - ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0) - ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0) - ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0) - ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0) - ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0) - ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0) - ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0) - ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0) - ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0) - ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0) - ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - for(i=0;i<32;i++) + for(i=0;i<NB_OF_VAR;i++) { - if(EE_ReadVariable(MM_SERVO0_OFFSET+i,&eeprom_data)==0) - ram_data[DARWIN_MM_SERVO0_OFFSET+i]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(i,&eeprom_data)==0) + ram_data[i]=(uint8_t)(eeprom_data&0x00FF); } - if(EE_ReadVariable(WALK_X_OFFSET,&eeprom_data)==0) - ram_data[WALK_X_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_Y_OFFSET,&eeprom_data)==0) - ram_data[WALK_Y_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_Z_OFFSET,&eeprom_data)==0) - ram_data[WALK_Z_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_ROLL_OFFSET,&eeprom_data)==0) - ram_data[WALK_ROLL_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_PITCH_OFFSET,&eeprom_data)==0) - ram_data[WALK_PITCH_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_YAW_OFFSET,&eeprom_data)==0) - ram_data[WALK_YAW_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_HIP_PITCH_OFF,&eeprom_data)==0) - ram_data[WALK_HIP_PITCH_OFF]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_HIP_PITCH_OFF+1,&eeprom_data)==0) - ram_data[WALK_HIP_PITCH_OFF+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_PERIOD_TIME,&eeprom_data)==0) - ram_data[WALK_PERIOD_TIME]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_PERIOD_TIME+1,&eeprom_data)==0) - ram_data[WALK_PERIOD_TIME+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_DSP_RATIO,&eeprom_data)==0) - ram_data[WALK_DSP_RATIO]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_STEP_FW_BW_RATIO,&eeprom_data)==0) - ram_data[WALK_STEP_FW_BW_RATIO]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_FOOT_HEIGHT,&eeprom_data)==0) - ram_data[WALK_FOOT_HEIGHT]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_SWING_RIGHT_LEFT,&eeprom_data)==0) - ram_data[WALK_SWING_RIGHT_LEFT]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_SWING_TOP_DOWN,&eeprom_data)==0) - ram_data[WALK_SWING_TOP_DOWN]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_PELVIS_OFFSET,&eeprom_data)==0) - ram_data[WALK_PELVIS_OFFSET]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_ARM_SWING_GAIN,&eeprom_data)==0) - ram_data[WALK_ARM_SWING_GAIN]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_MAX_VEL,&eeprom_data)==0) - ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0) - ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_P,&eeprom_data)==0) - ram_data[HEAD_PAN_P]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_P+1,&eeprom_data)==0) - ram_data[HEAD_PAN_P+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_I,&eeprom_data)==0) - ram_data[HEAD_PAN_I]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_I+1,&eeprom_data)==0) - ram_data[HEAD_PAN_I+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_D,&eeprom_data)==0) - ram_data[HEAD_PAN_D]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_D+1,&eeprom_data)==0) - ram_data[HEAD_PAN_D+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_I_CLAMP,&eeprom_data)==0) - ram_data[HEAD_PAN_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_PAN_I_CLAMP+1,&eeprom_data)==0) - ram_data[HEAD_PAN_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_P,&eeprom_data)==0) - ram_data[HEAD_TILT_P]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_P+1,&eeprom_data)==0) - ram_data[HEAD_TILT_P+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_I,&eeprom_data)==0) - ram_data[HEAD_TILT_I]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_I+1,&eeprom_data)==0) - ram_data[HEAD_TILT_I+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_D,&eeprom_data)==0) - ram_data[HEAD_TILT_D]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_D+1,&eeprom_data)==0) - ram_data[HEAD_TILT_D+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_I_CLAMP,&eeprom_data)==0) - ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0) - ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_TOP_ID,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_BOT_ID,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE+1,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0) - ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_TOP_ID,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_BOT_ID,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_BOT_ID]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE+1,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE+1,&eeprom_data)==0) - ram_data[GRIPPER_RIGHT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(SMART_CHARGER_PERIOD,&eeprom_data)==0) - ram_data[SMART_CHARGER_PERIOD]=(uint8_t)(eeprom_data&0x00FF); - if(EE_ReadVariable(SMART_CHARGER_PERIOD+1,&eeprom_data)==0) - ram_data[SMART_CHARGER_PERIOD+1]=(uint8_t)(eeprom_data&0x00FF); - } inline void ram_read_byte(uint16_t address,uint8_t *data) diff --git a/src/stairs.c b/src/stairs.c index dfa470a..5cefc8e 100755 --- a/src/stairs.c +++ b/src/stairs.c @@ -2,39 +2,40 @@ #include "darwin_kinematics.h" #include "darwin_math.h" #include "ram.h" +#include <stdio.h> #include <math.h> -typedef enum {SHIFT_WEIGHT_LEFT,RISE_RIGHT_FOOT,ADVANCE_RIGHT_FOOT,CONTACT_RIGHT_FOOT,SHIFT_WEIGHT_RIGHT,RISE_LEFT_FOOT,ADVANCE_LEFT_FOOT,CONTACT_LEFT_FOOT,CENTER_WEIGHT} stairs_phase_t; +typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90,STAND_UP=0xA0} stairs_phase_t; // internal motion variables -float m_Z_stair_height; -float m_Z_overshoot; -float m_Y_swap_amplitude; -float m_X_left_swap_amplitude; -float m_Y_right_swap_amplitude; +float stairs_Z_stair_height; +float stairs_Z_overshoot; +float stairs_Y_shift_amplitude; +float stairs_X_left_shift_amplitude; +float stairs_X_right_shift_amplitude; // internal offset variables -float m_Hip_Pitch_Offset; -float m_X_Offset; -float m_Y_Offset; -float m_Z_Offset; +float stairs_Hip_Pitch_Offset; +float stairs_X_Offset; +float stairs_Y_Offset; +float stairs_Z_Offset; // internal time variables -float m_Time; -float m_stairs_period; -float m_shift_weight_left_time; -float m_rise_right_foot_time; -float advance_right_foot_time; -float contact_right_foot_time; -float shift_weight_right_time; -float rise_left_foot_time; -float advance_left_foot_time; -float contact_left_foot_time; -float center_weight_time; -float total_time; +float stairs_Time; +float stairs_period; +float stairs_shift_weight_left_time; +float stairs_rise_right_foot_time; +float stairs_advance_right_foot_time; +float stairs_contact_right_foot_time; +float stairs_shift_weight_right_time; +float stairs_rise_left_foot_time; +float stairs_advance_left_foot_time; +float stairs_contact_left_foot_time; +float stairs_center_weight_time; +float stairs_walk_ready_time; // control variables -uint8_t m_Ctrl_Running; +uint8_t stairs_Ctrl_Running; // private functions @@ -45,11 +46,9 @@ void stairs_init(uint16_t period_us) // initialize internal control variables - m_Time=0; + stairs_Time=0; stairs_period=((float)period_us)/1000.0; - m_Ctrl_Running=0x00; - - stairs_process(); + stairs_Ctrl_Running=0x00; } inline void stairs_set_period(uint16_t period_us) @@ -64,18 +63,39 @@ inline uint16_t stairs_get_period(void) void stairs_start(void) { - ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS; - m_Ctrl_Running=0x01; + // load all parameters + stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8)); + stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8)); + stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8)); + stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8)); + stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8)); + stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8)); + stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8)); + stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8)); + stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8)); + stairs_walk_ready_time=((float)ram_data[DARWIN_STAIRS_PHASE10_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE10_TIME_H]<<8)); + stairs_Z_stair_height=((float)ram_data[DARWIN_STAIRS_Z_HEIGHT]); + stairs_Z_overshoot=((float)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]); + stairs_Y_shift_amplitude=((float)ram_data[DARWIN_STAIRS_Y_SHIFT]); + stairs_X_left_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_LEFT_SHIFT]); + stairs_X_right_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_RIGHT_SHIFT]); + stairs_Hip_Pitch_Offset=((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0; + stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET])); + stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET])); + stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET])); + // start operation + ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS; + stairs_Ctrl_Running=0x01; } void stairs_stop(void) { - m_Ctrl_Running=0x00; + stairs_Ctrl_Running=0x00; } uint8_t is_climbing_stairs(void) { - if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS) + if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) return 0x01; else return 0x00; @@ -84,225 +104,221 @@ uint8_t is_climbing_stairs(void) // motion manager interface functions void stairs_process(void) { - float x_swap,y_swap,z_swap,a_swap,b_swap,c_swap; - float x_move_r,y_move_r,z_move_r,a_move_r,b_move_r,c_move_r; - float x_move_l,y_move_l,z_move_l,a_move_l,b_move_l,c_move_l; - float pelvis_offset_r,pelvis_offset_l; -// float m_Body_Swing_Y,m_Body_Swing_Z; - float angle[14],ep[12]; + float angle[14]={0},ep[12]={0}; - if(m_Time==0) + if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) { - update_param_time(); - ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE); - ram_data[DARWIN_WALK_CNTRL]|=PHASE0; - if(m_Ctrl_Running==0x00) + ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE); + if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) { - if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0) - ram_data[DARWIN_WALK_CNTRL]&=(~WALK_STATUS); - else - { - ram_data[DARWIN_WALK_STEP_FW_BW]=0.0; - ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0; - ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0; - } + //1 + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time; + ep[2]=stairs_Z_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time; + ep[8]=stairs_Z_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; } - } - else if(m_Time>=(m_Phase_Time1-stairs_period/2.0) && m_Time<(m_Phase_Time1+stairs_period/2.0)) - { - update_param_move(); - ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE); - ram_data[DARWIN_WALK_CNTRL]|=PHASE1; - } - else if(m_Time>=(m_Phase_Time2-stairs_period/2.0) && m_Time<(m_Phase_Time2+stairs_period/2.0)) - { - update_param_time(); - m_Time=m_Phase_Time2; - ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE); - ram_data[DARWIN_WALK_CNTRL]|=PHASE2; - if(m_Ctrl_Running==0x00) + else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) { - if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0) - ram_data[DARWIN_WALK_CNTRL]&=~WALK_STATUS; - else - { - ram_data[DARWIN_WALK_STEP_FW_BW]=0.0; - ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0; - ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0; - } + //2 + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; + ep[2]=stairs_Z_Offset; + ep[3]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time); + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; + ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time); + ep[9]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time); + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; } - } - else if(m_Time>=(m_Phase_Time3-stairs_period/2.0) && m_Time<(m_Phase_Time3+stairs_period/2.0)) - { - update_param_move(); - ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE); - ram_data[DARWIN_WALK_CNTRL]|=PHASE3; - } - update_param_balance(); - - // Compute endpoints - x_swap=wsin(m_Time,m_X_Swap_PeriodTime,m_X_Swap_Phase_Shift,m_X_Swap_Amplitude,m_X_Swap_Amplitude_Shift); - y_swap=wsin(m_Time,m_Y_Swap_PeriodTime,m_Y_Swap_Phase_Shift,m_Y_Swap_Amplitude,m_Y_Swap_Amplitude_Shift); - z_swap=wsin(m_Time,m_Z_Swap_PeriodTime,m_Z_Swap_Phase_Shift,m_Z_Swap_Amplitude,m_Z_Swap_Amplitude_Shift); - a_swap=0; - b_swap=0; - c_swap=0; - if(m_Time<=m_SSP_Time_Start_L) - { - x_move_l=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift); - y_move_l=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift); - z_move_l=wsin(m_SSP_Time_Start_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_l=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift); - x_move_r=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift); - y_move_r=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift); - z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_r=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift); - pelvis_offset_l=0; - pelvis_offset_r=0; - } - else if(m_Time<=m_SSP_Time_End_L) - { - x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift); - y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift); - z_move_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift); - x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift); - y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift); - z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift); - pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Pelvis_Swing/2.0,m_Pelvis_Swing/2.0); - pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,-m_Pelvis_Offset/2.0,-m_Pelvis_Offset/2.0); - } - else if(m_Time<=m_SSP_Time_Start_R) - { - x_move_l=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift); - y_move_l=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift); - z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_l=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift); - x_move_r=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift); - y_move_r=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift); - z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_r=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift); - pelvis_offset_l=0.0; - pelvis_offset_r=0.0; - } - else if(m_Time<=m_SSP_Time_End_R) - { - x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift); - y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift); - z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift); - x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift); - y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift); - z_move_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift); - pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Pelvis_Offset/2,m_Pelvis_Offset/2); - pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,-m_Pelvis_Swing/2,-m_Pelvis_Swing/2); - } - else - { - x_move_l=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift); - y_move_l=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift); - z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_l=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift); - x_move_r=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift); - y_move_r=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift); - z_move_r=wsin(m_SSP_Time_End_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift); - c_move_r=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift); - pelvis_offset_l=0.0; - pelvis_offset_r=0.0; - } - a_move_l=0.0; - b_move_l=0.0; - a_move_r=0.0; - b_move_r=0.0; - - ep[0]=x_swap+x_move_r+m_X_Offset; - ep[1]=y_swap+y_move_r-m_Y_Offset / 2.0; - ep[2]=z_swap+z_move_r+m_Z_Offset; - ep[3]=a_swap+a_move_r-m_R_Offset / 2.0; - ep[4]=b_swap+b_move_r+m_P_Offset; - ep[5]=c_swap+c_move_r-m_A_Offset / 2.0; - ep[6]=x_swap+x_move_l+m_X_Offset; - ep[7]=y_swap+y_move_l+m_Y_Offset / 2.0; - ep[8]=z_swap+z_move_l+m_Z_Offset; - ep[9]=a_swap+a_move_l+m_R_Offset / 2.0; - ep[10]=b_swap+b_move_l+m_P_Offset; - ep[11]=c_swap+c_move_l+m_A_Offset / 2.0; - - // Compute body swing -/* if(m_Time<=m_SSP_Time_End_L) - { - m_Body_Swing_Y=-ep[7]; - m_Body_Swing_Z=ep[8]; - } - else - { - m_Body_Swing_Y=-ep[1]; - m_Body_Swing_Z=ep[2]; - } - m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/ + else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) + { + //3 + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time); + ep[2]=stairs_Z_Offset; + ep[3]=-0.05; + ep[4]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time); + ep[5]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time); + ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time); + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time); + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[9]=-0.05; + ep[10]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time); + ep[11]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time); + ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT; + } + else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time) + { + //4 + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0; + ep[2]=stairs_Z_Offset; + ep[3]=-0.05; + ep[4]=0.1; + ep[5]=0.3; + ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude; + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0; + ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot/(stairs_contact_right_foot_time-stairs_advance_right_foot_time)*(stairs_Time-stairs_advance_right_foot_time); + ep[9]=-0.05; + ep[10]=0.1; + ep[11]=0.3; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT; + } + else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time) + { + //5 + ep[0]=stairs_X_Offset-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[2]=stairs_Z_Offset; + ep[3]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[4]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[5]=0.3; + ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; + ep[9]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[10]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time); + ep[11]=0.3; + ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; + } + else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time) + { + //6 + ep[0]=stairs_X_Offset-35.0; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0; + ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time); + ep[3]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time); + ep[4]=-0.1; + ep[5]=0.3; + ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; + ep[9]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time); + ep[10]=-0.1; + ep[11]=0.3; + ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; + } + else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time) + { + //7 + ep[0]=stairs_X_Offset-35.0+35.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0+10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; + ep[3]=0.05; + ep[4]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ep[5]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0-(-35.0+stairs_X_right_shift_amplitude)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0-10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; + ep[9]=0.05; + ep[10]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ep[11]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time); + ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; + } + else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time) + { + //8 + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude; + ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time); + ep[3]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time); + ep[4]=0.0; + ep[5]=0.0; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude; + ep[8]=stairs_Z_Offset+stairs_Z_stair_height; + ep[9]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time); + ep[10]=0.0; + ep[11]=0.0; + ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; + } + else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) + { + //9 + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time); + ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time); + ep[3]=0.0; + ep[4]=0.0; + ep[5]=0.0; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time); + ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time); + ep[9]=0.0; + ep[10]=0.0; + ep[11]=0.0; + ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; + } + else if(stairs_Time>stairs_center_weight_time && stairs_Time<=stairs_walk_ready_time) + { + ep[0]=stairs_X_Offset; + ep[1]=stairs_Y_Offset/2.0; + ep[2]=stairs_Z_Offset; + ep[6]=stairs_X_Offset; + ep[7]=stairs_Y_Offset/2.0; + ep[8]=stairs_Z_Offset; + ram_data[DARWIN_STAIRS_CNTRL]|=STAND_UP; + } + else + { + ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); + stairs_Ctrl_Running=0x00; + } + printf("%x,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE,ep[0],ep[1],ep[2],ep[3],ep[4],ep[5],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11]); + + if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) + stairs_Time += stairs_period; + else + stairs_Time=0; - // Compute arm swing - if(m_X_Move_Amplitude==0) - { - angle[12]=0; // Right - angle[13]=0; // Left - } - else - { - angle[12]=wsin(m_Time,m_PeriodTime,0.0,-m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0); - angle[13]=wsin(m_Time,m_PeriodTime,0.0,m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0); - } + // Compute angles with the inverse kinematics + if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) || + (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1)) + { + printf("Impossible to find inverse kinematics solution\n"); + return;// Do not use angles + } - if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS) - { - m_Time += stairs_period; - if(m_Time >= m_PeriodTime) - m_Time = 0; + // Compute motor value + if(manager_get_module(R_HIP_YAW)==MM_WALKING) + manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0; + if(manager_get_module(R_HIP_ROLL)==MM_WALKING) + manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0; + if(manager_get_module(R_HIP_PITCH)==MM_WALKING) + manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0; + if(manager_get_module(R_KNEE)==MM_WALKING) + manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0; + if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING) + manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0; + if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING) + manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0; + if(manager_get_module(L_HIP_YAW)==MM_WALKING) + manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0; + if(manager_get_module(L_HIP_ROLL)==MM_WALKING) + manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0; + if(manager_get_module(L_HIP_PITCH)==MM_WALKING) + manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0; + if(manager_get_module(L_KNEE)==MM_WALKING) + manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0; + if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING) + manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0; + if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING) + manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0; + if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING) + manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0; + if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING) + manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0; } - - // Compute angles with the inverse kinematics - if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) || - (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1)) - return;// Do not use angles - - // Compute motor value - if(manager_get_module(R_HIP_YAW)==MM_WALKING) - manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0; - if(manager_get_module(R_HIP_ROLL)==MM_WALKING) - manager_current_angles[R_HIP_ROLL]=((-180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0; - if(manager_get_module(R_HIP_PITCH)==MM_WALKING) - manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-m_Hip_Pitch_Offset)*65536.0; - if(manager_get_module(R_KNEE)==MM_WALKING) - manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0; - if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING) - manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0; - if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING) - manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0; - if(manager_get_module(L_HIP_YAW)==MM_WALKING) - manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0; - if(manager_get_module(L_HIP_ROLL)==MM_WALKING) - manager_current_angles[L_HIP_ROLL]=((-180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0; - if(manager_get_module(L_HIP_PITCH)==MM_WALKING) - manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+m_Hip_Pitch_Offset)*65536.0; - if(manager_get_module(L_KNEE)==MM_WALKING) - manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0; - if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING) - manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0; - if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING) - manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0; - if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING) - manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0; - if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING) - manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0; } // operation functions uint8_t stairs_in_range(unsigned short int address, unsigned short int length) { - if(ram_in_window(WALK_BASE_ADDRESS,WALK_MEM_LENGTH,address,length) || - ram_in_window(WALK_EEPROM_ADDRESS,WALK_EEPROM_LENGTH,address,length)) + if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) || + ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_EEPROM_LENGTH,address,length)) return 0x01; else return 0x00; @@ -318,21 +334,14 @@ void stairs_process_write_cmd(unsigned short int address,unsigned short int leng uint16_t i; // walk control - if(ram_in_range(DARWIN_WALK_CNTRL,address,length)) + if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length)) { - if(data[DARWIN_WALK_CNTRL-address]&WALK_START) + if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START) stairs_start(); - if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP) + if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP) stairs_stop(); } - if(ram_in_range(DARWIN_WALK_STEP_FW_BW,address,length)) - ram_data[DARWIN_WALK_STEP_FW_BW]=data[DARWIN_WALK_STEP_FW_BW-address]; - if(ram_in_range(DARWIN_WALK_STEP_LEFT_RIGHT,address,length)) - ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=data[DARWIN_WALK_STEP_LEFT_RIGHT-address]; - if(ram_in_range(DARWIN_WALK_STEP_DIRECTION,address,length)) - ram_data[DARWIN_WALK_STEP_DIRECTION]=data[DARWIN_WALK_STEP_DIRECTION-address]; - // walk parameters - for(i=WALK_EEPROM_ADDRESS;i<=WALK_EEPROM_ADDRESS+WALK_EEPROM_LENGTH;i++) + for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++) if(ram_in_range(i,address,length)) ram_data[i]=data[i-address]; } -- GitLab From 7d7b711d334084db6c9052e69700757c19b50ef3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Sun, 27 Aug 2017 23:21:07 +0200 Subject: [PATCH 10/21] 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 <stdio.h> #include <math.h> -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?= <shernand@iri.upc.edu> Date: Sun, 27 Aug 2017 23:22:31 +0200 Subject: [PATCH 11/21] 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 <stdio.h> #include <math.h> 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?= <shernand@iri.upc.edu> Date: Mon, 28 Aug 2017 00:31:07 +0200 Subject: [PATCH 12/21] 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?= <shernand@iri.upc.edu> Date: Mon, 28 Aug 2017 23:12:06 +0200 Subject: [PATCH 13/21] 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?= <shernand@iri.upc.edu> Date: Tue, 29 Aug 2017 20:58:15 +0200 Subject: [PATCH 14/21] 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 <darwin@darwin.users.iri.prv> Date: Thu, 31 Aug 2017 18:54:48 +0200 Subject: [PATCH 15/21] 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 <darwin@darwin.users.iri.prv> Date: Thu, 31 Aug 2017 20:55:10 +0200 Subject: [PATCH 16/21] 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?= <shernand@iri.upc.edu> Date: Sat, 2 Sep 2017 15:36:14 +0200 Subject: [PATCH 17/21] 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 <darwin@darwin.users.iri.prv> Date: Sat, 2 Sep 2017 15:44:08 +0200 Subject: [PATCH 18/21] 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 <darwin@darwin.users.iri.prv> Date: Sat, 2 Sep 2017 19:16:54 +0200 Subject: [PATCH 19/21] 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 <darwin@darwin.users.iri.prv> Date: Sat, 2 Sep 2017 20:53:32 +0200 Subject: [PATCH 20/21] 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 <shernand@iri.upc.edu> Date: Tue, 5 Sep 2017 01:11:10 +0200 Subject: [PATCH 21/21] 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