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 index 8fd4e48..0716725 100644 --- 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 index 78e9e91..ea2cdcb 100644 --- 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 index 0b95c53..9a0ee74 100644 --- 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 index 90b5cfa..b39d4e9 100644 --- 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 index d625bb7..b35b369 100644 --- 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("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("epuck_proximity"); diff --git a/src/testing/watcher.h b/src/testing/watcher.h index 798595a..6020f91 100644 --- 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;