Diff: serial_connected_mcu.cpp

Revision:

13:2f9a5207fb51

Parent:

11:6f8e34a71c7e

Child:

14:014c339ecaf1

--- a/serial_connected_mcu.cpp	Sat Jul 30 06:03:21 2016 +0000
+++ b/serial_connected_mcu.cpp	Mon Aug 01 05:06:28 2016 +0000
@@ -5,7 +5,7 @@
 namespace serial_connected_mcu {
     
 serial_connected_mcu* serial_connected_mcu::_instance = NULL;
-
+/*
 const int16_t _init_write_data[] = {
     0, // ENCODER1
     0, // ENCODER2
@@ -14,6 +14,7 @@
     0, // POTENTIONMETER2
     0  // POTENTIONMETER3
 };
+*/
 const PinName serial_connected_mcu::_servo_pins[] = {
     PB_6,
     PB_7,
@@ -86,7 +87,7 @@
     float encoders_speed[] = {};
     
     for (size_t i = 0; i < _encoder_num; ++i) {
-        encoder_values[i] = _encoders[i]->get_revol_num();
+        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();
@@ -95,12 +96,12 @@
         _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(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);
     _slave.set(POTENTIONMETER3, (_analog_inputs[2]->read() - 0.5f) * 2.0f * 32767.0f);