nucleo側のプログラム
Dependents: serial_connected_mcu_nucleo serial_connected_mcu_nucleo
Fork of serial_connected_mcu by
serial_connected_mcu.cpp
- Committer:
- inst
- Date:
- 2016-08-01
- Revision:
- 14:014c339ecaf1
- Parent:
- 13:2f9a5207fb51
- Child:
- 15:9a50a5a747ad
File content as of revision 14:014c339ecaf1:
#include "serial_connected_mcu.hpp" #include "serial_connected_mcu_slave.hpp" #include "mbed.h" 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 PinName serial_connected_mcu::_servo_pins[] = { PB_6, PB_7, PB_8 }; 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[] = { 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) { _instance = new serial_connected_mcu; } return _instance; } 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); } _servos = new servo*[_servo_num]; for (size_t i = 0; i < _servo_num; ++i) { _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[i] = new rotary_encoder_ab_phase(_encoder_timers[i], _encoder_pulse_per_revol); _encoders[i]->start(); _prev_encoders_values[i] = 0.0f; } _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) { delete _servos[i]; } for (size_t i = 0; i < _encoder_num; ++i) { delete _encoders[i]; } for (size_t i = 0; i < _analog_input_num; ++i) { delete _analog_inputs[i]; } } void serial_connected_mcu::update() { float encoder_values[_encoder_num]; float encoders_speed[_encoder_num] = {}; 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(); } _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(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); _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); _servos[2]->set_position(_slave.get(ESC3) / 32767.0f); } } /* namespace serial_connected_mcu */