File content as of revision 18:111207e201b6:

#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 float serial_connected_mcu::_sampling_delta_time_usec = 5000.0f;

const PinName serial_connected_mcu::_servo_pins[] = {
    PB_6,
    PB_7,
    PB_8
};
TIM_TypeDef* serial_connected_mcu::_encoder_timers[] = {
    TIM1,
    TIM3,
    TIM2
};
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::_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;
    }
    _timer.start();
    
    _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]);
    }
}

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];
/*
    if (_timer.read_us() > _sampling_delta_time_usec) {
        float t = _timer.read_us();
        for (size_t i = 0; i < ENCODER_NUM; ++i) {
            encoder_values[i] = _encoders[i]->get_counts();
            if (encoder_values[i] == _prev_encoders_values[i]) {
                _encoders_speed[i] = 0.0f;
            } else {
                _encoders_speed[i] = (encoder_values[i] - _prev_encoders_values[i]) * 1000.0f / t;
            }
            _prev_encoders_values[i] = encoder_values[i];
        }
        _timer.reset();
    }
  */
    for (size_t i = 0; i < ENCODER_NUM; ++i) {
        encoder_values[i] = _encoders[i]->get_counts();
        //_encoders_speed[i] = encoder_values[i] - _prev_encoders_values[i];
        //_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 */