Publishing for Biomimetics.
Dependencies: CRC MODDMA MODSERIAL MPU6050IMU PID QEI mbed-rtos mbed-src
Diff: sensors.cpp
- Revision:
- 0:8cfa73bb68e4
diff -r 000000000000 -r 8cfa73bb68e4 sensors.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sensors.cpp Tue May 31 17:04:59 2016 +0000 @@ -0,0 +1,44 @@ +#include "sensors.h" + +Sensors::Sensors( + Timer *system_timer, + PinName voltage_pin, + PinName l_enc_a_pin, PinName l_enc_b_pin, + PinName r_enc_a_pin, PinName r_enc_b_pin, uint32_t tick_per_rev, + PinName imu_sda_pin, PinName imu_scl_pin) : + + system_timer_(system_timer), + voltage_pin_(voltage_pin), + left_qei_(l_enc_a_pin, l_enc_b_pin, NC, tick_per_rev, QEI::X4_ENCODING), + right_qei_(r_enc_a_pin, r_enc_b_pin, NC, tick_per_rev, QEI::X4_ENCODING), + mpu6050_(imu_sda_pin, imu_scl_pin) {} + +float Sensors::get_voltage(void) { + return voltage_pin_.read(); +} + +void Sensors::get_encoders(int32_t (&encoders)[2]) { + encoders[0] = left_qei_.getPulses(); + encoders[1] = -right_qei_.getPulses(); +} + +void Sensors::get_angles(float * angles) { + angles[0] = left_qei_.getPulseFraction(); + angles[1] = -right_qei_.getPulseFraction(); +} + +bool Sensors::get_imu(sensor_data_t* sensor_data) { + sensor_data->time = system_timer_->read_us(); + return mpu6050_.readCalibAccelGyroData(sensor_data->accel, sensor_data->gyro); +} + +bool Sensors::fill_sensor_packet(packet_t* pkt) { + pkt->header.type = PKT_TYPE_SENSOR; + pkt->header.length = sizeof(header_t) + sizeof(sensor_data_t) + 1; + sensor_data_t* sensor_data = (sensor_data_t*)pkt->data_crc; + sensor_data->voltage = get_voltage(); + get_encoders(sensor_data->encoder); + bool valid = get_imu(sensor_data); + pkt->header.flags = valid ? 0 : 1; + return valid; +} \ No newline at end of file