From 6ea7938c9bb1c96e3bbd0829ac9cc8b76d096495 Mon Sep 17 00:00:00 2001
From: Irene Garcia <igarcia@iri.upc.edu>
Date: Wed, 22 Feb 2017 11:06:57 +0100
Subject: [PATCH] Added to the darwin firmware the smart charger module
Added to the motion manager a bloc to detect, in any of the Dynamixel buses, devices other than servos such as the smart charger device.
Implemented a initialization bloc for smart charger's module.
Added the motion manager interface function of the smart charger into the manager's timer.
---
Makefile | 7 ++
include/motion_manager.h | 1 +
src/motion_manager.c | 148 +++++++++++++++++++++++++++++++++------
3 files changed, 134 insertions(+), 22 deletions(-)
diff --git a/Makefile b/Makefile
index 67bc17e..14e6134 100755
--- a/Makefile
+++ b/Makefile
@@ -123,6 +123,13 @@ $(MAIN_OUT_LSS): $(MAIN_OUT_ELF)
$(BUID_PATH)/$(STARTUP_FILE:.s=.o): $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE)
$(AS) $(ASFLAGS) -o $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE) > $(BUILD_PATH)/$(STARTUP_FILE:.s=.lst)
+
+doc:
+ doxygen $(DOC_DIR)/doxygen.conf
+
clean:
-rm -rf $(BUILD_PATH)
+ #-rm -rf doc/html
+
+#.PHONY: all clean doc
diff --git a/include/motion_manager.h b/include/motion_manager.h
index a764496..d4ef7cc 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -66,6 +66,7 @@ typedef struct{
TDynVersion dyn_version;
}TServoInfo;
+
#define MANAGER_MAX_NUM_SERVOS 31
// public variables
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 6a5e3f1..1633cda 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -9,6 +9,9 @@
#include "head_tracking.h"
#include "grippers.h"
#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()
@@ -21,7 +24,7 @@ TIM_HandleTypeDef MANAGER_TIM_Handle;
uint16_t manager_motion_period;
uint16_t manager_motion_period_us;
uint8_t manager_num_servos;
-TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
+TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS]; //motion_manager.h
// current angles used for all motion modules
int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
@@ -31,6 +34,7 @@ uint8_t manager_balance_enabled;
// manager offsets
int16_t manager_offset[MANAGER_MAX_NUM_SERVOS];
+
// private functions
void manager_send_motion_command(void)
{
@@ -174,6 +178,8 @@ void MANAGER_TIMER_IRQHandler(void)
grippers_process();
// balance the robot
manager_balance();
+ // access to smart charger
+ smart_charger_process();
// get the target angles from all modules
manager_get_target_position();
// send the motion commands to the servos
@@ -198,13 +204,13 @@ void manager_init(uint16_t period_us)
darwin_dyn_master_init();
darwin_dyn_master_v2_init();
- ram_data[DARWIN_MM_CNTRL]|=MANAGER_SCANNING;
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_SCANNING; //0X80 -> bit scanning
// enable power to the servos
darwin_dyn_master_enable_power();
HAL_Delay(1000);
// detect the servos connected on the v1 bus
dyn_master_scan(&darwin_dyn_master,&num,servo_ids);
- ram_data[DARWIN_MM_NUM_SERVOS]=num;
+ ram_data[DARWIN_MM_NUM_SERVOS]=num; //tmb sumara los otros dispositivos (cargador)
manager_num_servos=0;
for(i=0;i<MANAGER_MAX_NUM_SERVOS && current<num;i++)
{
@@ -275,26 +281,37 @@ void manager_init(uint16_t period_us)
manager_servos[i].center_angle=180<<7;
manager_servos[i].max_speed=270;
break;
+/* case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]|=SMART_CHARGER_DET; //smart charger detected
+ ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[current];
+ //battery_version.dyn_version = &darwin_dyn_master;
+ smart_charger_set_version(darwin_dyn_master);
+ //hacer un read table?
+ dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,SMART_CHARGER_MEM_LENGTH,(uint8_t *)&ram_data[DARWIN_BATT_CHARGER_STATUS]);
+ gpio_set_led(LED_3);
+ break;*/
default: break;
}
- manager_servos[i].id=servo_ids[current];
- manager_servos[i].model=model;
- manager_servos[i].module=MM_NONE;
- manager_servos[i].enabled=0x00;
- manager_servos[i].dyn_version=DYN_VER1;
- // get the servo's current position
- dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
- manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
- // read the servo limits
- dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
- manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
- dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
- manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
- // set the action current angles
- manager_current_angles[i]=manager_servos[i].current_angle<<9;
- manager_num_servos++;
+ //Config for all servos except battery
+ // if(model!=DYN_BATTERY_MODEL){
+ manager_servos[i].id=servo_ids[current];
+ manager_servos[i].model=model;
+ manager_servos[i].module=MM_NONE;
+ manager_servos[i].enabled=0x00;
+ manager_servos[i].dyn_version=DYN_VER1;
+ // get the servo's current position
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ // read the servo limits
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
+ manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
+ manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
+ // set the action current angles
+ manager_current_angles[i]=manager_servos[i].current_angle<<9;
+ manager_num_servos++;
+ // }
current++;
}
else
@@ -314,8 +331,27 @@ void manager_init(uint16_t period_us)
manager_servos[i].ccw_angle_limit=0;
}
}
+
+ //Other devices detected
+ for(i=current; i<num; i++){
+ dyn_master_read_word(&darwin_dyn_master,servo_ids[i],P_MODEL_NUMBER_L,&model);
+ switch(model)
+ {
+ case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]|=SMART_CHARGER_DET; //smart charger detected
+ //gpio_set_led(LED_TX);
+ // if(ram_data[DARWIN_SMART_CHARGER_CNTRL] ==1)
+ // gpio_set_led(LED_3);
+ ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i];
+ smart_charger_set_version(&darwin_dyn_master);
+ dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]);
+ break;
+ default: break;
+ }
+ }
+
darwin_dyn_master_disable_power();
-
+
+/* ORIGINAL
// 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;
@@ -358,7 +394,74 @@ void manager_init(uint16_t period_us)
current++;
}
}
+*/
+ // 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; //aumentar manager_max_num_servos (31)? la bateria se incluira
+ current=0;
+ for(i=0;i<MANAGER_MAX_NUM_SERVOS && current<num;i++)
+ {
+ if(i==servo_ids[current])
+ {
+ present_servos|=(0x00000001<<i);
+ // read the model of the i-th device
+ dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[current],XL_MODEL_NUMBER_L,&model); //o BATTERY_MODEL_NUMBER_L
+ switch(model)
+ {
+ case SERVO_XL320: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=238;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
+ manager_servos[i].max_speed=354;
+
+ manager_servos[i].id=servo_ids[current];
+ manager_servos[i].model=model;
+ manager_servos[i].module=MM_NONE;
+ manager_servos[i].enabled=0x00;
+ manager_servos[i].dyn_version=DYN_VER2;
+ // get the servo's current position
+ //CAMBIAR i POR current? -NO
+ dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ // read the servo limits
+ dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_CW_ANGLE_LIMIT_L,&value);
+ manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
+ dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_CCW_ANGLE_LIMIT_L,&value);
+ manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
+ // set the action current angles
+ manager_current_angles[i]=manager_servos[i].current_angle<<9;
+ break;
+ /* case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]|=SMART_CHARGER_DET; //smart charger detected
+ ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[current];
+ smart_charger_set_version(darwin_dyn_master_v2);
+ dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,SMART_CHARGER_MEM_LENGTH,(uint8_t *)&ram_data[DARWIN_BATT_CHARGER_STATUS]);
+ break;*/
+ default: break;
+ }
+ //antes habia la inicializacion general
+ manager_num_servos++;
+ current++;
+ }
+ }
+
+ //Other devices detected
+ for(i=current; i<num; i++){
+ dyn_master_read_word(&darwin_dyn_master,servo_ids[i],P_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_set_version(&darwin_dyn_master_v2);
+ 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;
+ }
+ }
+
+
ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF);
ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8);
ram_data[DARWIN_MM_PRESENT_SERVOS3]=((present_servos&0x00FF0000)>>16);
@@ -402,6 +505,7 @@ void manager_init(uint16_t period_us)
joint_motion_init(period_us);
head_tracking_init(period_us);
grippers_init(period_us);
+ smart_charger_init(period_us);
}
uint16_t manager_get_period(void)
--
GitLab