Diff: serial_connected_mcu.cpp

Revision:

11:6f8e34a71c7e

Parent:

10:819597ff4492

Child:

13:2f9a5207fb51

--- a/serial_connected_mcu.cpp	Sun Jul 10 22:43:30 2016 +0000
+++ b/serial_connected_mcu.cpp	Sat Jul 30 06:00:59 2016 +0000
@@ -6,7 +6,7 @@
     
 serial_connected_mcu* serial_connected_mcu::_instance = NULL;
 
-const int16_t _init_write_data[SIZE_OF_WRITE_DATA] = {
+const int16_t _init_write_data[] = {
     0, // ENCODER1
     0, // ENCODER2
     0, // ENCODER3
@@ -14,25 +14,26 @@
     0, // POTENTIONMETER2
     0  // POTENTIONMETER3
 };
-const PinName serial_connected_mcu::_servo_pins[SERVO_NUM] = {
+const PinName serial_connected_mcu::_servo_pins[] = {
     PB_6,
     PB_7,
     PB_8
 };
-
-TIM_TypeDef* serial_connected_mcu::_encoder_timers[ENCODER_NUM] = {
+TIM_TypeDef* serial_connected_mcu::_encoder_timers[] = {
     TIM1,
     TIM2,
     TIM3
 };
-
 const size_t serial_connected_mcu::_encoder_pulse_per_revol = 100;
 
-const PinName serial_connected_mcu::_analog_input_pins[ANALOG_INPUT_NUM] = {
+const PinName serial_connected_mcu::_analog_input_pins[] = {
     A3,
     A4,
     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;
 
 serial_connected_mcu* serial_connected_mcu::instance() {
     if (_instance == NULL) {
@@ -42,38 +43,64 @@
 }
 
 serial_connected_mcu::serial_connected_mcu() {
+    _prev_encoders_values = new float[_encoder_num];
+    
     for (size_t i = 0; i < SIZE_OF_WRITE_DATA; ++i) {
         _slave.set(i, 0);
     }
     
-    for (size_t i = 0; i < SERVO_NUM; ++i) {
+    _servos = new servo*[_servo_num];
+    for (size_t i = 0; i < _servo_num; ++i) {
         _servos[i] = new servo(_servo_pins[i]);
     }
-    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;
     }
-    for (size_t i = 0; i < SERVO_NUM; ++i) {
+    
+    _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() {
-    for (size_t i = 0; i < SERVO_NUM; ++i) {
+    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 < SERVO_NUM; ++i) {
+    for (size_t i = 0; i < _analog_input_num; ++i) {
         delete _analog_inputs[i];
     }
 }
 
 void serial_connected_mcu::update() {
-    _slave.set(ENCODER1, _encoders[0]->get_revol_num() * 32767.0f);
-    _slave.set(ENCODER2, _encoders[1]->get_revol_num() * 32767.0f);
-    _slave.set(ENCODER3, _encoders[2]->get_revol_num() * 32767.0f);
+    float encoder_values[_encoder_num];
+    float encoders_speed[] = {};
+    
+    for (size_t i = 0; i < _encoder_num; ++i) {
+        encoder_values[i] = _encoders[i]->get_revol_num();
+        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();
+        }
+        _prev_encoders_values[i] = encoder_values[i];
+    }
+    
+    _slave.set(ENCODER1, encoder_values[0] * 32767.0f);
+    _slave.set(ENCODER2, encoder_values[1] * 32767.0f);
+    _slave.set(ENCODER3, encoder_values[2] * 32767.0f);
+    _slave.set(ENCODER_SPEED1, encoder_speed[0] * 32767.0f);
+    _slave.set(ENCODER_SPEED2, encoder_speed[1] * 32767.0f);
+    _slave.set(ENCODER_SPEED3, encoder_speed[2] * 32767.0f);
     _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);