Diff: serial_connected_mcu.cpp

Revision:

15:9a50a5a747ad

Parent:

14:014c339ecaf1

Child:

16:4e310c3f3a2b

--- a/serial_connected_mcu.cpp	Mon Aug 01 05:12:00 2016 +0000
+++ b/serial_connected_mcu.cpp	Mon Aug 01 05:33:09 2016 +0000
@@ -36,6 +36,13 @@
 const size_t serial_connected_mcu::_encoder_num = 3;
 const size_t serial_connected_mcu::_analog_input_num = 3;
 
+#ifdef USE_LIFTER_MODE
+const PinName serial_connected_mcu::_limit_switch_pins[] = {
+    D11,
+    D12
+};
+#endif
+
 serial_connected_mcu* serial_connected_mcu::instance() {
     if (_instance == NULL) {
         _instance = new serial_connected_mcu;
@@ -44,6 +51,12 @@
 }
 
 serial_connected_mcu::serial_connected_mcu() {
+    #ifdef USE_LIFTER_MODE
+    for (size_t i = 0; i < LIMIT_SWITCHES_NUM; ++i) {
+        _limit_switches[i] = new DigitalIn(_limit_switch_pins[i]);
+    }
+    #endif
+    
     _prev_encoders_values = new float[_encoder_num];
     
     for (size_t i = 0; i < SIZE_OF_WRITE_DATA; ++i) {
@@ -104,7 +117,19 @@
     _slave.set(ENCODER3, encoder_values[2]);
     _slave.set(POTENTIONMETER1, (_analog_inputs[0]->read() - 0.5f) * 2.0f * 32767.0f);
     _slave.set(POTENTIONMETER2, (_analog_inputs[1]->read() - 0.5f) * 2.0f * 32767.0f);
-    _slave.set(POTENTIONMETER3, (_analog_inputs[2]->read() - 0.5f) * 2.0f * 32767.0f);
+    
+    float p = 0.0f;
+    #ifdef USE_LIFTER_MODE
+    if (_limit_switches[UPPER_LIMIT]->read() == 1) {
+        p = 1.0f;
+    } else if (_limit_switches[LOWER_LIMIT]->read() == 1) {
+        p = -1.0f;
+    }
+    #else
+    p = (_analog_inputs[2]->read() - 0.5f) * 2.0f * 32767.0f;
+    #endif
+    
+    _slave.set(POTENTIONMETER3, p);
     
     _servos[0]->set_position(_slave.get(ESC1) / 32767.0f);
     _servos[1]->set_position(_slave.get(ESC2) / 32767.0f);