Revision 0:8cfa73bb68e4, committed 2016-05-31

Comitter:

abuchan

Date:

Tue May 31 17:04:59 2016 +0000

Commit message:

Publishing for Biomimetics.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CRC.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/jpelletier/code/CRC/#58b0642c11b0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODDMA.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/AjK/code/MODDMA/#97a16bf2ff43

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/abuchan/code/MODSERIAL/#758424d8503e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050IMU.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/abuchan/code/MPU6050IMU/#359efdec694f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,97 @@
+#include "control.h"
+
+Control::Control(
+    PinName left_mot_0_pin, PinName left_mot_1_pin, 
+    PinName right_mot_0_pin, PinName right_mot_1_pin, 
+    Sensors *sensors, uint32_t tick_per_rev,
+    float kP, float kI, float kD, float period, float velocity_max,
+    float pid_dead_band
+  ) :
+    sensors_(sensors),
+    control_timer_(&Control::control_helper,osTimerPeriodic,this),
+    pid_dead_band_(pid_dead_band)
+  {
+
+  motors_[MOTOR_LEFT][0] = new PwmOut(left_mot_0_pin);
+  motors_[MOTOR_LEFT][1] = new PwmOut(left_mot_1_pin);
+  motors_[MOTOR_RIGHT][0] = new PwmOut(right_mot_0_pin);
+  motors_[MOTOR_RIGHT][1] = new PwmOut(right_mot_1_pin);
+  
+  // For the LPC1768, all PWM channels are on the same timer, so setting one
+  // period sets them all
+  motors_[MOTOR_LEFT][0]->period_us(50);
+
+  tick_to_angular_velocity_ = 2.0 * 3.14159265358979323846 / (float)(tick_per_rev * period);
+  
+  pid_init(MOTOR_LEFT,kP,kI,kD,period,velocity_max);
+  pid_init(MOTOR_RIGHT,kP,kI,kD,period,velocity_max);
+
+  control_timer_.start(period*1000);
+
+  last_positions_[MOTOR_LEFT] = 0;
+  last_positions_[MOTOR_RIGHT] = 0;
+}
+
+void Control::set_setpoints(float left, float right) {
+  pids_[MOTOR_LEFT]->setSetPoint(left);
+  pids_[MOTOR_RIGHT]->setSetPoint(right);
+}
+
+void Control::fill_pid_packet(packet_t* pkt) {
+  pkt->header.type = PKT_TYPE_PID;
+  pkt->header.length = sizeof(header_t) + sizeof(pid_data_t) + 1;
+  pid_data_t* pid_data = (pid_data_t*)pkt->data_crc;
+  pid_data->vel[MOTOR_LEFT] = velocities_[MOTOR_LEFT];
+  pid_data->vel[MOTOR_RIGHT] = velocities_[MOTOR_RIGHT];
+  pid_data->pwm[MOTOR_LEFT] = pwms_[MOTOR_LEFT];
+  pid_data->pwm[MOTOR_RIGHT] = pwms_[MOTOR_RIGHT];
+}
+
+void Control::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->velocity[MOTOR_LEFT] = velocities_[MOTOR_LEFT];
+  sensor_data->velocity[MOTOR_RIGHT] = velocities_[MOTOR_RIGHT];
+}
+
+void Control::set_motor_pwm(int motor, float value) {
+  if (value >= 0.0) {
+    motors_[motor][0]->write(value);
+    motors_[motor][1]->write(0.0);
+  } else {
+    motors_[motor][0]->write(0.0);
+    motors_[motor][1]->write(-value);
+  }
+}
+
+void Control::pid_init(int motor, float kP, float kI, float kD, float period, float velocity_max) {
+  pids_[motor] = new PID(kP,kI,kD,period);
+  pids_[motor]->setInputLimits(-velocity_max, velocity_max);
+  pids_[motor]->setOutputLimits(-1.0,1.0);
+  pids_[motor]->setBias(0.0);
+  pids_[motor]->setSetPoint(0.0);
+}
+
+void Control::control_helper(const void* p) {
+  Control* instance = (Control*)p;
+  instance->control_update();
+}
+
+void Control::control_update(void) {
+  float positions[2];
+  
+  sensors_->get_angles(positions);
+
+  for (uint32_t i=0; i<2; i++) {
+    velocities_[i] = tick_to_angular_velocity_ * (positions[i] - last_positions_[i]);
+    last_positions_[i] = positions[i];
+    pids_[i]->setProcessValue(velocities_[i]);
+    pwms_[i] = pids_[i]->compute();
+    if (fabs(pwms_[i]) < pid_dead_band_)
+      pwms_[i] = 0.0;
+  }
+
+  set_motor_pwm(MOTOR_LEFT, pwms_[MOTOR_LEFT]);
+  set_motor_pwm(MOTOR_RIGHT, pwms_[MOTOR_RIGHT]);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.h	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,58 @@
+#ifndef CONTROL_H
+#define CONTROL_H
+
+#include "mbed.h"
+
+#include "rtos.h"
+#include "PID.h"
+
+#include "protocol.h"
+#include "packet_parser.h"
+#include "sensors.h"
+
+#define MOTOR_RIGHT 1
+#define MOTOR_LEFT 0
+
+class Control {
+
+  public:
+  
+    Control(
+      PinName left_mot_0_pin, PinName left_mot_1_pin, 
+      PinName right_mot_0_pin, PinName right_mot_1_pin, 
+      Sensors *sensors, uint32_t tick_per_rev,
+      float kP, float kI, float kD, float period, float velocity_max, float pid_dead_band
+    );
+
+    void set_setpoints(float left, float right);
+  
+    void fill_pid_packet(packet_t* pkt);
+    
+    void fill_sensor_packet(packet_t* pkt);
+
+  private:
+    
+    Sensors* sensors_;
+    
+    RtosTimer control_timer_;
+
+    PID* pids_[2];
+
+    void pid_init(int motor, float kP, float kI, float kD, float period, float velocity_max);
+
+    void set_motor_pwm(int motor, float value);
+   
+    static void control_helper(const void* p);
+    void control_update(void);
+    
+    PwmOut* motors_[2][2];
+
+    float tick_to_angular_velocity_;
+    float pid_dead_band_;
+
+    float last_positions_[2];
+    float velocities_[2];
+    float pwms_[2];
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,143 @@
+#include "mbed.h"
+
+#include "protocol.h"
+#include "packet_parser.h"
+#include "sensors.h"
+#include "control.h"
+
+#define PID_KP      1.0f
+#define PID_KI      0.1f
+#define PID_KD      0.0f
+
+#define PID_PERIOD  0.01f
+
+#define PID_IN_MIN  (-50.0f)
+#define PID_IN_MAX  50.0f
+#define PID_OUT_MIN (-1.0f)
+#define PID_OUT_MAX 1.0f
+
+#define TICK_PER_REV 1200
+
+#define PID_DEAD_BAND 0.03f
+
+#define SERIAL_BAUDRATE 230400
+
+#define VOLTAGE_PIN   p15
+
+#define L_ENC_A_PIN   p29
+#define L_ENC_B_PIN   p30
+#define R_ENC_A_PIN   p11
+#define R_ENC_B_PIN   p12
+
+#define IMU_SDA_PIN   p9
+#define IMU_SCL_PIN   p10
+
+#define L_MOT_0_PIN   p21
+#define L_MOT_1_PIN   p22
+#define R_MOT_0_PIN   p23
+#define R_MOT_1_PIN   p24
+
+void fill_time_packet(packet_t* pkt, uint32_t time) {
+  pkt->header.type = PKT_TYPE_TIME;
+  pkt->header.length = sizeof(header_t) + sizeof(time_data_t) + 1;
+  time_data_t* time_data = (time_data_t*)pkt->data_crc;
+  time_data->time = time;
+}
+
+extern "C" void mbed_reset();
+
+int main() {
+  
+  DigitalOut led1(LED1);
+  DigitalOut led4(LED4);
+  
+  led1 = 1;
+
+  Timer system_timer;
+
+  system_timer.start();
+  uint32_t last_time = system_timer.read_ms();
+  uint32_t current_time = last_time;
+  
+  PacketParser parser(SERIAL_BAUDRATE, USBTX, USBRX, LED2, LED3);
+
+  packet_union_t* recv_pkt = NULL;
+  packet_union_t* send_pkt = NULL;
+  command_data_t* command;
+
+  send_pkt = parser.get_send_packet();
+  if (send_pkt != NULL) {
+    fill_time_packet(&(send_pkt->packet), system_timer.read_us());
+    parser.send_packet(send_pkt);
+  }
+  
+  Sensors sensors(
+    &system_timer,
+    VOLTAGE_PIN,
+    L_ENC_A_PIN, L_ENC_B_PIN,
+    R_ENC_A_PIN, R_ENC_B_PIN, TICK_PER_REV,
+    IMU_SDA_PIN, IMU_SCL_PIN
+  );
+  
+  Control control(
+    L_MOT_0_PIN, L_MOT_1_PIN, R_MOT_0_PIN, R_MOT_1_PIN,
+    &sensors, TICK_PER_REV,
+    PID_KP, PID_KI, PID_KD, PID_PERIOD, PID_IN_MAX, PID_DEAD_BAND
+  );
+ 
+  led4 = 1;
+
+  packet_union_t* sensor_pkt = parser.get_send_packet();
+
+  while(1) {
+     
+    recv_pkt = parser.get_received_packet();
+
+    if (recv_pkt != NULL) {
+      
+      switch (recv_pkt->packet.header.type) {
+        
+        case PKT_TYPE_RESET:
+          mbed_reset();
+          break;
+        
+        case PKT_TYPE_COMMAND:
+          command = (command_data_t*)recv_pkt->packet.data_crc;
+          control.set_setpoints(command->left, command->right);
+          break;
+
+        case PKT_TYPE_TIME:
+          send_pkt = parser.get_send_packet();
+          if (send_pkt != NULL) {
+            fill_time_packet(&(send_pkt->packet), system_timer.read_us());
+            parser.send_packet(send_pkt);
+          }
+          break;
+        
+        case PKT_TYPE_READ:
+          if (sensor_pkt != NULL) {
+            if(sensors.fill_sensor_packet(&(sensor_pkt->packet))) {
+              control.fill_sensor_packet(&(sensor_pkt->packet));
+              parser.send_packet(sensor_pkt);
+              sensor_pkt = parser.get_send_packet();
+            }
+          } else {
+            sensor_pkt = parser.get_send_packet();
+          }
+          break;
+      }
+
+      parser.free_received_packet(recv_pkt);
+    }
+    
+    current_time = system_timer.read_ms();
+
+    if (current_time - last_time > 500) {
+      last_time = current_time;
+      led1 = !led1;
+      led4 = !led4;
+    }
+  
+    Thread::yield();
+  }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/mbed_official/code/mbed-rtos/#53ace74b190c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-src.lib	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-src/#a11c0372f0ba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/packet_parser.cpp	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,137 @@
+#include "packet_parser.h"
+
+
+Thread* global_thread_ = NULL;
+
+void dma_complete_signal(MODSERIAL_IRQ_INFO *q) {
+  if (global_thread_ != NULL) {
+    global_thread_->signal_set(DMA_COMPLETE_FLAG);
+  }
+}
+
+PacketParser::PacketParser(
+  uint32_t baudrate, PinName tx_pin, PinName rx_pin, PinName tx_led, PinName rx_led) :
+  pc_(tx_pin, rx_pin), dma_(),
+  tx_led_(tx_led), send_thread_(&PacketParser::thread_starter, this),
+  rx_led_(rx_led) {
+  
+  pc_.baud(baudrate);
+  pc_.MODDMA(&dma_);
+  //pc_.attach_dmaSendComplete(this, &PacketParser::send_complete);
+  pc_.attach_dmaSendComplete(&dma_complete_signal);
+  global_thread_ = &send_thread_;
+
+  out_pkt_ = NULL;
+  tx_sequence_ = 0;
+
+  pc_.attach(this, &PacketParser::receive_callback, MODSERIAL::RxIrq);
+  in_pkt_ = (packet_union_t*)in_box_.alloc();
+  in_pkt_idx_ = 0;
+  in_pkt_len_ = MAX_PACKET_LENGTH;
+  in_pkt_crc_ = 0;
+  
+  send_thread_.signal_set(START_THREAD_FLAG);
+}
+
+packet_union_t* PacketParser::get_received_packet(void) {
+  osEvent evt = in_box_.get(0);
+  if (evt.status == osEventMail) {
+    return (packet_union_t*)evt.value.p;
+  } else {
+    return NULL;
+  }
+}
+
+void PacketParser::free_received_packet(packet_union_t* packet) {
+  in_box_.free(packet);
+}
+
+packet_union_t* PacketParser::get_send_packet(void) {
+  return (packet_union_t*)out_box_.alloc();
+}
+
+void PacketParser::send_packet(packet_union_t* packet) {
+  out_box_.put(packet);
+}
+
+void PacketParser::thread_starter(void const *p) {
+  PacketParser* instance = (PacketParser*)p;
+  instance->send_worker();
+}
+
+void PacketParser::send_worker(void) {
+  send_thread_.signal_wait(START_THREAD_FLAG);
+  while(true) {
+    osEvent evt = out_box_.get();
+    if (evt.status == osEventMail) {
+      tx_led_ = 1;
+      out_pkt_ = (packet_union_t*)evt.value.p;
+      out_pkt_->packet.header.start = 0;
+      out_pkt_->packet.header.sequence = tx_sequence_++;
+      uint8_t crc_value = calculate_crc8(out_pkt_->raw, out_pkt_->packet.header.length-1);
+      out_pkt_->raw[out_pkt_->packet.header.length-1] = crc_value;
+      pc_.dmaSend(out_pkt_->raw, out_pkt_->packet.header.length);
+      tx_led_ = 0;
+      send_thread_.signal_wait(DMA_COMPLETE_FLAG);
+      tx_led_ = 1;
+      send_thread_.signal_clr(DMA_COMPLETE_FLAG);
+      out_box_.free(out_pkt_);
+      out_pkt_ = NULL;
+      tx_led_ = 0;
+    }
+    Thread::yield();
+  }
+}
+
+void PacketParser::send_complete(MODSERIAL_IRQ_INFO *q) {
+  tx_led_ = 1;
+  if (out_pkt_ != NULL) {
+    out_box_.free(out_pkt_);
+    out_pkt_ = NULL;
+  }
+  tx_led_ = 0;
+}
+
+void PacketParser::receive_callback(MODSERIAL_IRQ_INFO *q) {
+  rx_led_ = 1;
+  MODSERIAL* serial = q->serial;
+
+  if (in_pkt_ != NULL) {
+    while(serial->readable()) {
+      char c = serial->getc();
+
+      // If we just received the second character, set packet length
+      if (in_pkt_idx_ == 1) {
+        in_pkt_len_ = c;
+      }
+
+      // If there has been a parse error, reset packet buffer
+      if ((in_pkt_idx_ == 0 && c != 0) || in_pkt_len_ < sizeof(header_t)+1 ) {
+        in_pkt_idx_ = 0;
+        in_pkt_len_ = MAX_PACKET_LENGTH;
+        in_pkt_crc_ = 0;
+
+      // Store byte in packet buffer and update CRC
+      } else {
+        in_pkt_->raw[in_pkt_idx_++] = c;
+        in_pkt_crc_ = update_crc8(in_pkt_crc_, c);
+      }
+
+      // If we just received the last character, put valid packets in mailbox
+      // and reset packet buffer
+      if (in_pkt_idx_ == in_pkt_len_) {
+        if (in_pkt_crc_ == 0) {
+          in_box_.put(in_pkt_);
+          in_pkt_ = (packet_union_t*)in_box_.alloc();
+        }
+        in_pkt_idx_ = 0;
+        in_pkt_len_ = MAX_PACKET_LENGTH;
+        in_pkt_crc_ = 0;
+      }
+    }
+  } else {
+    in_pkt_ = (packet_union_t*)in_box_.alloc();
+  }
+
+  rx_led_ = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/packet_parser.h	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,90 @@
+#ifndef PACKET_PARSER_H
+#define PACKET_PARSER_H
+
+#include "mbed.h"
+
+#include "MODDMA.h"
+#include "MODSERIAL.h"
+#include "rtos.h"
+#include "lib_crc.h"
+
+#include "protocol.h"
+
+/**
+ * Defines the number of packets in the incoming and outgoing buffers.
+ */
+#define PACKET_BUFFER_LENGTH 4
+
+/**
+ * Thread flag to start work.
+ */
+#define START_THREAD_FLAG (1<<0)
+#define DMA_COMPLETE_FLAG (1<<1)
+
+class PacketParser {
+  
+  public:
+
+    /**
+     * Constructor.
+     *
+     * Creates a packet parsing protocol on the USB serial connection.
+     *
+     */
+    PacketParser(uint32_t baudrate, PinName tx_pin, PinName rx_pin, PinName tx_led, PinName rx_led);
+
+    /**
+     * Get a pointer to the next received packet, or NULL if there is no packet.
+     */
+    packet_union_t* get_received_packet(void);
+
+    /**
+     * Return a received packet to the packet pool. Must be called after using
+     * a packet from get_received_packet.
+     *
+     * @param packet - pointer to packet to be freed.
+     */
+    void free_received_packet(packet_union_t* packet);
+
+    /**
+     * Get a pointer to a packet to be sent. Will return NULL if there are no
+     * available outgoing packets.
+     */
+    packet_union_t* get_send_packet(void);
+
+    /**
+     * Send the packet returned by get_send_packet.
+     *
+     * @param packet - pointer to packet to be sent.
+     */
+    void send_packet(packet_union_t* packet);
+
+  private:
+
+    MODSERIAL pc_;
+    MODDMA dma_;
+    
+    DigitalOut tx_led_;
+    uint32_t tx_sequence_;
+
+    Mail<packet_union_t, PACKET_BUFFER_LENGTH> out_box_;
+    packet_union_t* out_pkt_;
+    
+    static void thread_starter(void const *p);
+    Thread send_thread_;
+    
+    void send_worker(void);
+    void send_complete(MODSERIAL_IRQ_INFO *q);
+
+    DigitalOut rx_led_;
+    Mail<packet_union_t, PACKET_BUFFER_LENGTH> in_box_;
+    
+    packet_union_t* in_pkt_;
+    uint32_t in_pkt_idx_;
+    uint32_t in_pkt_len_;
+    uint8_t in_pkt_crc_;
+
+    void receive_callback(MODSERIAL_IRQ_INFO *q);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/protocol.h	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,68 @@
+#ifndef PROTOCOL_H
+#define PROTOCOL_H
+
+#include <stdint.h>
+/**
+ * Packet type characters.
+ */
+#define PKT_TYPE_COMMAND  'C'
+#define PKT_TYPE_SENSOR   'S'
+#define PKT_TYPE_READ     'G'
+#define PKT_TYPE_RESET    'R'
+#define PKT_TYPE_TIME     'T'
+#define PKT_TYPE_PID      'P'
+
+/**
+ * Defines the total maximum size of a packet, including header
+ */
+#define MAX_PACKET_LENGTH 256
+
+/**
+ * Packet structure definitions
+ */
+typedef struct header_t {
+  uint8_t start;
+  uint8_t length;
+  char type;
+  uint8_t flags;
+  uint32_t sequence;
+} header_t;
+
+typedef struct packet_t {
+  header_t header;
+  uint8_t data_crc[MAX_PACKET_LENGTH-sizeof(header_t)];
+} packet_t;
+
+typedef union packet_union_t {
+  packet_t packet;
+  char raw[MAX_PACKET_LENGTH];
+} packet_union_t;
+
+typedef struct command_data_t {
+  float left;
+  float right;
+} command_data_t;
+
+typedef struct sensor_data_t {
+  uint32_t time;
+  float accel[3];
+  float gyro[3];
+  int32_t encoder[2];
+  float velocity[2];
+  float voltage;
+} sensor_data_t;
+
+typedef struct read_data_t {
+  int32_t period;
+} read_data_t;
+
+typedef struct time_data_t {
+  uint32_t time;
+} time_data_t;
+
+typedef struct pid_data_t {
+  float vel[2];
+  float pwm[2];
+} pid_data_t;
+
+#endif
\ No newline at end of file

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors.h	Tue May 31 17:04:59 2016 +0000
@@ -0,0 +1,39 @@
+#ifndef SENSORS_H
+#define SENSORS_H
+
+#include "mbed.h"
+
+#include "MPU6050.h"
+#include "QEI.h"
+
+#include "protocol.h"
+
+class Sensors {
+  
+  public:
+    
+    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);
+    
+    float get_voltage(void);
+    void get_encoders(int32_t (&encoders)[2]);
+    void get_angles(float* angles);
+    bool get_imu(sensor_data_t* sensor_data);
+
+    bool fill_sensor_packet(packet_t* packet);
+  
+  private:
+    
+    Timer* system_timer_;
+    AnalogIn voltage_pin_;
+    QEI left_qei_;
+    QEI right_qei_;
+    MPU6050 mpu6050_;
+    bool imu_ready_;
+};
+
+#endif
\ No newline at end of file