tarou yamada / serial_connected_mcu

nucleo側のプログラム

Dependents:   serial_connected_mcu_nucleo serial_connected_mcu_nucleo

Fork of serial_connected_mcu by tarou yamada

serial_connected_mcu.cpp@16:4e310c3f3a2b, 2016-08-08 (annotated)

Committer:
inst
Date:
Mon Aug 08 05:12:35 2016 +0000
Revision:
16:4e310c3f3a2b
Parent:
15:9a50a5a747ad
Child:
17:814679e7b0fc
????????????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
inst 16:4e310c3f3a2b 1
inst 0:9ca9a6eddffa 2 #include "serial_connected_mcu.hpp"
inst 4:1323ef48a984 3 #include "serial_connected_mcu_slave.hpp"
inst 0:9ca9a6eddffa 4 #include "mbed.h"
inst 0:9ca9a6eddffa 5
inst 0:9ca9a6eddffa 6 namespace serial_connected_mcu {
inst 0:9ca9a6eddffa 7
inst 0:9ca9a6eddffa 8 serial_connected_mcu* serial_connected_mcu::_instance = NULL;
inst 16:4e310c3f3a2b 9
inst 16:4e310c3f3a2b 10 const float serial_connected_mcu::_sampling_delta_time_usec = 1000.0f;
inst 16:4e310c3f3a2b 11
inst 11:6f8e34a71c7e 12 const PinName serial_connected_mcu::_servo_pins[] = {
inst 0:9ca9a6eddffa 13 PB_6,
inst 0:9ca9a6eddffa 14 PB_7,
inst 0:9ca9a6eddffa 15 PB_8
inst 0:9ca9a6eddffa 16 };
inst 11:6f8e34a71c7e 17 TIM_TypeDef* serial_connected_mcu::_encoder_timers[] = {
inst 0:9ca9a6eddffa 18 TIM1,
inst 16:4e310c3f3a2b 19 TIM3,
inst 16:4e310c3f3a2b 20 TIM2
inst 0:9ca9a6eddffa 21 };
inst 0:9ca9a6eddffa 22 const size_t serial_connected_mcu::_encoder_pulse_per_revol = 100;
inst 0:9ca9a6eddffa 23
inst 11:6f8e34a71c7e 24 const PinName serial_connected_mcu::_analog_input_pins[] = {
inst 0:9ca9a6eddffa 25 A3,
inst 0:9ca9a6eddffa 26 A4,
inst 0:9ca9a6eddffa 27 A5
inst 0:9ca9a6eddffa 28 };
inst 11:6f8e34a71c7e 29 const size_t serial_connected_mcu::_servo_num = 3;
inst 11:6f8e34a71c7e 30 const size_t serial_connected_mcu::_analog_input_num = 3;
inst 0:9ca9a6eddffa 31
inst 0:9ca9a6eddffa 32 serial_connected_mcu* serial_connected_mcu::instance() {
inst 0:9ca9a6eddffa 33 if (_instance == NULL) {
inst 0:9ca9a6eddffa 34 _instance = new serial_connected_mcu;
inst 0:9ca9a6eddffa 35 }
inst 0:9ca9a6eddffa 36 return _instance;
inst 0:9ca9a6eddffa 37 }
inst 0:9ca9a6eddffa 38
inst 0:9ca9a6eddffa 39 serial_connected_mcu::serial_connected_mcu() {
inst 16:4e310c3f3a2b 40 _prev_encoders_values = new float[ENCODER_NUM];
inst 11:6f8e34a71c7e 41
inst 8:b916e6f45f75 42 for (size_t i = 0; i < SIZE_OF_WRITE_DATA; ++i) {
inst 6:a0cc9c27cedf 43 _slave.set(i, 0);
inst 4:1323ef48a984 44 }
inst 4:1323ef48a984 45
inst 11:6f8e34a71c7e 46 _servos = new servo*[_servo_num];
inst 11:6f8e34a71c7e 47 for (size_t i = 0; i < _servo_num; ++i) {
inst 0:9ca9a6eddffa 48 _servos[i] = new servo(_servo_pins[i]);
inst 0:9ca9a6eddffa 49 }
inst 11:6f8e34a71c7e 50
inst 16:4e310c3f3a2b 51 _encoders = new rotary_encoder_ab_phase*[ENCODER_NUM];
inst 16:4e310c3f3a2b 52 for (size_t i = 0; i < ENCODER_NUM; ++i) {
inst 0:9ca9a6eddffa 53 _encoders[i] = new rotary_encoder_ab_phase(_encoder_timers[i], _encoder_pulse_per_revol);
inst 8:b916e6f45f75 54 _encoders[i]->start();
inst 11:6f8e34a71c7e 55 _prev_encoders_values[i] = 0.0f;
inst 0:9ca9a6eddffa 56 }
inst 11:6f8e34a71c7e 57
inst 11:6f8e34a71c7e 58 _analog_inputs = new AnalogIn*[_analog_input_num];
inst 11:6f8e34a71c7e 59 for (size_t i = 0; i < _analog_input_num; ++i) {
inst 0:9ca9a6eddffa 60 _analog_inputs[i] = new AnalogIn(_analog_input_pins[i]);
inst 0:9ca9a6eddffa 61 }
inst 11:6f8e34a71c7e 62
inst 11:6f8e34a71c7e 63 _timer.start();
inst 0:9ca9a6eddffa 64 }
inst 0:9ca9a6eddffa 65
inst 1:0ba151c701a3 66 serial_connected_mcu::~serial_connected_mcu() {
inst 11:6f8e34a71c7e 67 for (size_t i = 0; i < _servo_num; ++i) {
inst 1:0ba151c701a3 68 delete _servos[i];
inst 2:9d7a03c79cd3 69 }
inst 16:4e310c3f3a2b 70 for (size_t i = 0; i < ENCODER_NUM; ++i) {
inst 1:0ba151c701a3 71 delete _encoders[i];
inst 1:0ba151c701a3 72 }
inst 11:6f8e34a71c7e 73 for (size_t i = 0; i < _analog_input_num; ++i) {
inst 1:0ba151c701a3 74 delete _analog_inputs[i];
inst 1:0ba151c701a3 75 }
inst 1:0ba151c701a3 76 }
inst 1:0ba151c701a3 77
inst 0:9ca9a6eddffa 78 void serial_connected_mcu::update() {
inst 16:4e310c3f3a2b 79 static bool is_active = false;
inst 16:4e310c3f3a2b 80 float encoder_values[ENCODER_NUM];
inst 11:6f8e34a71c7e 81
inst 16:4e310c3f3a2b 82 for (size_t i = 0; i < ENCODER_NUM; ++i) {
inst 13:2f9a5207fb51 83 encoder_values[i] = _encoders[i]->get_counts();
inst 16:4e310c3f3a2b 84
inst 16:4e310c3f3a2b 85 if (_timer.read_us() > _sampling_delta_time_usec) {
inst 16:4e310c3f3a2b 86 if (encoder_values[i] == _prev_encoders_values[i]) {
inst 16:4e310c3f3a2b 87 _encoders_speed[i] = 0.0f;
inst 16:4e310c3f3a2b 88 is_active = false;
inst 16:4e310c3f3a2b 89 } else {
inst 16:4e310c3f3a2b 90 if (is_active) {
inst 16:4e310c3f3a2b 91 _encoders_speed[i] = (encoder_values[i] - _prev_encoders_values[i]) * 1000.0f / _timer.read_us();
inst 16:4e310c3f3a2b 92 _prev_encoders_values[i] = encoder_values[i];
inst 16:4e310c3f3a2b 93 } else {
inst 16:4e310c3f3a2b 94 is_active = true;
inst 16:4e310c3f3a2b 95 }
inst 16:4e310c3f3a2b 96 _timer.reset();
inst 16:4e310c3f3a2b 97 }
inst 11:6f8e34a71c7e 98 }
inst 11:6f8e34a71c7e 99 }
inst 11:6f8e34a71c7e 100
inst 16:4e310c3f3a2b 101 _slave.set(ENCODER_SPEED1, _encoders_speed[0]);
inst 16:4e310c3f3a2b 102 _slave.set(ENCODER_SPEED2, _encoders_speed[1]);
inst 16:4e310c3f3a2b 103 _slave.set(ENCODER_SPEED3, _encoders_speed[2]);
inst 13:2f9a5207fb51 104 _slave.set(ENCODER1, encoder_values[0]);
inst 13:2f9a5207fb51 105 _slave.set(ENCODER2, encoder_values[1]);
inst 13:2f9a5207fb51 106 _slave.set(ENCODER3, encoder_values[2]);
inst 9:21f623d2ee34 107 _slave.set(POTENTIONMETER1, (_analog_inputs[0]->read() - 0.5f) * 2.0f * 32767.0f);
inst 9:21f623d2ee34 108 _slave.set(POTENTIONMETER2, (_analog_inputs[1]->read() - 0.5f) * 2.0f * 32767.0f);
inst 16:4e310c3f3a2b 109 _slave.set(POTENTIONMETER3, (_analog_inputs[2]->read() - 0.5f) * 2.0f * 32767.0f);
inst 0:9ca9a6eddffa 110
inst 9:21f623d2ee34 111 _servos[0]->set_position(_slave.get(ESC1) / 32767.0f);
inst 9:21f623d2ee34 112 _servos[1]->set_position(_slave.get(ESC2) / 32767.0f);
inst 9:21f623d2ee34 113 _servos[2]->set_position(_slave.get(ESC3) / 32767.0f);
inst 0:9ca9a6eddffa 114 }
inst 0:9ca9a6eddffa 115
inst 0:9ca9a6eddffa 116 } /* namespace serial_connected_mcu */