adds PWM as parameter at initialization

demiurge-project · Apr 18, 2020 · 78b4d3c · 78b4d3c
1 parent 1f2254f
commit 78b4d3c
Show file tree

Hide file tree

Showing 6 changed files with 95 additions and 40 deletions.
diff --git a/src/plugins/robots/e-puck/control_interface/ci_epuck_ground_leds_actuator.cpp b/src/plugins/robots/e-puck/control_interface/ci_epuck_ground_leds_actuator.cpp
@@ -14,7 +14,26 @@ namespace argos {
    CCI_EPuckGroundLEDsActuator::CCI_EPuckGroundLEDsActuator() {
      m_unLEDSettingsD1ToD8 = 0;
      m_unLEDSettingsD9 = 0;
-     m_unPWMGroundLED = 0;
+     //m_unPWMGroundLED = 0;
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CCI_EPuckGroundLEDsActuator::Init(TConfigurationNode& t_tree) {
+      try {
+         /* get the ground leds' PWM value from configuration file */
+
+           GetNodeAttributeOrDefault(t_tree, "ground_led_pwm", m_unPWMGroundLED, 50);
+         if (m_unPWMGroundLED > 255) {
+            LOGERR << "[WARNING] Ground LEDS' PWM value =" << m_unPWMGroundLED
+                   << ", value incorrect, back to default value data_size=" << 50 << std::endl;
+            m_unPWMGroundLED = 50;
+            LOGERR << "PWM in ci_robot Init" << m_unPWMGroundLED << std::endl;
+         }
+      } catch (CARGoSException& ex) {
+         THROW_ARGOSEXCEPTION_NESTED("Error initializing ground leds actuator", ex);
+      }
    }
 
    /****************************************/
@@ -134,18 +153,18 @@ namespace argos {
 
    }
 
-   /****************************************/
-   /****************************************/
-
-   void CCI_EPuckGroundLEDsActuator::SetPWM(UInt8 un_PWM_Ground_LED){
-      m_unPWMGroundLED = un_PWM_Ground_LED;
-   }
-
-   /****************************************/
-   /****************************************/
-
-   UInt8 CCI_EPuckGroundLEDsActuator::GetPWM(){
-     return m_unPWMGroundLED;
-   }
+   // /****************************************/
+   // /****************************************/
+   //
+   // void CCI_EPuckGroundLEDsActuator::SetPWM(UInt8 un_PWM_Ground_LED){
+   //    m_unPWMGroundLED = un_PWM_Ground_LED;
+   // }
+   //
+   // /****************************************/
+   // /****************************************/
+   //
+   // UInt8 CCI_EPuckGroundLEDsActuator::GetPWM(){
+   //   return m_unPWMGroundLED;
+   // }
 
 }
diff --git a/src/plugins/robots/e-puck/control_interface/ci_epuck_ground_leds_actuator.h b/src/plugins/robots/e-puck/control_interface/ci_epuck_ground_leds_actuator.h
@@ -42,6 +42,8 @@ namespace argos {
 
       virtual ~CCI_EPuckGroundLEDsActuator() {}
 
+      virtual void Init(TConfigurationNode& t_tree);
+
       /**
        * Switch a single LED.
        * @param un_led_number The number of the LED [1-9]
@@ -65,9 +67,9 @@ namespace argos {
 
       UInt8 GetD9();
 
-      virtual void SetPWM(UInt8 un_PWM_Ground_LED);
+      //virtual void SetPWM(UInt8 un_PWM_Ground_LED);
 
-      UInt8 GetPWM();
+      //UInt8 GetPWM();
 
 
 

diff --git a/src/plugins/robots/e-puck/real_robot/real_epuck_ground_leds_actuator.cpp b/src/plugins/robots/e-puck/real_robot/real_epuck_ground_leds_actuator.cpp
@@ -31,15 +31,15 @@ enum GLEDsRegisters {
   REG_RESET = 61   // Write 255 to reset the LED driver
 };
 
-enum DriverParameters{
-  DRIVER_PWM = 220 // @param DRIVER_PWM Can be modified to set the brightness [0 - 255]
-};
+// enum DriverParameters{
+//   DRIVER_PWM = 220 // @param DRIVER_PWM Can be modified to set the brightness [0 - 255]
+// };
 
 
 CRealEPuckGroundLEDsActuator::CRealEPuckGroundLEDsActuator() {
 
     m_tDeviceStream = OpenDevice(0x32);
-    Init_LED_Driver();
+    //Init_LED_Driver();
 
 }
 
@@ -54,27 +54,58 @@ CRealEPuckGroundLEDsActuator::~CRealEPuckGroundLEDsActuator() {
 
 }
 
+/****************************************/
+/****************************************/
+
+void CRealEPuckGroundLEDsActuator::Init(TConfigurationNode& t_node) {
+    CCI_EPuckGroundLEDsActuator::Init(t_node);
+
+    try {
+        /*
+          * Parse XML
+          */
+        UInt8 m_unPWMGroundLED = 50;
+        /*To understand */
+        GetNodeAttributeOrDefault(t_node, "ground_led_pwm", m_unPWMGroundLED, m_unPWMGroundLED);
+        /*
+          * Initialize led driver
+          */
+
+        Init_LED_Driver();
+
+
+    } catch (CARGoSException& ex) {
+        THROW_ARGOSEXCEPTION_NESTED("Error initializing ground leds actuator", ex);
+
+    }
+
+}
+
 /****************************************/
 /****************************************/
  void CRealEPuckGroundLEDsActuator::Init_LED_Driver(){
 
    WriteRegister(m_tDeviceStream,REG_CNTRL1,64); // initialize
    WriteRegister(m_tDeviceStream,REG_MISC,83); // misc reg
-/***
-* Disable all outputs of the Driver
-***/
+   /***
+   * Disable all outputs of the Driver
+   ***/
    WriteRegister(m_tDeviceStream,REG_D9_OUTPUT_CONTROL,0);
    WriteRegister(m_tDeviceStream,REG_D1TOD8_OUTPUT_CONTROL,0);
-/***
-* Set PWM of each LED
-***/
-  InitPWM(DRIVER_PWM);
+   /***
+   * Set PWM of each LED
+   ***/
+  InitPWM(m_unPWMGroundLED);
+  //LOG << "PWM in Init_LED_Driver" << m_unPWMGroundLED << std::endl;
  }
 
  /****************************************/
  /****************************************/
 
  void CRealEPuckGroundLEDsActuator::InitPWM(UInt8 un_PWM_Ground_LED) {
+
+//LOG << "PWM in InitPWM: " << un_PWM_Ground_LED << std::endl;
+
    WriteRegister(m_tDeviceStream,REG_D1_PWM,un_PWM_Ground_LED);
    WriteRegister(m_tDeviceStream,REG_D2_PWM,un_PWM_Ground_LED);
    WriteRegister(m_tDeviceStream,REG_D3_PWM,un_PWM_Ground_LED);
@@ -97,9 +128,9 @@ void CRealEPuckGroundLEDsActuator::SendData() {
 /****************************************/
 /****************************************/
 
-void CRealEPuckGroundLEDsActuator::SetPWM(UInt8 un_PWM_Ground_LED){
-  CCI_EPuckGroundLEDsActuator::SetPWM(un_PWM_Ground_LED);
-  InitPWM(un_PWM_Ground_LED);
-}
+// void CRealEPuckGroundLEDsActuator::SetPWM(UInt8 un_PWM_Ground_LED){
+//   CCI_EPuckGroundLEDsActuator::SetPWM(un_PWM_Ground_LED);
+//   InitPWM(un_PWM_Ground_LED);
+// }
 
 }
diff --git a/src/plugins/robots/e-puck/real_robot/real_epuck_ground_leds_actuator.h b/src/plugins/robots/e-puck/real_robot/real_epuck_ground_leds_actuator.h
@@ -24,11 +24,14 @@ namespace argos {
       CRealEPuckGroundLEDsActuator();
       virtual ~CRealEPuckGroundLEDsActuator();
 
+      virtual void Init(TConfigurationNode& t_node);
+
       virtual void Init_LED_Driver();
+
       void InitPWM(UInt8 DRIVER_PWM);
 
       virtual void SendData();
-      virtual void SetPWM(UInt8 un_PWM_Ground_LED);
+      // virtual void SetPWM(UInt8 un_PWM_Ground_LED);
 
    private:
 

diff --git a/src/testing/watcher.cpp b/src/testing/watcher.cpp
@@ -29,7 +29,7 @@ CWatcherController::CWatcherController() :
     m_sRightWheelSpeed(0),
     m_nControlStep(0),
     m_unNumberOfGroundLeds(0),          // no. of leds to switch on
-    m_unPWMGroundLED(0),              // PWM ground LEDs
+    // m_unPWMGroundLED(0),              // PWM ground LEDs
     m_pcWheelsActuator(NULL),
     m_pcLEDsActuator(NULL),
     m_pcRGBLEDsActuator(NULL),
@@ -69,7 +69,7 @@ void CWatcherController::ParseParams(TConfigurationNode& t_node) {
         GetNodeAttributeOrDefault(t_node, "distance", m_nDistance, m_nDistance);
         GetNodeAttributeOrDefault(t_node, "run", m_nRun, m_nRun);
         GetNodeAttributeOrDefault(t_node, "no_of_leds", m_unNumberOfGroundLeds, m_unNumberOfGroundLeds); //ground LEDs
-        GetNodeAttributeOrDefault(t_node, "pwm", m_unPWMGroundLED, m_unPWMGroundLED);
+        // GetNodeAttributeOrDefault(t_node, "pwm", m_unPWMGroundLED, m_unPWMGroundLED);
 
 
         if (filename != "") {
@@ -109,11 +109,11 @@ void CWatcherController::Init(TConfigurationNode& t_node) {
     try {
         m_pcGroundLEDsActuator = GetActuator<CCI_EPuckGroundLEDsActuator>("epuck_ground_leds");
     } catch (CARGoSException ex) {}
-    if (m_pcGroundLEDsActuator != NULL) {
-        /* Set PWM of ground LEDs */
-        m_pcGroundLEDsActuator->SetPWM(m_unPWMGroundLED);
-        //LOG << "PWM " << m_unPWMGroundLED << std::endl;
-    }
+    // if (m_pcGroundLEDsActuator != NULL) {
+    //     /* Set PWM of ground LEDs */
+    //     m_pcGroundLEDsActuator->SetPWM(m_unPWMGroundLED);
+    //     //LOG << "PWM " << m_unPWMGroundLED << std::endl;
+    // }
     /* sensors */
     try {
         m_pcProximitySensor = GetSensor<CCI_EPuckProximitySensor>("epuck_proximity");

diff --git a/src/testing/watcher.h b/src/testing/watcher.h
@@ -49,7 +49,7 @@ namespace argos {
       UInt32 m_nControlStep;
 
       size_t m_unNumberOfGroundLeds;
-      UInt8  m_unPWMGroundLED;
+      //UInt8  m_unPWMGroundLED;
 
       CCI_EPuckWheelsActuator* m_pcWheelsActuator;
       CCI_EPuckBaseLEDsActuator* m_pcLEDsActuator;