nucleo側のプログラム
Dependents: serial_connected_mcu_nucleo serial_connected_mcu_nucleo
Fork of serial_connected_mcu by
Diff: serial_connected_mcu.cpp
- Revision:
- 16:4e310c3f3a2b
- Parent:
- 15:9a50a5a747ad
- Child:
- 17:814679e7b0fc
diff -r 9a50a5a747ad -r 4e310c3f3a2b serial_connected_mcu.cpp --- 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);