From 1aa1f12651c9d29ff0838131303031a750ff57a9 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 29 Aug 2017 14:49:42 +0200
Subject: [PATCH] Added the necessary chnages to uase the new power enable
feature for the second dynamixel bus.
---
src/darwin_robot.cpp | 52 +++++++++++++++++++++++---------
src/darwin_robot.h | 8 +++--
src/examples/darwin_adc_test.cpp | 2 --
3 files changed, 43 insertions(+), 19 deletions(-)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 5079fd8..fa75ab9 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -277,20 +277,6 @@ double CDarwinRobot::adc_get_temperature(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-double CDarwinRobot::adc_get_vrefint(void)
-{
- unsigned short int value;
-
- if(this->robot_device!=NULL)
- {
- this->robot_device->read_word_register(DARWIN_ADC_VREF_L,&value);
-
- return ((double)value)/((double)(1<<12));
- }
- else
- throw CDarwinRobotException(_HERE_,"Invalid robot device");
-}
-
void CDarwinRobot::adc_stop(void)
{
if(this->robot_device!=NULL)
@@ -590,6 +576,44 @@ bool CDarwinRobot::mm_is_power_enabled(void)
throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
+void CDarwinRobot::mm_enable_power_v2(void)
+{
+ if(this->robot_device!=NULL)
+ {
+ this->manager_status|=MANAGER_EN_PWR_V2;
+ this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+void CDarwinRobot::mm_disable_power_v2(void)
+{
+ if(this->robot_device!=NULL)
+ {
+ this->manager_status&=(~MANAGER_EN_PWR_V2);
+ this->robot_device->write_byte_register(DARWIN_MM_CNTRL,this->manager_status);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
+bool CDarwinRobot::mm_is_power_enabled_v2(void)
+{
+ unsigned char value;
+
+ if(this->robot_device!=NULL)
+ {
+ this->robot_device->read_byte_register(DARWIN_MM_CNTRL,&value);
+ if(value&MANAGER_EN_PWR_V2)
+ return true;
+ else
+ return false;
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
+}
+
bool CDarwinRobot::mm_has_fallen(void)
{
unsigned char value;
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index acda926..b3f6130 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -31,10 +31,10 @@ typedef struct
typedef enum {START_PB=0,MODE_PB=1} pushbutton_t;
-#define ADC_NUM_CHANNELS 16
+#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_t;
+ ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_t;
#define JOINTS_NUM_GROUPS 4
@@ -84,7 +84,6 @@ class CDarwinRobot
void adc_set_period(unsigned char period_ms);
double adc_get_value(adc_t adc);
double adc_get_temperature(void);
- double adc_get_vrefint(void);
void adc_stop(void);
// imu interface
void imu_start(void);
@@ -111,6 +110,9 @@ class CDarwinRobot
void mm_enable_power(void);
void mm_disable_power(void);
bool mm_is_power_enabled(void);
+ void mm_enable_power_v2(void);
+ void mm_disable_power_v2(void);
+ bool mm_is_power_enabled_v2(void);
bool mm_has_fallen(void);
fall_t mm_get_fallen_position(void);
void mm_enable_servo(unsigned char servo_id);
diff --git a/src/examples/darwin_adc_test.cpp b/src/examples/darwin_adc_test.cpp
index adbf5bd..93ff870 100644
--- a/src/examples/darwin_adc_test.cpp
+++ b/src/examples/darwin_adc_test.cpp
@@ -17,7 +17,6 @@ int main(int argc, char *argv[])
for(i=0;i<5000;i++)
{
std::cout << "Temperature: " << darwin.adc_get_temperature() << std::endl;
- std::cout << "Vref: " << darwin.adc_get_vrefint() << std::endl;
std::cout << "Channel1: " << darwin.adc_get_value(ADC_CH1) << std::endl;
std::cout << "Channel2: " << darwin.adc_get_value(ADC_CH2) << std::endl;
std::cout << "Channel3: " << darwin.adc_get_value(ADC_CH3) << std::endl;
@@ -29,7 +28,6 @@ int main(int argc, char *argv[])
std::cout << "Channel9: " << darwin.adc_get_value(ADC_CH9) << std::endl;
std::cout << "Channel10: " << darwin.adc_get_value(ADC_CH10) << std::endl;
std::cout << "Channel12: " << darwin.adc_get_value(ADC_CH12) << std::endl;
- std::cout << "Channel14: " << darwin.adc_get_value(ADC_CH14) << std::endl;
usleep(100000);
}
darwin.adc_stop();
--
GitLab