Diff: serial_connected_mcu.cpp

Revision:

17:814679e7b0fc

Parent:

16:4e310c3f3a2b

Child:

18:111207e201b6

--- a/serial_connected_mcu.cpp	Mon Aug 08 05:12:35 2016 +0000
+++ b/serial_connected_mcu.cpp	Mon Aug 08 09:15:56 2016 +0000
@@ -7,7 +7,7 @@
     
 serial_connected_mcu* serial_connected_mcu::_instance = NULL;
 
-const float serial_connected_mcu::_sampling_delta_time_usec = 1000.0f;
+const float serial_connected_mcu::_sampling_delta_time_usec = 1500.0f;
 
 const PinName serial_connected_mcu::_servo_pins[] = {
     PB_6,
@@ -54,13 +54,12 @@
         _encoders[i]->start();
         _prev_encoders_values[i] = 0.0f;
     }
+    _timer.start();
     
     _analog_inputs = new AnalogIn*[_analog_input_num];
     for (size_t i = 0; i < _analog_input_num; ++i) {
         _analog_inputs[i] = new AnalogIn(_analog_input_pins[i]);
     }
-    
-    _timer.start();
 }
 
 serial_connected_mcu::~serial_connected_mcu() {
@@ -76,26 +75,20 @@
 }
 
 void serial_connected_mcu::update() {
-    static bool is_active = false;
     float encoder_values[ENCODER_NUM];
-    
-    for (size_t i = 0; i < ENCODER_NUM; ++i) {
-        encoder_values[i] = _encoders[i]->get_counts();
-        
-        if (_timer.read_us() > _sampling_delta_time_usec) {
+
+    if (_timer.read_us() > _sampling_delta_time_usec) {
+        float t = _timer.read_us();
+        for (size_t i = 0; i < ENCODER_NUM; ++i) {
+            encoder_values[i] = _encoders[i]->get_counts();
             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();
+                _encoders_speed[i] = (encoder_values[i] - _prev_encoders_values[i]) * 1000.0f / t;
             }
+            _prev_encoders_values[i] = encoder_values[i];
         }
+        _timer.reset();
     }
     
     _slave.set(ENCODER_SPEED1, _encoders_speed[0]);