Austin Buchan / mbed_zumy_packet

Publishing for Biomimetics.

Dependencies:   CRC MODDMA MODSERIAL MPU6050IMU PID QEI mbed-rtos mbed-src

sensors.cpp@0:8cfa73bb68e4, 2016-05-31 (annotated)

Committer:
abuchan
Date:
Tue May 31 17:04:59 2016 +0000
Revision:
0:8cfa73bb68e4
Publishing for Biomimetics.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abuchan 0:8cfa73bb68e4 1 #include "sensors.h"
abuchan 0:8cfa73bb68e4 2
abuchan 0:8cfa73bb68e4 3 Sensors::Sensors(
abuchan 0:8cfa73bb68e4 4 Timer *system_timer,
abuchan 0:8cfa73bb68e4 5 PinName voltage_pin,
abuchan 0:8cfa73bb68e4 6 PinName l_enc_a_pin, PinName l_enc_b_pin,
abuchan 0:8cfa73bb68e4 7 PinName r_enc_a_pin, PinName r_enc_b_pin, uint32_t tick_per_rev,
abuchan 0:8cfa73bb68e4 8 PinName imu_sda_pin, PinName imu_scl_pin) :
abuchan 0:8cfa73bb68e4 9
abuchan 0:8cfa73bb68e4 10 system_timer_(system_timer),
abuchan 0:8cfa73bb68e4 11 voltage_pin_(voltage_pin),
abuchan 0:8cfa73bb68e4 12 left_qei_(l_enc_a_pin, l_enc_b_pin, NC, tick_per_rev, QEI::X4_ENCODING),
abuchan 0:8cfa73bb68e4 13 right_qei_(r_enc_a_pin, r_enc_b_pin, NC, tick_per_rev, QEI::X4_ENCODING),
abuchan 0:8cfa73bb68e4 14 mpu6050_(imu_sda_pin, imu_scl_pin) {}
abuchan 0:8cfa73bb68e4 15
abuchan 0:8cfa73bb68e4 16 float Sensors::get_voltage(void) {
abuchan 0:8cfa73bb68e4 17 return voltage_pin_.read();
abuchan 0:8cfa73bb68e4 18 }
abuchan 0:8cfa73bb68e4 19
abuchan 0:8cfa73bb68e4 20 void Sensors::get_encoders(int32_t (&encoders)[2]) {
abuchan 0:8cfa73bb68e4 21 encoders[0] = left_qei_.getPulses();
abuchan 0:8cfa73bb68e4 22 encoders[1] = -right_qei_.getPulses();
abuchan 0:8cfa73bb68e4 23 }
abuchan 0:8cfa73bb68e4 24
abuchan 0:8cfa73bb68e4 25 void Sensors::get_angles(float * angles) {
abuchan 0:8cfa73bb68e4 26 angles[0] = left_qei_.getPulseFraction();
abuchan 0:8cfa73bb68e4 27 angles[1] = -right_qei_.getPulseFraction();
abuchan 0:8cfa73bb68e4 28 }
abuchan 0:8cfa73bb68e4 29
abuchan 0:8cfa73bb68e4 30 bool Sensors::get_imu(sensor_data_t* sensor_data) {
abuchan 0:8cfa73bb68e4 31 sensor_data->time = system_timer_->read_us();
abuchan 0:8cfa73bb68e4 32 return mpu6050_.readCalibAccelGyroData(sensor_data->accel, sensor_data->gyro);
abuchan 0:8cfa73bb68e4 33 }
abuchan 0:8cfa73bb68e4 34
abuchan 0:8cfa73bb68e4 35 bool Sensors::fill_sensor_packet(packet_t* pkt) {
abuchan 0:8cfa73bb68e4 36 pkt->header.type = PKT_TYPE_SENSOR;
abuchan 0:8cfa73bb68e4 37 pkt->header.length = sizeof(header_t) + sizeof(sensor_data_t) + 1;
abuchan 0:8cfa73bb68e4 38 sensor_data_t* sensor_data = (sensor_data_t*)pkt->data_crc;
abuchan 0:8cfa73bb68e4 39 sensor_data->voltage = get_voltage();
abuchan 0:8cfa73bb68e4 40 get_encoders(sensor_data->encoder);
abuchan 0:8cfa73bb68e4 41 bool valid = get_imu(sensor_data);
abuchan 0:8cfa73bb68e4 42 pkt->header.flags = valid ? 0 : 1;
abuchan 0:8cfa73bb68e4 43 return valid;
abuchan 0:8cfa73bb68e4 44 }