From 2b6ce8d63895bf48a6f5cf497533b1a90f744a58 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Mon, 8 Aug 2016 12:51:24 +0200
Subject: [PATCH] Added a second dynamixel bus using some of the analog ports.
Added the registers for the smart battery module.
---
include/darwin_registers.h | 45 +++++++++++++++++++++++++++++++++++++-
src/motion_manager.c | 8 ++++++-
2 files changed, 51 insertions(+), 2 deletions(-)
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 8815856..b89b263 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -539,7 +539,50 @@ typedef enum {
DARWIN_HEAD_MIN_TILT_L = 0x0243, // angle in fixed point format 9|7
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
+ DARWIN_HEAD_TILT_TARGET_H = 0x0246,
+ 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
} darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 8e2ff6e..227517f 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -1,4 +1,5 @@
#include "darwin_dyn_master.h"
+#include "darwin_dyn_master_v2.h"
#include "motion_manager.h"
#include "dyn_servos.h"
#include "ram.h"
@@ -179,12 +180,13 @@ void manager_init(uint16_t period_us)
/* initialize the dynamixel master module for the servos */
darwin_dyn_master_init();
+ darwin_dyn_master_v2_init();
ram_data[DARWIN_MM_CNTRL]|=MANAGER_SCANNING;
// enable power to the servos
darwin_dyn_master_enable_power();
HAL_Delay(1000);
- // detect the servos connected
+ // detect the servos connected on the v1 bus
dyn_master_scan(&darwin_dyn_master,&num,servo_ids);
ram_data[DARWIN_MM_NUM_SERVOS]=num;
manager_num_servos=0;
@@ -299,6 +301,10 @@ void manager_init(uint16_t period_us)
ram_data[DARWIN_MM_PRESENT_SERVOS3]=((present_servos&0x00FF0000)>>16);
ram_data[DARWIN_MM_PRESENT_SERVOS4]=((present_servos&0xFF000000)>>24);
darwin_dyn_master_disable_power();
+
+ // detect the servos connected on the v2 bus
+ dyn_master_scan(&darwin_dyn_master_v2,&num,servo_ids);
+
ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_SCANNING);
/* configure timer */
--
GitLab