Diff: serial_connected_mcu.cpp

Revision:

16:4e310c3f3a2b

Parent:

15:9a50a5a747ad

Child:

17:814679e7b0fc

--- a/serial_connected_mcu.cpp	Mon Aug 01 05:33:09 2016 +0000
+++ b/serial_connected_mcu.cpp	Mon Aug 08 05:12:35 2016 +0000
@@ -1,3 +1,4 @@
+
 #include "serial_connected_mcu.hpp"
 #include "serial_connected_mcu_slave.hpp"
 #include "mbed.h"
@@ -5,16 +6,9 @@
 namespace serial_connected_mcu {
     
 serial_connected_mcu* serial_connected_mcu::_instance = NULL;
-/*
-const int16_t _init_write_data[] = {
-    0, // ENCODER1
-    0, // ENCODER2
-    0, // ENCODER3
-    0, // POTENTIONMETER1
-    0, // POTENTIONMETER2
-    0  // POTENTIONMETER3
-};
-*/
+
+const float serial_connected_mcu::_sampling_delta_time_usec = 1000.0f;
+
 const PinName serial_connected_mcu::_servo_pins[] = {
     PB_6,
     PB_7,
@@ -22,8 +16,8 @@
 };
 TIM_TypeDef* serial_connected_mcu::_encoder_timers[] = {
     TIM1,
-    TIM2,
-    TIM3
+    TIM3,
+    TIM2
 };
 const size_t serial_connected_mcu::_encoder_pulse_per_revol = 100;
 
@@ -33,16 +27,8 @@
     A5
 };
 const size_t serial_connected_mcu::_servo_num = 3;
-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;
@@ -51,13 +37,7 @@
 }
 
 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];
+    _prev_encoders_values = new float[ENCODER_NUM];
     
     for (size_t i = 0; i < SIZE_OF_WRITE_DATA; ++i) {
         _slave.set(i, 0);
@@ -68,8 +48,8 @@
         _servos[i] = new servo(_servo_pins[i]);
     }
     
-    _encoders = new rotary_encoder_ab_phase*[_encoder_num];
-    for (size_t i = 0; i < _encoder_num; ++i) {
+    _encoders = new rotary_encoder_ab_phase*[ENCODER_NUM];
+    for (size_t i = 0; i < ENCODER_NUM; ++i) {
         _encoders[i] = new rotary_encoder_ab_phase(_encoder_timers[i], _encoder_pulse_per_revol);
         _encoders[i]->start();
         _prev_encoders_values[i] = 0.0f;
@@ -87,7 +67,7 @@
     for (size_t i = 0; i < _servo_num; ++i) {
         delete _servos[i];
     }
-    for (size_t i = 0; i < _encoder_num; ++i) {
+    for (size_t i = 0; i < ENCODER_NUM; ++i) {
         delete _encoders[i];
     }
     for (size_t i = 0; i < _analog_input_num; ++i) {
@@ -96,40 +76,37 @@
 }
 
 void serial_connected_mcu::update() {
-    float encoder_values[_encoder_num];
-    float encoders_speed[_encoder_num] = {};
+    static bool is_active = false;
+    float encoder_values[ENCODER_NUM];
     
-    for (size_t i = 0; i < _encoder_num; ++i) {
+    for (size_t i = 0; i < ENCODER_NUM; ++i) {
         encoder_values[i] = _encoders[i]->get_counts();
-        if ((encoder_values[i] != _prev_encoders_values[i]) &&
-             _timer.read_us() != 0) {
-            encoders_speed[i] = (encoder_values[i] - _prev_encoders_values[i]) * 1000.0f / _timer.read_us();
-            _timer.reset();
+        
+        if (_timer.read_us() > _sampling_delta_time_usec) {
+            if (encoder_values[i] == _prev_encoders_values[i]) {
+                _encoders_speed[i] = 0.0f;
+                is_active = false;
+            } else {
+                if (is_active) {
+                    _encoders_speed[i] = (encoder_values[i] - _prev_encoders_values[i]) * 1000.0f / _timer.read_us();
+                    _prev_encoders_values[i] = encoder_values[i];
+                } else {
+                    is_active = true;
+                }
+                _timer.reset();
+            }
         }
-        _prev_encoders_values[i] = encoder_values[i];
     }
     
-    _slave.set(ENCODER_SPEED1, encoders_speed[0]);
-    _slave.set(ENCODER_SPEED2, encoders_speed[1]);
-    _slave.set(ENCODER_SPEED3, encoders_speed[2]);
+    _slave.set(ENCODER_SPEED1, _encoders_speed[0]);
+    _slave.set(ENCODER_SPEED2, _encoders_speed[1]);
+    _slave.set(ENCODER_SPEED3, _encoders_speed[2]);
     _slave.set(ENCODER1, encoder_values[0]);
     _slave.set(ENCODER2, encoder_values[1]);
     _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);
-    
-    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);
+    _slave.set(POTENTIONMETER3, (_analog_inputs[2]->read() - 0.5f) * 2.0f * 32767.0f);
     
     _servos[0]->set_position(_slave.get(ESC1) / 32767.0f);
     _servos[1]->set_position(_slave.get(ESC2) / 32767.0f);