nucleo側のプログラム
Dependents: serial_connected_mcu_nucleo serial_connected_mcu_nucleo
Fork of serial_connected_mcu by
serial_connected_mcu.cpp@9:21f623d2ee34, 2016-07-10 (annotated)
- Committer:
- inst
- Date:
- Sun Jul 10 22:41:25 2016 +0000
- Revision:
- 9:21f623d2ee34
- Parent:
- 8:b916e6f45f75
- Child:
- 10:819597ff4492
?????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
inst | 0:9ca9a6eddffa | 1 | #include "serial_connected_mcu.hpp" |
inst | 4:1323ef48a984 | 2 | #include "serial_connected_mcu_slave.hpp" |
inst | 0:9ca9a6eddffa | 3 | #include "mbed.h" |
inst | 0:9ca9a6eddffa | 4 | |
inst | 0:9ca9a6eddffa | 5 | namespace serial_connected_mcu { |
inst | 0:9ca9a6eddffa | 6 | |
inst | 0:9ca9a6eddffa | 7 | serial_connected_mcu* serial_connected_mcu::_instance = NULL; |
inst | 6:a0cc9c27cedf | 8 | |
inst | 6:a0cc9c27cedf | 9 | const int16_t _init_write_data[SIZE_OF_WRITE_DATA] = { |
inst | 6:a0cc9c27cedf | 10 | 0, // ENCODER1 |
inst | 6:a0cc9c27cedf | 11 | 0, // ENCODER2 |
inst | 6:a0cc9c27cedf | 12 | 0, // ENCODER3 |
inst | 6:a0cc9c27cedf | 13 | 65535 / 2, // POTENTIONMETER1 |
inst | 6:a0cc9c27cedf | 14 | 65535 / 2, // POTENTIONMETER2 |
inst | 6:a0cc9c27cedf | 15 | 65535 / 2 // POTENTIONMETER3 |
inst | 6:a0cc9c27cedf | 16 | }; |
inst | 0:9ca9a6eddffa | 17 | const PinName serial_connected_mcu::_servo_pins[SERVO_NUM] = { |
inst | 0:9ca9a6eddffa | 18 | PB_6, |
inst | 0:9ca9a6eddffa | 19 | PB_7, |
inst | 0:9ca9a6eddffa | 20 | PB_8 |
inst | 0:9ca9a6eddffa | 21 | }; |
inst | 0:9ca9a6eddffa | 22 | |
inst | 0:9ca9a6eddffa | 23 | TIM_TypeDef* serial_connected_mcu::_encoder_timers[ENCODER_NUM] = { |
inst | 0:9ca9a6eddffa | 24 | TIM1, |
inst | 0:9ca9a6eddffa | 25 | TIM2, |
inst | 0:9ca9a6eddffa | 26 | TIM3 |
inst | 0:9ca9a6eddffa | 27 | }; |
inst | 0:9ca9a6eddffa | 28 | |
inst | 0:9ca9a6eddffa | 29 | const size_t serial_connected_mcu::_encoder_pulse_per_revol = 100; |
inst | 0:9ca9a6eddffa | 30 | |
inst | 0:9ca9a6eddffa | 31 | const PinName serial_connected_mcu::_analog_input_pins[ANALOG_INPUT_NUM] = { |
inst | 0:9ca9a6eddffa | 32 | A3, |
inst | 0:9ca9a6eddffa | 33 | A4, |
inst | 0:9ca9a6eddffa | 34 | A5 |
inst | 0:9ca9a6eddffa | 35 | }; |
inst | 0:9ca9a6eddffa | 36 | |
inst | 0:9ca9a6eddffa | 37 | serial_connected_mcu* serial_connected_mcu::instance() { |
inst | 0:9ca9a6eddffa | 38 | if (_instance == NULL) { |
inst | 0:9ca9a6eddffa | 39 | _instance = new serial_connected_mcu; |
inst | 0:9ca9a6eddffa | 40 | } |
inst | 0:9ca9a6eddffa | 41 | return _instance; |
inst | 0:9ca9a6eddffa | 42 | } |
inst | 0:9ca9a6eddffa | 43 | |
inst | 0:9ca9a6eddffa | 44 | serial_connected_mcu::serial_connected_mcu() { |
inst | 8:b916e6f45f75 | 45 | for (size_t i = 0; i < SIZE_OF_WRITE_DATA; ++i) { |
inst | 6:a0cc9c27cedf | 46 | _slave.set(i, 0); |
inst | 4:1323ef48a984 | 47 | } |
inst | 4:1323ef48a984 | 48 | |
inst | 0:9ca9a6eddffa | 49 | for (size_t i = 0; i < SERVO_NUM; ++i) { |
inst | 0:9ca9a6eddffa | 50 | _servos[i] = new servo(_servo_pins[i]); |
inst | 0:9ca9a6eddffa | 51 | } |
inst | 0:9ca9a6eddffa | 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 | 0:9ca9a6eddffa | 55 | } |
inst | 0:9ca9a6eddffa | 56 | for (size_t i = 0; i < SERVO_NUM; ++i) { |
inst | 0:9ca9a6eddffa | 57 | _analog_inputs[i] = new AnalogIn(_analog_input_pins[i]); |
inst | 0:9ca9a6eddffa | 58 | } |
inst | 0:9ca9a6eddffa | 59 | } |
inst | 0:9ca9a6eddffa | 60 | |
inst | 1:0ba151c701a3 | 61 | serial_connected_mcu::~serial_connected_mcu() { |
inst | 1:0ba151c701a3 | 62 | for (size_t i = 0; i < SERVO_NUM; ++i) { |
inst | 1:0ba151c701a3 | 63 | delete _servos[i]; |
inst | 2:9d7a03c79cd3 | 64 | } |
inst | 1:0ba151c701a3 | 65 | for (size_t i = 0; i < ENCODER_NUM; ++i) { |
inst | 1:0ba151c701a3 | 66 | delete _encoders[i]; |
inst | 1:0ba151c701a3 | 67 | } |
inst | 1:0ba151c701a3 | 68 | for (size_t i = 0; i < SERVO_NUM; ++i) { |
inst | 1:0ba151c701a3 | 69 | delete _analog_inputs[i]; |
inst | 1:0ba151c701a3 | 70 | } |
inst | 1:0ba151c701a3 | 71 | } |
inst | 1:0ba151c701a3 | 72 | |
inst | 0:9ca9a6eddffa | 73 | void serial_connected_mcu::update() { |
inst | 9:21f623d2ee34 | 74 | _slave.set(ENCODER1, _encoders[0]->get_revol_num() * 32767.0f); |
inst | 9:21f623d2ee34 | 75 | _slave.set(ENCODER2, _encoders[1]->get_revol_num() * 32767.0f); |
inst | 9:21f623d2ee34 | 76 | _slave.set(ENCODER3, _encoders[2]->get_revol_num() * 32767.0f); |
inst | 9:21f623d2ee34 | 77 | _slave.set(POTENTIONMETER1, (_analog_inputs[0]->read() - 0.5f) * 2.0f * 32767.0f); |
inst | 9:21f623d2ee34 | 78 | _slave.set(POTENTIONMETER2, (_analog_inputs[1]->read() - 0.5f) * 2.0f * 32767.0f); |
inst | 9:21f623d2ee34 | 79 | _slave.set(POTENTIONMETER3, (_analog_inputs[2]->read() - 0.5f) * 2.0f * 32767.0f); |
inst | 0:9ca9a6eddffa | 80 | |
inst | 9:21f623d2ee34 | 81 | _servos[0]->set_position(_slave.get(ESC1) / 32767.0f); |
inst | 9:21f623d2ee34 | 82 | _servos[1]->set_position(_slave.get(ESC2) / 32767.0f); |
inst | 9:21f623d2ee34 | 83 | _servos[2]->set_position(_slave.get(ESC3) / 32767.0f); |
inst | 0:9ca9a6eddffa | 84 | } |
inst | 0:9ca9a6eddffa | 85 | |
inst | 0:9ca9a6eddffa | 86 | } /* namespace serial_connected_mcu */ |