tarou yamada / serial_connected_mcu

nucleo側のプログラム

Dependents:   serial_connected_mcu_nucleo serial_connected_mcu_nucleo

Fork of serial_connected_mcu by tarou yamada

Files at this revision

API Documentation at this revision

Revision 11:6f8e34a71c7e, committed 2016-07-30

Comitter:
inst
Date:
Sat Jul 30 06:00:59 2016 +0000
Parent:
10:819597ff4492
Child:
12:810a484f113f
Commit message:
?? ENCODER_SPEED

Changed in this revision

serial_connected_mcu.cpp Show annotated file Show diff for this revision Revisions of this file
serial_connected_mcu.hpp Show annotated file Show diff for this revision Revisions of this file
serial_connected_mcu_slave.hpp Show annotated file Show diff for this revision Revisions of this file 
--- 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);

--- a/serial_connected_mcu.hpp	Sun Jul 10 22:43:30 2016 +0000
+++ b/serial_connected_mcu.hpp	Sat Jul 30 06:00:59 2016 +0000
@@ -10,27 +10,6 @@
 
 class serial_connected_mcu {
 public:
-    enum servo_id {
-        SERVO_0,
-        SERVO_1,
-        SERVO_2,
-        SERVO_NUM
-    };
-    
-    enum encoder_id {
-        ENCODER_0,
-        ENCODER_1,
-        ENCODER_2,
-        ENCODER_NUM
-    };
-    
-    enum analog_input_id {
-        ANALOG_IN_0,
-        ANALOG_IN_1,
-        ANALOG_IN_2,
-        ANALOG_INPUT_NUM
-    };
-
     static serial_connected_mcu* instance();
     void update();
 
@@ -46,16 +25,22 @@
     
     static const int16_t _init_write_data[SIZE_OF_WRITE_DATA];
     
-    static const PinName _servo_pins[SERVO_NUM];
-    
-    static TIM_TypeDef* _encoder_timers[ENCODER_NUM];
+    static const PinName _servo_pins[];
+    static TIM_TypeDef* _encoder_timers[];
     static const size_t _encoder_pulse_per_revol;
+    static const PinName _analog_input_pins[];
     
-    static const PinName _analog_input_pins[ANALOG_INPUT_NUM];
+    static const size_t _servo_num;
+    static const size_t _encoder_num;
+    static const size_t _analog_input_num;
     
-    servo* _servos[SERVO_NUM];
-    rotary_encoder_ab_phase* _encoders[ENCODER_NUM];
-    AnalogIn* _analog_inputs[ANALOG_INPUT_NUM];
+    servo** _servos;
+    rotary_encoder_ab_phase** _encoders;
+    AnalogIn** _analog_inputs;
+    
+    float* _prev_encoders_values;
+    float 
+    Timer _timer;
     
     serial_connected_mcu_slave _slave;
 };

--- a/serial_connected_mcu_slave.hpp	Sun Jul 10 22:43:30 2016 +0000
+++ b/serial_connected_mcu_slave.hpp	Sat Jul 30 06:00:59 2016 +0000
@@ -16,6 +16,9 @@
     };
     
     enum write_id {
+        ENCODER_SPEED1,
+        ENCODER_SPEED2,
+        ENCODER_SPEED3,
         ENCODER1,
         ENCODER2,
         ENCODER3,