diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 596fdb262010913b41fc1df95d0be9ca320edf10..c648bb56c946816dd4d8656d2c49fcc2b3789e94 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -570,7 +570,8 @@ typedef enum { DARWIN_HEAD_TILT_TARGET_L = 0x0245, // angle in fixed point format 9|7 DARWIN_HEAD_TILT_TARGET_H = 0x0246, - DARWIN_BATT_CHARGER_STATUS = 0x0247, +// 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, @@ -613,16 +614,24 @@ typedef enum { DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F, DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270, DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271, - -//añadidos - DARWIN_SMART_CHARGER_CNTRL = 0x0272, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 - // | detected | enable - DARWIN_SMART_CHARGER_ID = 0x0273, - +*/ +//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 = 0x0274, // 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; + DARWIN_GRIPPER_CNTRL = 0x0252 // 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; #define GPIO_BASE_ADDRESS 0x0100 #define GPIO_MEM_LENGTH 21 @@ -703,7 +712,7 @@ typedef enum { #define HEAD_STOP 0x02 #define HEAD_STATUS 0x10 -#define GRIPPER_BASE_ADDRESS 0x0274 +#define GRIPPER_BASE_ADDRESS 0x0252//0x0274 #define GRIPPER_MEM_LENGTH 1 #define GRIPPER_EEPROM_ADDRESS 0x0054 #define GRIPPER_EEPROM_LENGTH 14 @@ -713,7 +722,7 @@ typedef enum { #define GRIPPER_CLOSE_LEFT 0x08 #define SMART_CHARGER_BASE_ADDRESS 0x0247 -#define SMART_CHARGER_MEM_LENGTH 45 +#define SMART_CHARGER_MEM_LENGTH 10 //45 #define SMART_CHARGER_EEPROM_BASE 0x0060 #define SMART_CHARGER_EEPROM_LENGTH 2 #define SMART_CHARGER_DET 0x01 diff --git a/src/motion_manager.c b/src/motion_manager.c index fc30ca6bc96de64da23f5b59de8155f44867cfd0..4672002a904cf6b4af702a78645883c7176ac278 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -11,7 +11,6 @@ #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() @@ -177,7 +176,7 @@ void MANAGER_TIMER_IRQHandler(void) // call the grippers process grippers_process(); // balance the robot - manager_balance(); + manager_balance(); // access to smart charger smart_charger_process(); // get the target angles from all modules @@ -185,7 +184,7 @@ void MANAGER_TIMER_IRQHandler(void) // send the motion commands to the servos manager_send_motion_command(); // get the disabled servos position - // manager_get_current_position(); + // manager_get_current_position(); } } } @@ -327,12 +326,14 @@ void manager_init(uint16_t period_us) 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 + 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]); - break; + //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; default: break; } } @@ -384,17 +385,19 @@ void manager_init(uint16_t period_us) //Other devices detected - smart charger 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_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]); - break; - default: break; - } + 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]); + break; + default: break; + } } diff --git a/src/smart_charger.c b/src/smart_charger.c index 6570023a5f6fc141eb59ceb1ba0ed12d216e408a..daa8ef803b42c9ce9088255ffb6a6c3bf8645785 100644 --- a/src/smart_charger.c +++ b/src/smart_charger.c @@ -2,6 +2,7 @@ #include "ram.h" #include "dyn_battery.h" #include "smart_charger.h" +#include "gpio.h" // private variables uint8_t smart_charger_id; //smart charger dynamixel ID @@ -91,8 +92,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; + //uint8_t i; + uint16_t period,i; //Enable/Disable smart charger if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length)) @@ -109,21 +110,27 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i smart_charger_set_period(period); } - //Write Battery limit current (EEPROM) - if(ram_in_range(BATTERY_INPUT_MAX_CURRENT_L,address,length)&& ram_in_range(BATTERY_INPUT_MAX_CURRENT_H,address,length)) + //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)) { smart_charger_write = 0x01; if(infifo==&write_fifo[fifosize]); - infifo=&write_fifo[0]; + infifo=&write_fifo[0]; //go to first position of fifo if(numdata<fifosize) //free space in fifo { - for(i=BATTERY_INPUT_MAX_CURRENT_L;i<=BATTERY_INPUT_MAX_CURRENT_H;i++) + 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++; */ } } } @@ -133,13 +140,13 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i void smart_charger_process(void) { counter++; - +uint8_t error; //Write smart_charger - Battery limit current (EEPROM) if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count) { 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]; + outfifo=&write_fifo[0]; //go to first position of fifo else outfifo=outfifo+2; numdata=numdata-2; @@ -151,7 +158,11 @@ void smart_charger_process(void) if(smart_charger_detected && smart_charger_enabled && counter==smart_charger_count) { counter = 0; - dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_BATT_AVG_TIME_EMPTY_L]); + 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_3);//verde + else + gpio_set_led(LED_4); } }