From 61a5f3e54d535c6aabc40d202d05eb7d8a7d2141 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 7 Apr 2020 20:30:08 +0200
Subject: [PATCH] Added a module to interface with the ADC.

---
 include/darwin_adc.h       | 26 ++++++++++++++++
 include/darwin_registers.h | 23 ++++++++++++++
 src/CMakeLists.txt         |  6 ++--
 src/darwin_adc.cpp         | 62 ++++++++++++++++++++++++++++++++++++++
 4 files changed, 114 insertions(+), 3 deletions(-)
 create mode 100644 include/darwin_adc.h
 create mode 100644 src/darwin_adc.cpp

diff --git a/include/darwin_adc.h b/include/darwin_adc.h
new file mode 100644
index 0000000..d50932a
--- /dev/null
+++ b/include/darwin_adc.h
@@ -0,0 +1,26 @@
+#ifndef _DARWIN_ADC_H
+#define _DARWIN_ADC_H
+
+#include "darwin_robot_base.h"
+#include "darwin_registers.h"
+
+#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=11} adc_t;
+
+class CDarwinADC : public CDarwinRobotBase
+{
+  public:
+    CDarwinADC(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
+    // ADC interface
+    void start(void);
+    bool is_running(void);
+    void set_period(unsigned char period_ms);
+    double get_value(adc_t adc);
+    double get_temperature(void);
+    void stop(void);
+    ~CDarwinADC();
+};
+
+#endif
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 689d39a..04fc7ec 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -8,6 +8,9 @@
   #define MANAGER_START                0x01
   #define MANAGER_STOP                 0x02
   #define MANAGER_START_SCAN           0x04
+  #define MANAGER_ENABLE_POWER         0x08
+  #define MANAGER_DISABLE_POWER        0x10
+  #define MANAGER_POWERED              0x20
   #define MANAGER_RUNNING              0x40
   #define MANAGER_SCANNING             0x80
 #define MANAGER_NUM_DEVICES            131
@@ -138,5 +141,25 @@
 #define BALANCE_ANKLE_PITCH_GAIN_OFFSET 11
 #define BALANCE_HIP_ROLL_GAIN_OFFSET    13
 
+#define ADC_PERIOD_OFFSET               15
+#define ADC_CONTROL_OFFSET              403// bit 7 | bit 6 | bit 5 |  bit 4  | bit 3 | bit 2 | bit 1 | bit 0
+                                           //       |       |       | running |       |       | stop  | start 
+
+#define ADC_START                       0x01
+#define ADC_STOP                        0x02
+#define ADC_RUNNING                     0x10
+#define ADC_CH1_VOLTAGE_OFFSET          404
+#define ADC_CH2_VOLTAGE_OFFSET          406
+#define ADC_CH3_VOLTAGE_OFFSET          408
+#define ADC_CH4_VOLTAGE_OFFSET          410
+#define ADC_CH5_VOLTAGE_OFFSET          412
+#define ADC_CH6_VOLTAGE_OFFSET          414
+#define ADC_CH7_VOLTAGE_OFFSET          416
+#define ADC_CH8_VOLTAGE_OFFSET          418
+#define ADC_CH9_VOLTAGE_OFFSET          420
+#define ADC_CH10_VOLTAGE_OFFSET         422
+#define ADC_TEMP_OFFSET                 424
+#define ADC_CH12_VOLTAGE_OFFSET         426
+
 
 #endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c2497d6..36145dd 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -11,9 +11,9 @@ SET(KDL_INSTALL /opt/ros/indigo)
 INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
 
 # driver source files
-SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp darwin_joint_motion.cpp darwin_walk.cpp darwin_head_tracking.cpp)
+SET(robot_sources darwin_robot_base.cpp darwin_robot_exceptions.cpp darwin_imu.cpp darwin_dyn_manager.cpp darwin_mmanager.cpp darwin_action.cpp darwin_balance.cpp darwin_joint_motion.cpp darwin_walk.cpp darwin_head_tracking.cpp darwin_adc.cpp)
 # application header files
-SET(robot_headers ../include/darwin_robot_base.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h ../include/darwin_joint_motion.h ../include/darwin_walk.h ../include/darwin_head_tracking.h)
+SET(robot_headers ../include/darwin_robot_base.h ../include/darwin_robot_exceptions.h ../include/darwin_imu.h ../include/darwin_dyn_manager.h ../include/darwin_mmanager.h ../include/darwin_action.h ../include/darwin_balance.h ../include/darwin_joint_motion.h ../include/darwin_walk.h ../include/darwin_head_tracking.h ../include/darwin_adc.h)
 
 # locate the necessary dependencies
 FIND_PACKAGE(iriutils REQUIRED)
@@ -35,7 +35,7 @@ IF(EIGEN3_FOUND)
   SET(kin_leg_headers ../include/darwin_leg_kinematics.h ../include/darwin_robot_exceptions.h)
 ENDIF(EIGEN3_FOUND)
 
-#ADD_DEFINITIONS(-D_SIM)
+ADD_DEFINITIONS(-D_SIM)
 
 # add the necessary include directories
 INCLUDE_DIRECTORIES(../include)
diff --git a/src/darwin_adc.cpp b/src/darwin_adc.cpp
new file mode 100644
index 0000000..68d9ab6
--- /dev/null
+++ b/src/darwin_adc.cpp
@@ -0,0 +1,62 @@
+#include "darwin_adc.h"
+#include "darwin_robot_exceptions.h"
+
+CDarwinADC::CDarwinADC(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id) : CDarwinRobotBase(name,bus_id,bus_speed,id)
+{
+}
+
+// ADC interface
+
+void CDarwinADC::start(void)
+{
+  this->is_valid();
+  this->robot_device->write_byte_register(ADC_CONTROL_OFFSET,ADC_START);
+}
+
+bool CDarwinADC::is_running(void)
+{
+  unsigned char status;
+
+  this->is_valid();
+  this->robot_device->read_byte_register(ADC_CONTROL_OFFSET,&status);
+  if(status&ADC_RUNNING)
+    return true;
+  else
+    return false;
+}
+
+void CDarwinADC::set_period(unsigned char period_ms)
+{
+  this->is_valid();
+  this->robot_device->write_byte_register(ADC_PERIOD_OFFSET,period_ms);
+}
+double CDarwinADC::get_value(adc_t adc)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  this->robot_device->read_word_register(ADC_CH1_VOLTAGE_OFFSET+((int)adc)*2,&value);
+
+  return ((double)value)/((double)(1<<12));
+}
+
+double CDarwinADC::get_temperature(void)
+{
+  unsigned short int value;
+
+  this->is_valid();
+  this->robot_device->read_word_register(ADC_TEMP_OFFSET,&value);
+
+  return ((double)value)/((double)(1<<10));
+}
+
+void CDarwinADC::stop(void)
+{
+  this->is_valid();
+  this->robot_device->write_byte_register(ADC_CONTROL_OFFSET,ADC_STOP);
+}
+
+CDarwinADC::~CDarwinADC()
+{
+}
+
-- 
GitLab