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@15:9a50a5a747ad, 2016-08-01 (annotated)

Committer:
inst
Date:
Mon Aug 01 05:33:09 2016 +0000
Revision:
15:9a50a5a747ad
Parent:
14:014c339ecaf1
Child:
16:4e310c3f3a2b
USE_LIFTER_MODE ???;

Who changed what in which revision?

UserRevisionLine numberNew 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 13:2f9a5207fb51 8 /*
inst 11:6f8e34a71c7e 9 const int16_t _init_write_data[] = {
inst 6:a0cc9c27cedf 10 0, // ENCODER1
inst 6:a0cc9c27cedf 11 0, // ENCODER2
inst 6:a0cc9c27cedf 12 0, // ENCODER3
inst 10:819597ff4492 13 0, // POTENTIONMETER1
inst 10:819597ff4492 14 0, // POTENTIONMETER2
inst 10:819597ff4492 15 0 // POTENTIONMETER3
inst 6:a0cc9c27cedf 16 };
inst 13:2f9a5207fb51 17 */
inst 11:6f8e34a71c7e 18 const PinName serial_connected_mcu::_servo_pins[] = {
inst 0:9ca9a6eddffa 19 PB_6,
inst 0:9ca9a6eddffa 20 PB_7,
inst 0:9ca9a6eddffa 21 PB_8
inst 0:9ca9a6eddffa 22 };
inst 11:6f8e34a71c7e 23 TIM_TypeDef* serial_connected_mcu::_encoder_timers[] = {
inst 0:9ca9a6eddffa 24 TIM1,
inst 0:9ca9a6eddffa 25 TIM2,
inst 0:9ca9a6eddffa 26 TIM3
inst 0:9ca9a6eddffa 27 };
inst 0:9ca9a6eddffa 28 const size_t serial_connected_mcu::_encoder_pulse_per_revol = 100;
inst 0:9ca9a6eddffa 29
inst 11:6f8e34a71c7e 30 const PinName serial_connected_mcu::_analog_input_pins[] = {
inst 0:9ca9a6eddffa 31 A3,
inst 0:9ca9a6eddffa 32 A4,
inst 0:9ca9a6eddffa 33 A5
inst 0:9ca9a6eddffa 34 };
inst 11:6f8e34a71c7e 35 const size_t serial_connected_mcu::_servo_num = 3;
inst 11:6f8e34a71c7e 36 const size_t serial_connected_mcu::_encoder_num = 3;
inst 11:6f8e34a71c7e 37 const size_t serial_connected_mcu::_analog_input_num = 3;
inst 0:9ca9a6eddffa 38
inst 15:9a50a5a747ad 39 #ifdef USE_LIFTER_MODE
inst 15:9a50a5a747ad 40 const PinName serial_connected_mcu::_limit_switch_pins[] = {
inst 15:9a50a5a747ad 41 D11,
inst 15:9a50a5a747ad 42 D12
inst 15:9a50a5a747ad 43 };
inst 15:9a50a5a747ad 44 #endif
inst 15:9a50a5a747ad 45
inst 0:9ca9a6eddffa 46 serial_connected_mcu* serial_connected_mcu::instance() {
inst 0:9ca9a6eddffa 47 if (_instance == NULL) {
inst 0:9ca9a6eddffa 48 _instance = new serial_connected_mcu;
inst 0:9ca9a6eddffa 49 }
inst 0:9ca9a6eddffa 50 return _instance;
inst 0:9ca9a6eddffa 51 }
inst 0:9ca9a6eddffa 52
inst 0:9ca9a6eddffa 53 serial_connected_mcu::serial_connected_mcu() {
inst 15:9a50a5a747ad 54 #ifdef USE_LIFTER_MODE
inst 15:9a50a5a747ad 55 for (size_t i = 0; i < LIMIT_SWITCHES_NUM; ++i) {
inst 15:9a50a5a747ad 56 _limit_switches[i] = new DigitalIn(_limit_switch_pins[i]);
inst 15:9a50a5a747ad 57 }
inst 15:9a50a5a747ad 58 #endif
inst 15:9a50a5a747ad 59
inst 11:6f8e34a71c7e 60 _prev_encoders_values = new float[_encoder_num];
inst 11:6f8e34a71c7e 61
inst 8:b916e6f45f75 62 for (size_t i = 0; i < SIZE_OF_WRITE_DATA; ++i) {
inst 6:a0cc9c27cedf 63 _slave.set(i, 0);
inst 4:1323ef48a984 64 }
inst 4:1323ef48a984 65
inst 11:6f8e34a71c7e 66 _servos = new servo*[_servo_num];
inst 11:6f8e34a71c7e 67 for (size_t i = 0; i < _servo_num; ++i) {
inst 0:9ca9a6eddffa 68 _servos[i] = new servo(_servo_pins[i]);
inst 0:9ca9a6eddffa 69 }
inst 11:6f8e34a71c7e 70
inst 11:6f8e34a71c7e 71 _encoders = new rotary_encoder_ab_phase*[_encoder_num];
inst 11:6f8e34a71c7e 72 for (size_t i = 0; i < _encoder_num; ++i) {
inst 0:9ca9a6eddffa 73 _encoders[i] = new rotary_encoder_ab_phase(_encoder_timers[i], _encoder_pulse_per_revol);
inst 8:b916e6f45f75 74 _encoders[i]->start();
inst 11:6f8e34a71c7e 75 _prev_encoders_values[i] = 0.0f;
inst 0:9ca9a6eddffa 76 }
inst 11:6f8e34a71c7e 77
inst 11:6f8e34a71c7e 78 _analog_inputs = new AnalogIn*[_analog_input_num];
inst 11:6f8e34a71c7e 79 for (size_t i = 0; i < _analog_input_num; ++i) {
inst 0:9ca9a6eddffa 80 _analog_inputs[i] = new AnalogIn(_analog_input_pins[i]);
inst 0:9ca9a6eddffa 81 }
inst 11:6f8e34a71c7e 82
inst 11:6f8e34a71c7e 83 _timer.start();
inst 0:9ca9a6eddffa 84 }
inst 0:9ca9a6eddffa 85
inst 1:0ba151c701a3 86 serial_connected_mcu::~serial_connected_mcu() {
inst 11:6f8e34a71c7e 87 for (size_t i = 0; i < _servo_num; ++i) {
inst 1:0ba151c701a3 88 delete _servos[i];
inst 2:9d7a03c79cd3 89 }
inst 11:6f8e34a71c7e 90 for (size_t i = 0; i < _encoder_num; ++i) {
inst 1:0ba151c701a3 91 delete _encoders[i];
inst 1:0ba151c701a3 92 }
inst 11:6f8e34a71c7e 93 for (size_t i = 0; i < _analog_input_num; ++i) {
inst 1:0ba151c701a3 94 delete _analog_inputs[i];
inst 1:0ba151c701a3 95 }
inst 1:0ba151c701a3 96 }
inst 1:0ba151c701a3 97
inst 0:9ca9a6eddffa 98 void serial_connected_mcu::update() {
inst 11:6f8e34a71c7e 99 float encoder_values[_encoder_num];
inst 14:014c339ecaf1 100 float encoders_speed[_encoder_num] = {};
inst 11:6f8e34a71c7e 101
inst 11:6f8e34a71c7e 102 for (size_t i = 0; i < _encoder_num; ++i) {
inst 13:2f9a5207fb51 103 encoder_values[i] = _encoders[i]->get_counts();
inst 11:6f8e34a71c7e 104 if ((encoder_values[i] != _prev_encoders_values[i]) &&
inst 11:6f8e34a71c7e 105 _timer.read_us() != 0) {
inst 11:6f8e34a71c7e 106 encoders_speed[i] = (encoder_values[i] - _prev_encoders_values[i]) * 1000.0f / _timer.read_us();
inst 11:6f8e34a71c7e 107 _timer.reset();
inst 11:6f8e34a71c7e 108 }
inst 11:6f8e34a71c7e 109 _prev_encoders_values[i] = encoder_values[i];
inst 11:6f8e34a71c7e 110 }
inst 11:6f8e34a71c7e 111
inst 13:2f9a5207fb51 112 _slave.set(ENCODER_SPEED1, encoders_speed[0]);
inst 13:2f9a5207fb51 113 _slave.set(ENCODER_SPEED2, encoders_speed[1]);
inst 13:2f9a5207fb51 114 _slave.set(ENCODER_SPEED3, encoders_speed[2]);
inst 13:2f9a5207fb51 115 _slave.set(ENCODER1, encoder_values[0]);
inst 13:2f9a5207fb51 116 _slave.set(ENCODER2, encoder_values[1]);
inst 13:2f9a5207fb51 117 _slave.set(ENCODER3, encoder_values[2]);
inst 9:21f623d2ee34 118 _slave.set(POTENTIONMETER1, (_analog_inputs[0]->read() - 0.5f) * 2.0f * 32767.0f);
inst 9:21f623d2ee34 119 _slave.set(POTENTIONMETER2, (_analog_inputs[1]->read() - 0.5f) * 2.0f * 32767.0f);
inst 15:9a50a5a747ad 120
inst 15:9a50a5a747ad 121 float p = 0.0f;
inst 15:9a50a5a747ad 122 #ifdef USE_LIFTER_MODE
inst 15:9a50a5a747ad 123 if (_limit_switches[UPPER_LIMIT]->read() == 1) {
inst 15:9a50a5a747ad 124 p = 1.0f;
inst 15:9a50a5a747ad 125 } else if (_limit_switches[LOWER_LIMIT]->read() == 1) {
inst 15:9a50a5a747ad 126 p = -1.0f;
inst 15:9a50a5a747ad 127 }
inst 15:9a50a5a747ad 128 #else
inst 15:9a50a5a747ad 129 p = (_analog_inputs[2]->read() - 0.5f) * 2.0f * 32767.0f;
inst 15:9a50a5a747ad 130 #endif
inst 15:9a50a5a747ad 131
inst 15:9a50a5a747ad 132 _slave.set(POTENTIONMETER3, p);
inst 0:9ca9a6eddffa 133
inst 9:21f623d2ee34 134 _servos[0]->set_position(_slave.get(ESC1) / 32767.0f);
inst 9:21f623d2ee34 135 _servos[1]->set_position(_slave.get(ESC2) / 32767.0f);
inst 9:21f623d2ee34 136 _servos[2]->set_position(_slave.get(ESC3) / 32767.0f);
inst 0:9ca9a6eddffa 137 }
inst 0:9ca9a6eddffa 138
inst 0:9ca9a6eddffa 139 } /* namespace serial_connected_mcu */