Changed Arduino Code to support multiple commands simultaneously. Working on ROS node.

config/qt_pi_arduino_params.yaml

@@ -1,5 +1,5 @@
 
-port: /dev/rfcomm1
+port: /dev/ttyUSB0
 baud: 57600
 timeout: 0.1
 

include/qt_pi/commands.h

@@ -0,0 +1,35 @@
+#ifndef  COMMANDS_H
+#define  COMMANDS_H
+
+#define INVALID            0xFF
+#define INVALID_LENGTH     0xFE
+
+#define GET_BAUDRATE       0x51
+#define GET_ENCODERS       0x52
+#define SET_SPEEDS         0x53
+#define RESET_ENCODERS     0x54
+#define SET_PID_L          0x55
+#define SET_PID_R          0x56
+#define GET_PID_P          0x57
+#define GET_BATTERY_LVL    0x58
+#define EN_IMU             0x59
+#define EN_ODOM            0x60
+#define ODOM_DATA          0x61
+#define IMU_DATA           0x62
+
+#define GET_BAUDRATE_LEN    0x01
+#define GET_ENCODERS_LEN    0x01
+#define SET_SPEEDS_LEN      0x03
+#define RESET_ENCODERS_LEN  0x01
+#define SET_PID_LEN         0x05
+#define GET_PID_P_LEN       0x01
+#define GET_BATTERY_LVL_LEN 0x01
+#define EN_IMU_LEN          0x02
+#define EN_ODOM_LEN         0x02
+#define ODOM_DATA_LEN       0x08
+#define IMU_DATA_LEN        0x08
+
+#define LEFT                0x00
+#define RIGHT               0x01
+
+#endif

include/qt_pi/payload_specs.h

@@ -0,0 +1,27 @@
+#ifndef PAYLOAD_SPECS_H
+#define PAYLOAD_SPECS_H
+
+#define MB1P              0
+#define MB2P              1
+#define LB1P              2
+#define LB2P              3
+#define CIDBP             4
+#define COMBP             5
+#define DATASTRBP         6
+
+#define start_magic_len   2
+#define end_magic_len     1
+#define len_len           2
+
+#define BID               0xFF
+
+#define CIDB              0
+#define COMB              1
+#define DATASTRB          2
+
+const int com_preamble_len = 2;
+
+const int preamble_len  = start_magic_len + len_len;
+const int net_extra_len = preamble_len + end_magic_len;
+
+#endif

include/qt_pi/serial_driver.h

@@ -0,0 +1,9 @@
+#include <errno.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+#include "commands.h"
+#include "payload_specs.h"

launch/depth_to_laser.launch

@@ -0,0 +1,7 @@
+<launch>
+<remap from="image" to="kinect2/sd/image_depth_rect"/>
+   <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
+	  <param name="scan_height" value="1" />
+	  <param name="range_max" value="12" />
+   </node>
+</launch>

launch/gmapping.launch

@@ -0,0 +1,8 @@
+<launch>
+	<node name="gmapping" pkg="gmapping" type="slam_gmapping">
+	<param name="base_frame" value="base_link" type="string" />
+	<param name="maxUrange" value="8" />
+	<param name="maxRange" value="12" />
+	<param name="minimumScore" value="50" />
+	</node>
+</launch>

launch/qt_pi.launch

@@ -1,17 +1,10 @@
 <launch>
-   <node name="arduino" pkg="qt_pi" type="arduino_node.py" output="screen">
+	
-      <rosparam file="$(find qt_pi)/config/qt_pi_arduino_params.yaml" command="load" />
+	<include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch"/>
-   </node>
+	<include file="$(find qt_pi)/launch/arduino.launch"/>	
+	<include file="$(find qt_pi)/launch/depth_to_laser.launch"/>
+	<include file="$(find qt_pi)/launch/gmapping.launch"/>
 
-   <node name="kinect2_bridge_1455208236617866858" pkg="kinect2_bridge" type="kinect2_bridge">
-   </node>
 
-   <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
-      <param name="image" value="kinect2/sd/image_depth_rect" />
-   </node>
-
-   <node name="gmapping" pkg="gmapping" type="slam_gmapping">
-      <param name="base_frame" value="base_link" type="string" />
-   </node>
 
 </launch>

map/F404.yaml

@@ -1,7 +0,0 @@
-image: F404.pgm
-resolution: 0.050000
-origin: [-2000,-2000, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-

map/Untitled.yaml

@@ -1,7 +0,0 @@
-image: F404.pgm
-resolution: 0.050000
-origin: [10,10, 0.000000]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-

src/libraries/Arduino/Arduino.ino

@@ -1,211 +1,96 @@
-#define BAUDRATE     57600
+#include "qt_pi.h"
-#define MAX_PWM        255
 
-#if defined(ARDUINO) && ARDUINO >= 100
+#define _TASK_SLEEP_ON_IDLE_RUN
-#include "Arduino.h"
+#define _TASK_INLINE
-#else
+#define _TASK_TIMEOUT
-#include "WProgram.h"
-#endif
-#include "commands.h"
-#include "motor_driver.h"
-#include "encoder_driver.h"
-#include "diff_controller.h"
 
-float batt_lvl;
+#include <TaskScheduler.h>
 
-#define PID_RATE           30     // Hz
+Task *batt_mon;
-const int PID_INTERVAL = 1000 / PID_RATE;
+Task *diff_control;
-unsigned long nextPID = PID_INTERVAL;
+Task *move_cmd_timeout;
-#define AUTO_STOP_INTERVAL 2000
+Task *odom_out;
-long lastMotorCommand = AUTO_STOP_INTERVAL;
+Task *imu_out;
-int arg = 0;
+Task *ser_comm_timeout;
-int index = 0;
-char chr;
-char cmd;
-char argv1[16];
-char argv2[16];
-long arg1;
-long arg2;
 
-void resetCommand() {
+Scheduler ts;
-  cmd = NULL;
-  memset(argv1, 0, sizeof(argv1));
-  memset(argv2, 0, sizeof(argv2));
-  arg1 = 0;
-  arg2 = 0;
-  arg = 0;
-  index = 0;
-}
 
-int runCommand() {
-  int i = 0;
-  char *p = argv1;
-  char *str;
-  int pid_args[4];
-  arg1 = atoi(argv1);
-  arg2 = atoi(argv2);
-
-  switch(cmd) {
-  case GET_BAUDRATE:
-    Serial.println(BAUDRATE);
-    break;
-
-  case READ_ENCODERS:
-    Serial.print(readEncoder(LEFT));
-    Serial.print(" ");
-    Serial.println(readEncoder(RIGHT));
-    break;
-
-   case RESET_ENCODERS:
-    resetEncoders();
-    resetPID();
-    Serial.println("OK");
-    break;
-
-  case MOTOR_SPEEDS:
-    /* Reset the auto stop timer */
-    lastMotorCommand = millis();
-    if (arg1 == 0 && arg2 == 0) {
-      setMotorSpeeds(0, 0);
-      resetPID();
-      moving = 0;
-    }
-    else moving = 1;
-    leftPID.TargetTicksPerFrame = arg1;
-    rightPID.TargetTicksPerFrame = arg2;
-    Serial.println("OK");
-    break;
-
-  case UPDATE_PID_L:
-    while ((str = strtok_r(p, ":", &p)) != '\0') {
-       pid_args[i] = atoi(str);
-       i++;
-    }
-    leftPID.Kp = pid_args[0];
-    leftPID.Kd = pid_args[1];
-    leftPID.Ki = pid_args[2];
-    leftPID.Ko = pid_args[3];
-    Serial.println("OK");
-    break;
-
-  case UPDATE_PID_R:
-    while ((str = strtok_r(p, ":", &p)) != '\0') {
-       pid_args[i] = atoi(str);
-       i++;
-    }
-    rightPID.Kp = pid_args[0];
-    rightPID.Kd = pid_args[1];
-    rightPID.Ki = pid_args[2];
-    rightPID.Ko = pid_args[3];
-    Serial.println("OK");
-    break;
-
-  case DISP_PID_P:
-    Serial.println(leftPID.Kp);
-    Serial.println(leftPID.Kd);
-    Serial.println(leftPID.Ki);
-    Serial.println(leftPID.Ko);
-    Serial.println(rightPID.Kp);
-    Serial.println(rightPID.Kd);
-    Serial.println(rightPID.Ki);
-    Serial.println(rightPID.Ko);
-    Serial.println("OK");
-    break;
-  case BATTERY_LVL:
-	  Serial.println((int)(batt_lvl * 100));
-	  break;
-  default:
-    Serial.println("Invalid Command");
-    break;
-  }
-}
-
-/* Setup function--runs once at startup. */
 void setup() {
-  Serial.begin(BAUDRATE);
+
-  
+    init_comm();
-    pinMode(A0,INPUT);
-    digitalWrite(A0,1);
-    batt_lvl = analogRead(A0);
     
-    DDRC &= ~(1<<LEFT_ENC_PIN_A);
+    init_enc();
-    DDRC &= ~(1<<LEFT_ENC_PIN_B);
+    
-    DDRD &= ~(1<<RIGHT_ENC_PIN_A);
+    initMotorController();
-    DDRD &= ~(1<<RIGHT_ENC_PIN_B);
 
-    //enable pull up resistors
+    batt_mon = new Task(BATT_INTERVAL, TASK_FOREVER, &updateBattLevel, &ts, true, &initBatt, NULL);
-    PORTC |= (1<<LEFT_ENC_PIN_A);
+    diff_control = new Task(PID_INTERVAL, TASK_FOREVER, &updatePID, &ts, true, &resetPID, NULL);
-    PORTC |= (1<<LEFT_ENC_PIN_B);
+    move_cmd_timeout = new Task(AUTO_STOP_INTERVAL, 1, &moveTimeout, &ts, true, NULL, NULL);
-    PORTD |= (1<<RIGHT_ENC_PIN_A);
+    odom_out = new Task(ODOM_INTERVAL, TASK_FOREVER, &sendOdom, &ts, false, NULL, NULL);
-    PORTD |= (1<<RIGHT_ENC_PIN_B);
+    imu_out = new Task(IMU_INTERVAL, TASK_FOREVER, &sendIMU, &ts, false,  &initIMU, NULL);
-
+    ser_comm_timeout = new Task(AUTO_STOP_INTERVAL, 1, &serCommTimeout, &ts, true, NULL, NULL);
-    // tell pin change mask to listen to left encoder pins
-    PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
-    // tell pin change mask to listen to right encoder pins
-    PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
-
-    // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
-    PCICR |= (1 << PCIE1) | (1 << PCIE2);
-
-  initMotorController();
-  resetPID();
 
 }
 
-void loop() {
+void loop() 
+{
 
-  batt_lvl = ( analogRead(A0) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
+  ts.execute();
-
+  proc_serial();
-  while (Serial.available() > 0) {
-
-    // Read the next character
-    chr = Serial.read();
-
-    // Terminate a command with a CR
-    if (chr == 13) {
-      if (arg == 1) argv1[index] = NULL;
-      else if (arg == 2) argv2[index] = NULL;
-      runCommand();
-      resetCommand();
-    }
-    // Use spaces to delimit parts of the command
-    else if (chr == ' ') {
-      // Step through the arguments
-      if (arg == 0) arg = 1;
-      else if (arg == 1)  {
-        argv1[index] = NULL;
-        arg = 2;
-        index = 0;
-      }
-      continue;
-    }
-    else {
-      if (arg == 0) {
-        // The first arg is the single-letter command
-        cmd = chr;
-      }
-      else if (arg == 1) {
-        // Subsequent arguments can be more than one character
-        argv1[index] = chr;
-        index++;
-      }
-      else if (arg == 2) {
-        argv2[index] = chr;
-        index++;
-      }
-    }
-  }
-
-  if (millis() > nextPID) {
-    updatePID();
-    nextPID += PID_INTERVAL;
-  }
-
-  // Check to see if we have exceeded the auto-stop interval
-  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
-    setMotorSpeeds(0, 0);
-    moving = 0;
-    resetPID();
-  }
 
 }
+
+void move_cmd_timeout_reset()
+{
+//  move_cmd_timeout->resetTimeout();  
+}
+
+void enable_odom(bool a)
+{
+  switch(a)
+  {
+    case 0:
+      switch(odom_out->isEnabled())
+      {
+        case 1:
+          odom_out->disable();
+          break;
+      }
+      break;
+      
+    case 1:
+      switch(odom_out->isEnabled())
+      {
+        case 0:
+          odom_out->enable();
+          break;
+      }
+      break;
+  }
+  
+}
+
+void enable_imu(bool a)
+{
+  switch(a)
+  {
+    case 0:
+      switch(imu_out->isEnabled())
+      {
+        case 1:
+          imu_out->disable();
+          break;
+      }
+      break;
+      
+    case 1:
+      switch(imu_out->isEnabled())
+      {
+        case 0:
+          imu_out->enable();
+          break;
+      }
+      break;
+  }
+  
+}

src/libraries/Arduino/batt.cpp

@@ -0,0 +1,26 @@
+#include "qt_pi.h"
+
+float batt_lvl;
+
+void initBatt(){
+    
+    pinMode(BATT_PIN,INPUT);
+    digitalWrite(BATT_PIN,1);
+    
+    batt_lvl = analogRead(BATT_PIN);
+
+}
+
+void updateBattLevel(){
+
+    batt_lvl = ( analogRead(BATT_PIN) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
+  
+}
+
+void getBattLvl(uint8_t *buf){
+  
+    uint16_t tmp = (uint16_t)(batt_lvl * 100);
+    
+    memcpy(buf, &tmp, 2);
+    
+}

src/libraries/Arduino/batt.h

@@ -0,0 +1,12 @@
+#ifndef BATT
+#define BATT
+
+#define BATT_PIN A0
+
+const int BATT_INTERVAL = 1000;
+
+extern void initBatt();
+extern void updateBattLevel();
+extern void getBattLvl(uint8_t *);
+
+#endif

src/libraries/Arduino/comm.cpp

@@ -0,0 +1,365 @@
+#include "qt_pi.h"
+
+const uint8_t magic_start1 = 0xEE;
+const uint8_t magic_start2 = 0xAE;
+const uint8_t magic_end    = 0xFC;
+
+#define buf_size          32
+
+/*
+#define  WAITING_FOR_NEW_COM    0
+#define  FIRST_BYTE_VERIFIED    1
+#define  SECOND_BYTE_VERIFIED   2
+#define  LENGTH_INPUT           3
+#define  READING_PAYLOAD        4
+#define  READING_COMPLETE       5
+#define  COM_RECEIVED           6
+*/
+enum COMM_STATE {
+  WAITING_FOR_NEW_COM,
+  FIRST_BYTE_VERIFIED, 
+  SECOND_BYTE_VERIFIED, 
+  LENGTH_INPUT,
+  READING_PAYLOAD, 
+  READING_COMPLETE, 
+  COM_RECEIVED
+};
+
+int curState = WAITING_FOR_NEW_COM;
+
+int com_len;
+
+uint8_t in_buf[buf_size];
+int in_buf_i;
+
+uint8_t ser_write_buf[buf_size];
+uint8_t com_recv[buf_size];
+
+int init_comm(){
+   
+  ser_write_buf[MB1P] = magic_start1;
+  ser_write_buf[MB2P] = magic_start2;
+  
+  Serial.begin(BAUDRATE);
+  Serial.setTimeout(0);
+  
+  resetComm();
+  
+  curState = WAITING_FOR_NEW_COM;
+   
+}
+
+int sendCommand(int16_t len)
+{
+  if((len + net_extra_len) > buf_size)
+    return -1;
+
+  memcpy(ser_write_buf + start_magic_len, &len, len_len);
+  
+  ser_write_buf[preamble_len + len] = magic_end;
+  
+  return(Serial.write(ser_write_buf, len + net_extra_len));
+    
+}
+
+int sendData(int len, uint8_t *buf){
+
+  if((len + net_extra_len) > buf_size)
+    return -1;
+
+  memcpy(ser_write_buf + preamble_len, buf, len);
+
+  return sendCommand(len);
+
+}
+
+int runInpCommand() {
+
+  int data_len = com_len;
+  
+  switch(com_recv[COMB]) {
+    
+  case GET_BAUDRATE:
+
+    if(com_len != GET_BAUDRATE_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+
+    memcpy(ser_write_buf + DATASTRBP, &BAUDRATE, 4);
+
+    data_len = 4;
+
+    break;
+  
+  case GET_ENCODERS:
+  {
+    
+    if(com_len != GET_ENCODERS_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+    
+    long l = readEncoder(LEFT);
+    long r = readEncoder(RIGHT);
+  
+    memcpy(ser_write_buf + DATASTRBP, &l, 4);
+    memcpy(ser_write_buf + DATASTRBP + 4, &r, 4);
+  
+    data_len = 8;
+     
+    break;
+  }
+  
+  case RESET_ENCODERS:
+    
+    if(com_len != RESET_ENCODERS_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+
+    resetEncoders();
+    resetPID();
+    
+    data_len = 0;
+    
+    break;
+  
+  case SET_SPEEDS:
+
+    if(com_len != SET_SPEEDS_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+    
+    move_cmd_timeout_reset();
+    
+    if (com_recv[DATASTRB] == 0 && com_recv[DATASTRB + 1] == 0) {
+      
+      setMotorSpeeds(0, 0);
+      
+      resetPID();
+      
+      setMoving(false);
+      
+    }
+    else
+    {
+      setMoving(true);
+    }
+    
+    setTicksPerFrame(com_recv + DATASTRB);
+    
+    data_len = 0;
+    
+    break;
+  
+  case SET_PID_L:
+
+    if(com_len != SET_PID_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+  
+    setPID_L(com_recv + DATASTRB);
+    
+    data_len = 0;
+    
+    break;
+  
+  case SET_PID_R:
+
+    if(com_len != SET_PID_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+    
+    setPID_R(com_recv + DATASTRB);
+    
+    data_len = 0;
+    
+    break;
+  
+  case GET_PID_P:
+    
+    if(com_len != GET_PID_P_LEN)
+    {
+      data_len = -1;
+      break;
+    }  
+       
+    getPID(ser_write_buf + DATASTRBP);
+  
+    data_len = 8;
+    
+    break;
+  
+  case GET_BATTERY_LVL:  
+
+    if(com_len != GET_BATTERY_LVL_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+    
+    getBattLvl(ser_write_buf + DATASTRBP);
+    
+    data_len = 2;
+  
+    break;
+  
+  case EN_IMU:
+
+    if(com_len != EN_IMU_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+       
+    enable_imu(com_recv[DATASTRB]);
+    
+    ser_write_buf[DATASTRBP] = 1;
+    data_len = 1;
+
+    break;
+  
+  case EN_ODOM:
+    
+    if(com_len != EN_ODOM_LEN)
+    {
+      data_len = -1;
+      break;
+    }
+    
+    enable_odom(com_recv[DATASTRB]);   
+
+    ser_write_buf[DATASTRBP] = 1;
+    data_len = 1;
+    
+    break;
+
+  
+  default:
+    
+    ser_write_buf[DATASTRBP] = com_recv[COMB];
+    com_recv[COMB] = INVALID;
+    data_len = 1;
+    
+    break;
+  }
+
+  if(data_len == -1)
+  {
+    ser_write_buf[COMBP] = INVALID_LENGTH;
+    ser_write_buf[CIDBP] = com_recv[CIDB];
+    ser_write_buf[DATASTRBP] = com_recv[COMB];
+    data_len = 1;
+  }
+  else
+  {
+    ser_write_buf[COMBP] = com_recv[COMB];
+    ser_write_buf[CIDBP] = com_recv[CIDB];
+  }
+  
+  sendCommand(data_len + com_preamble_len);
+
+}
+
+void resetComm(){
+  com_len = 1;
+  in_buf_i = 0;
+}
+
+uint8_t temp_buf[64];
+void serCommTimeout(){
+  Serial.readBytes(temp_buf, 64);
+}
+
+void proc_serial(){
+  
+  if(!Serial.available())
+    return;
+
+  move_cmd_timeout_reset();
+  
+  switch(curState){
+    
+      case WAITING_FOR_NEW_COM:
+
+          if(Serial.read() == magic_start1)
+          {
+            curState = FIRST_BYTE_VERIFIED;
+          }
+          
+          break;
+          
+      case FIRST_BYTE_VERIFIED:
+
+          if(Serial.read() == magic_start2)
+          {
+            curState = SECOND_BYTE_VERIFIED;
+          }
+          else
+          {
+            curState = WAITING_FOR_NEW_COM;
+          }
+          
+          break;
+          
+      case SECOND_BYTE_VERIFIED:
+
+          com_len = Serial.read();
+          curState = LENGTH_INPUT;
+
+          break;
+          
+      case LENGTH_INPUT:
+
+          com_len = com_len | (Serial.read() << 8);
+
+          if(com_len < 20)
+          {
+            curState = READING_PAYLOAD;
+          }
+          else
+          {
+            curState = WAITING_FOR_NEW_COM;
+          }
+              
+          break;
+          
+      case READING_PAYLOAD:
+
+          in_buf_i += Serial.readBytes(&in_buf[in_buf_i], com_len - in_buf_i);
+
+          if(in_buf_i == com_len)
+          {
+            curState = READING_COMPLETE;
+          }
+          break;
+          
+      case READING_COMPLETE:
+
+          if(Serial.read() == magic_end){
+
+              memcpy(com_recv, in_buf, com_len);
+          
+              runInpCommand();
+
+              curState = WAITING_FOR_NEW_COM;
+          }
+          
+          resetComm();
+          
+          break;
+      }
+    
+    if((Serial.available() > 0) && curState != WAITING_FOR_NEW_COM)
+      proc_serial();
+  }

src/libraries/Arduino/comm.h

@@ -0,0 +1,12 @@
+#ifndef COMM_H
+#define COMM_H
+
+const unsigned long BAUDRATE = 115200;
+
+extern int init_comm();
+extern void proc_serial();
+extern int sendData(int, uint8_t *buf);
+extern void serCommTimeout();
+extern void resetComm();
+
+#endif

src/libraries/Arduino/commands.h

@@ -1,16 +1,35 @@
 #ifndef  COMMANDS_H
 #define  COMMANDS_H
 
-#define  GET_BAUDRATE   'b'
+#define INVALID            0xFF
-#define  READ_ENCODERS  'e'
+#define INVALID_LENGTH     0xFE
-#define  MOTOR_SPEEDS   'm'
+
-#define  RESET_ENCODERS 'r'
+#define GET_BAUDRATE       0x51
-#define  UPDATE_PID_L   'L'
+#define GET_ENCODERS       0x52
-#define  UPDATE_PID_R   'R'
+#define SET_SPEEDS         0x53
-#define  DISP_PID_P     'z'
+#define RESET_ENCODERS     0x54
-#define  BATTERY_LVL    'B'
+#define SET_PID_L          0x55
-#define  LEFT            0
+#define SET_PID_R          0x56
-#define  RIGHT           1
+#define GET_PID_P          0x57
+#define GET_BATTERY_LVL    0x58
+#define EN_IMU             0x59
+#define EN_ODOM            0x60
+#define ODOM_DATA          0x61
+#define IMU_DATA           0x62
+
+#define GET_BAUDRATE_LEN    0x01
+#define GET_ENCODERS_LEN    0x01
+#define SET_SPEEDS_LEN      0x03
+#define RESET_ENCODERS_LEN  0x01
+#define SET_PID_LEN         0x05
+#define GET_PID_P_LEN       0x01
+#define GET_BATTERY_LVL_LEN 0x01
+#define EN_IMU_LEN          0x02
+#define EN_ODOM_LEN         0x02
+#define ODOM_DATA_LEN       0x08
+#define IMU_DATA_LEN        0x08
+
+#define LEFT                0x00
+#define RIGHT               0x01
 
 #endif
-

src/libraries/Arduino/diff_controller.cpp

@@ -0,0 +1,160 @@
+#include "qt_pi.h"
+
+typedef struct {
+  
+  double TargetTicksPerFrame;
+  long Encoder;
+  long PrevEnc;
+
+  int PrevInput;
+  int ITerm;
+  
+  uint8_t Kp = 45;
+  uint8_t Kd = 0;
+  uint8_t Ki = 5;
+  uint8_t Ko = 3;
+  
+  uint8_t base_pwm = 70;
+  
+  long output;
+
+}
+
+SetPointInfo;
+
+SetPointInfo leftPID, rightPID;
+
+unsigned char moving = 0;
+
+void resetPID(){
+  
+  leftPID.TargetTicksPerFrame = 0.0;
+  leftPID.Encoder = readEncoder(LEFT);
+  leftPID.PrevEnc = leftPID.Encoder;
+  leftPID.output = 0;
+  leftPID.PrevInput = 0;
+  leftPID.ITerm = 0;
+  
+  rightPID.TargetTicksPerFrame = 0.0;
+  rightPID.Encoder = readEncoder(RIGHT);
+  rightPID.PrevEnc = rightPID.Encoder;
+  rightPID.output = 0;
+  rightPID.PrevInput = 0;
+  rightPID.ITerm = 0;
+
+}
+
+void doPID(SetPointInfo * p) {
+  
+  long Perror;
+  long output;
+  int input;
+  
+  input = p->Encoder - p->PrevEnc;
+  Perror = p->TargetTicksPerFrame - input;
+  
+  p->ITerm += (p->Ki) * Perror;
+  output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
+  
+  if(p->TargetTicksPerFrame > 0)
+  {
+    output += p->base_pwm;
+  }
+  else if(p->TargetTicksPerFrame < 0) 
+  { 
+    output -= p->base_pwm;
+  }
+  
+  p->PrevEnc = p->Encoder;
+  
+  if (output >= MAX_PWM)
+  {
+    output = MAX_PWM;
+  }
+  else if (output <= -MAX_PWM)
+  {
+    output = -MAX_PWM;
+  }
+  else
+  {
+    p->output = output;
+  }
+  
+  p->PrevInput = input;
+}
+
+void updatePID() {
+
+  leftPID.Encoder = readEncoder(LEFT);
+  rightPID.Encoder = readEncoder(RIGHT);
+
+  if (!moving){
+    
+    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0)
+      resetPID();
+      
+    return;
+  }
+
+  if(leftPID.TargetTicksPerFrame != 0)
+  {
+    doPID(&leftPID);
+  }
+  
+  if(rightPID.TargetTicksPerFrame != 0)
+  {
+    doPID(&rightPID);
+  }
+  
+  setMotorSpeeds(leftPID.output, rightPID.output);
+}
+
+void setPID_L(uint8_t *buf){
+   
+  leftPID.Kp = buf[0];
+  leftPID.Kd = buf[1];
+  leftPID.Ki = buf[2];
+  leftPID.Ko = buf[3];
+
+}
+
+void setPID_R(uint8_t *buf){
+                  
+  rightPID.Kp = buf[0];
+  rightPID.Kd = buf[1];
+  rightPID.Ki = buf[2];
+  rightPID.Ko = buf[3];
+}
+
+void moveTimeout(){
+    
+  setMotorSpeeds(0, 0);
+  moving = 0;
+  resetPID();
+    
+}
+
+void setMoving(bool t){
+  moving = t;
+}
+
+void getPID(uint8_t *buf){
+  
+  buf[0] = leftPID.Kp;
+  buf[1] = leftPID.Kd;
+  buf[2] = leftPID.Ki;
+  buf[3] = leftPID.Ko;
+  
+  buf[4] = rightPID.Kp;
+  buf[5] = rightPID.Kd;
+  buf[6] = rightPID.Ki;
+  buf[7] = rightPID.Ko;
+  
+}
+
+void setTicksPerFrame(uint8_t *buf){
+
+  leftPID.TargetTicksPerFrame = buf[0];
+  rightPID.TargetTicksPerFrame = buf[1];
+
+}

src/libraries/Arduino/diff_controller.h

@@ -1,79 +1,26 @@
-typedef struct {
+#ifndef DIFF
-  double TargetTicksPerFrame;
+#define DIFF
-  long Encoder;
-  long PrevEnc;
 
-  int PrevInput;
+#define PID_RATE 30
-  
+#define AUTO_STOP_INTERVAL 2000
-  int ITerm;
-  int Kp = 45;
-  int Kd = 0;
-  int Ki = 5;
-  int Ko = 3;
-  int base_pwm = 70;
-  long output;
-}
 
-SetPointInfo;
+const int PID_INTERVAL = 1000 / PID_RATE;
 
-SetPointInfo leftPID, rightPID;
+const int MAX_PWM = 255;
+
+extern void resetPID();
+
+extern void updatePID();
+
+extern void moveTimeout();
+
+extern void setPID_R(uint8_t *);
+extern void setPID_L(uint8_t *);
+extern void getPID(uint8_t *);
+
+extern void setTicksPerFrame(uint8_t *);
+
+extern void setMoving(bool);
 
 
-unsigned char moving = 0;
+#endif
-
-void resetPID(){
-   leftPID.TargetTicksPerFrame = 0.0;
-   leftPID.Encoder = readEncoder(LEFT);
-   leftPID.PrevEnc = leftPID.Encoder;
-   leftPID.output = 0;
-   leftPID.PrevInput = 0;
-   leftPID.ITerm = 0;
-
-   rightPID.TargetTicksPerFrame = 0.0;
-   rightPID.Encoder = readEncoder(RIGHT);
-   rightPID.PrevEnc = rightPID.Encoder;
-   rightPID.output = 0;
-   rightPID.PrevInput = 0;
-   rightPID.ITerm = 0;
-}
-
-void doPID(SetPointInfo * p) {
-  long Perror;
-  long output;
-  int input;
-
-  input = p->Encoder - p->PrevEnc;
-  Perror = p->TargetTicksPerFrame - input;
-  p->ITerm += (p->Ki) * Perror;
-  output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
-  
-  if(p->TargetTicksPerFrame > 0) output += p->base_pwm;
-  else if(p->TargetTicksPerFrame < 0) output -= p->base_pwm;
-  
-  p->PrevEnc = p->Encoder;
-  
-  if (output >= MAX_PWM)
-    output = MAX_PWM;
-  else if (output <= -MAX_PWM)
-    output = -MAX_PWM;
-  else
-
-  p->output = output;
-  p->PrevInput = input;
-}
-
-void updatePID() {
-
-  leftPID.Encoder = readEncoder(LEFT);
-  rightPID.Encoder = readEncoder(RIGHT);
-
-  if (!moving){
-    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
-    return;
-  }
-
-  if(leftPID.TargetTicksPerFrame != 0) doPID(&leftPID);
-  if(rightPID.TargetTicksPerFrame != 0) doPID(&rightPID);
-  
-  setMotorSpeeds(leftPID.output, rightPID.output);
-}

src/libraries/Arduino/encoder_driver.cpp

@@ -0,0 +1,93 @@
+#include "qt_pi.h"
+
+volatile long left_enc_pos = 0L;
+volatile long right_enc_pos = 0L;
+static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
+  
+ISR (PCINT1_vect){
+  
+  static uint8_t enc_last=0;
+      
+  enc_last <<= 2;
+  enc_last |= (PINC & (3 << 2)) >> 2;
+
+  left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+
+}
+
+ISR (PCINT2_vect){
+      
+  static uint8_t enc_last=0;
+          
+  enc_last <<=2;
+  enc_last |= (PIND & (3 << 2)) >> 2;
+
+  right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
+
+}
+
+void init_enc(){
+  
+  DDRC &= ~(1<<LEFT_ENC_PIN_A);
+  DDRC &= ~(1<<LEFT_ENC_PIN_B);
+  DDRD &= ~(1<<RIGHT_ENC_PIN_A);
+  DDRD &= ~(1<<RIGHT_ENC_PIN_B);
+
+  //enable pull up resistors
+  PORTC |= (1<<LEFT_ENC_PIN_A);
+  PORTC |= (1<<LEFT_ENC_PIN_B);
+  PORTD |= (1<<RIGHT_ENC_PIN_A);
+  PORTD |= (1<<RIGHT_ENC_PIN_B);
+
+  // tell pin change mask to listen to left encoder pins
+  PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
+  // tell pin change mask to listen to right encoder pins
+  PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
+
+  // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
+  PCICR |= (1 << PCIE1) | (1 << PCIE2);
+
+}
+
+long readEncoder(int i) {
+  
+  if (i == LEFT) 
+      return left_enc_pos;
+    
+    else 
+      return right_enc_pos;
+  
+  }
+
+  void resetEncoder(int i) {
+    
+    if (i == LEFT){
+      left_enc_pos=0L;
+      return;
+    } 
+    else 
+    { 
+      right_enc_pos=0L;
+      return;
+    }
+  }
+
+void resetEncoders() {
+  
+  resetEncoder(LEFT);
+  resetEncoder(RIGHT);
+
+}
+
+void sendOdom(){
+
+  static uint8_t buf[8 + com_preamble_len];
+
+  buf[COMB] = ODOM_DATA;
+  buf[CIDB] = 0xFF;
+  
+  memcpy(buf + DATASTRB, &left_enc_pos, 4);
+  memcpy(buf + DATASTRB + 4, &right_enc_pos, 4);
+
+  sendData(buf, 8 + com_preamble_len);
+}

src/libraries/Arduino/encoder_driver.h

@@ -1,10 +1,19 @@
-  #define LEFT_ENC_PIN_A PC2
+#ifndef ENCODER_H
-  #define LEFT_ENC_PIN_B PC3
+#define ENCODER_H
-  
-  #define RIGHT_ENC_PIN_A PD2
-  #define RIGHT_ENC_PIN_B PD3
-   
-long readEncoder(int i);
-void resetEncoder(int i);
-void resetEncoders();
 
+#define LEFT_ENC_PIN_A PC2
+#define LEFT_ENC_PIN_B PC3
+  
+#define RIGHT_ENC_PIN_A PD2
+#define RIGHT_ENC_PIN_B PD3
+
+#define ODOM_RATE 50
+const int ODOM_INTERVAL = 1000 / ODOM_RATE;
+
+extern void init_enc();
+extern void resetEncoder(int);
+extern void resetEncoders();
+extern long readEncoder(int);
+extern void sendOdom();
+
+#endif

src/libraries/Arduino/encoder_driver.ino

@@ -1,43 +0,0 @@
-  volatile long left_enc_pos = 0L;
-  volatile long right_enc_pos = 0L;
-  static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
-    
-  ISR (PCINT1_vect){
-  	static uint8_t enc_last=0;
-        
-	enc_last <<=2;
-  enc_last |= (PINC & (3 << 2)) >> 2;
-  
-  	left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
-  }
-  
-  ISR (PCINT2_vect){
-        static uint8_t enc_last=0;
-          	
-	enc_last <<=2;
-  enc_last |= (PIND & (3 << 2)) >> 2;
-  
-  	right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
-  }
-  
-  long readEncoder(int i) {
-    if (i == LEFT) return left_enc_pos;
-    else return right_enc_pos;
-  }
-
-  void resetEncoder(int i) {
-    if (i == LEFT){
-      left_enc_pos=0L;
-      return;
-    } else { 
-      right_enc_pos=0L;
-      return;
-    }
-  }
-
-void resetEncoders() {
-  resetEncoder(LEFT);
-  resetEncoder(RIGHT);
-}
-
-

src/libraries/Arduino/imu.cpp

@@ -0,0 +1,43 @@
+#include "qt_pi.h"
+
+void initIMU(){
+    
+    Wire.begin();
+
+    uint8_t buf;
+
+    I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
+    
+    I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250);
+    
+    I2Cdev::writeBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2);
+
+    I2Cdev::writeBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, false);
+    
+    I2Cdev::readBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &buf);
+    
+    // use the code below to change accel/gyro offset values
+    /*
+    Serial.println("Updating internal sensor offsets...");
+    // -76  -2359 1688  0 0 0
+    Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
+    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
+    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
+    accelgyro.setXGyroOffset(220);
+    accelgyro.setYGyroOffset(76);
+    accelgyro.setZGyroOffset(-85);
+*/
+}
+
+void sendIMU() {
+
+  static uint8_t buf[14 + com_preamble_len];
+  
+  I2Cdev::readBytes(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 14, buf + DATASTRB);
+  
+  buf[COMB] = IMU_DATA;
+  buf[CIDB] = BID;
+  
+  sendData(14 + com_preamble_len, buf);
+
+}

src/libraries/Arduino/imu.h

@@ -0,0 +1,36 @@
+#ifndef IMU_H
+#define IMU_H
+
+#include "I2Cdev.h"
+#include "Wire.h"
+
+#define MPU6050_DEFAULT_ADDRESS         0x68
+#define MPU6050_RA_PWR_MGMT_1           0x6B
+#define MPU6050_PWR1_CLKSEL_BIT            2
+#define MPU6050_PWR1_CLKSEL_LENGTH         3
+#define MPU6050_CLOCK_PLL_XGYRO         0x01
+#define MPU6050_RA_GYRO_CONFIG          0x1B
+#define MPU6050_RA_ACCEL_CONFIG         0x1C
+#define MPU6050_GCONFIG_FS_SEL_BIT         4
+#define MPU6050_GCONFIG_FS_SEL_LENGTH      2
+#define MPU6050_ACONFIG_AFS_SEL_BIT        4
+#define MPU6050_ACONFIG_AFS_SEL_LENGTH     2
+#define MPU6050_GYRO_FS_250             0x00
+
+#define MPU6050_ACCEL_FS_2              0x00
+#define MPU6050_RA_PWR_MGMT_1           0x6B
+#define MPU6050_PWR1_SLEEP_BIT             6
+#define MPU6050_WHO_AM_I_BIT               6
+#define MPU6050_WHO_AM_I_LENGTH            6
+#define MPU6050_RA_WHO_AM_I             0x75
+#define MPU6050_RA_ACCEL_XOUT_H         0x3B
+
+#define IMU_RATE 100
+
+const int IMU_INTERVAL = 1000 / IMU_RATE;
+
+extern void initIMU();
+
+extern void sendIMU();
+
+#endif

src/libraries/Arduino/motor_driver.cpp

@@ -0,0 +1,72 @@
+#include "qt_pi.h"
+
+void initMotorController() {
+  
+  digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
+  digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
+  
+}
+  
+void setMotorSpeed(int i, int spd) {
+    
+  unsigned char reverse = 0;
+  
+  if (spd < 0)
+  {
+    spd = -spd;
+    reverse = 1;
+  }
+  
+  if (spd > 255)
+  {
+    spd = 255;
+  }
+  
+  if(spd == 0)           
+  { 
+    digitalWrite(LEFT_MOTOR_FORWARD, 1);
+    digitalWrite(LEFT_MOTOR_BACKWARD, 1);
+  }
+  
+  else 
+  { 
+    switch(i){
+      
+      case LEFT:
+        
+        switch(reverse)
+        {
+            case 0:  
+              analogWrite(LEFT_MOTOR_FORWARD, spd);
+              analogWrite(LEFT_MOTOR_BACKWARD, 0);
+              break;
+            case 1:
+              analogWrite(LEFT_MOTOR_FORWARD, 0);
+              analogWrite(LEFT_MOTOR_BACKWARD, spd);
+              break;
+        }
+        break;
+        
+      case RIGHT:
+        switch(reverse)
+        {
+            case 0:  
+              analogWrite(RIGHT_MOTOR_FORWARD, spd);
+              analogWrite(RIGHT_MOTOR_BACKWARD, 0);
+              break;
+            case 1:
+              analogWrite(RIGHT_MOTOR_FORWARD, 0);
+              analogWrite(RIGHT_MOTOR_BACKWARD, spd);
+              break;
+        }
+        break;
+      }
+  } 
+}
+  
+void setMotorSpeeds(int leftSpeed, int rightSpeed) {
+  
+  setMotorSpeed(LEFT, leftSpeed);
+  setMotorSpeed(RIGHT, rightSpeed);
+  
+}

src/libraries/Arduino/motor_driver.h

@@ -1,10 +1,17 @@
-  #define RIGHT_MOTOR_BACKWARD 5
+#ifndef MOTOR_H
-  #define LEFT_MOTOR_BACKWARD  6
+#define MOTOR_H
-  #define RIGHT_MOTOR_FORWARD  9
-  #define LEFT_MOTOR_FORWARD   10
-  #define RIGHT_MOTOR_ENABLE 12
-  #define LEFT_MOTOR_ENABLE 13
 
-void initMotorController();
+#define RIGHT_MOTOR_BACKWARD 5
-void setMotorSpeed(int i, int spd);
+#define LEFT_MOTOR_BACKWARD  6
-void setMotorSpeeds(int leftSpeed, int rightSpeed);
+#define RIGHT_MOTOR_FORWARD  9
+#define LEFT_MOTOR_FORWARD  10
+#define RIGHT_MOTOR_ENABLE  12
+#define LEFT_MOTOR_ENABLE   13
+
+extern void initMotorController();
+  
+extern void setMotorSpeed(int, int);
+  
+extern void setMotorSpeeds(int, int);
+
+#endif

src/libraries/Arduino/motor_driver.ino

@@ -1,32 +0,0 @@
-  void initMotorController() {
-    digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
-    digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
-  }
-  
-  void setMotorSpeed(int i, int spd) {
-    unsigned char reverse = 0;
-  
-    if (spd < 0)
-    {
-      spd = -spd;
-      reverse = 1;
-    }
-    if (spd > 255)
-      spd = 255;
-    
-    if (i == LEFT) { 
-      if(spd == 0)           { digitalWrite(LEFT_MOTOR_FORWARD, 1); digitalWrite(LEFT_MOTOR_BACKWARD, 1); }
-      else if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
-      else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
-    }
-    else if (i == RIGHT){
-      if(spd == 0)           { digitalWrite(RIGHT_MOTOR_FORWARD, 1); digitalWrite(RIGHT_MOTOR_BACKWARD, 1); }
-      else if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
-      else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
-    }
-  }
-  
-  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
-    setMotorSpeed(LEFT, leftSpeed);
-    setMotorSpeed(RIGHT, rightSpeed);
-  }

src/libraries/Arduino/payload_specs.h

@@ -0,0 +1,27 @@
+#ifndef PAYLOAD_SPECS_H
+#define PAYLOAD_SPECS_H
+
+#define MB1P              0
+#define MB2P              1
+#define LB1P              2
+#define LB2P              3
+#define CIDBP             4
+#define COMBP             5
+#define DATASTRBP         6
+
+#define start_magic_len   2
+#define end_magic_len     1
+#define len_len           2
+
+#define BID               0xFF
+
+#define CIDB              0
+#define COMB              1
+#define DATASTRB          2
+
+const int com_preamble_len = 2;
+
+const int preamble_len  = start_magic_len + len_len;
+const int net_extra_len = preamble_len + end_magic_len;
+
+#endif

src/libraries/Arduino/qt_pi.h

@@ -0,0 +1,20 @@
+#ifndef QT_PI_H
+#define QT_PI_H
+
+#include <Arduino.h>
+
+#include "commands.h"
+#include "payload_specs.h"
+
+#include "imu.h"
+#include "comm.h"
+#include "batt.h"
+#include "motor_driver.h"
+#include "encoder_driver.h"
+#include "diff_controller.h"
+
+extern void move_cmd_timeout_reset();
+extern void enable_imu(bool);
+extern void enable_odom(bool);
+
+#endif

src/serial_driver.cpp

@@ -0,0 +1,253 @@
+#include <qt_pi/serial_driver.h>
+
+int open_port(const char *port) {
+  int fd;
+
+  fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
+  if (fd == -1) {
+    perror("open_port: Unable to open - ");
+  } else
+    fcntl(fd, F_SETFL, FNDELAY);
+
+  struct termios options;
+
+  tcgetattr(fd, &options);
+
+  cfsetispeed(&options, B115200);
+  cfsetospeed(&options, B115200);
+
+  options.c_cflag |= (CLOCAL | CREAD);
+
+  options.c_cflag &= ~PARENB;
+  options.c_cflag &= ~CSIZE;
+  options.c_cflag &= ~CSTOPB;
+  options.c_cflag &= ~HUPCL;
+  options.c_cflag |= CS8;
+  options.c_cc[VTIME] = 100;
+  options.c_cc[VMIN] = 1;
+
+  if (tcsetattr(fd, TCSANOW, &options)) {
+    perror("Setting options failed.\n");
+    close(fd);
+  }
+
+  return (fd);
+}
+
+volatile int sig = 1;
+
+void runRecvCom(uint16_t com_len, unsigned char *com) {
+
+  switch (com[COMB]) {
+
+  case GET_BAUDRATE:
+
+    unsigned long baud;
+    memcpy(&baud, com + DATASTRB, 4);
+
+    BAUDRATE_CB(com[CIDB], baud);
+
+    break;
+
+  case GET_ENCODERS: {
+
+    long l, r;
+
+    memcpy(&l, com + DATASTRB, 4);
+    memcpy(&r, com + DATASTRB + 4, 4);
+
+    ENCODERS_CB(com[CIDB], l, r);
+
+    break;
+  }
+
+  case GET_PID_P:
+
+    PID_P_CB(com[CIDB], com + DATASTRB);
+
+    break;
+
+  case GET_BATTERY_LVL:
+
+    uint16_t batt_lvl;
+    memcpy(&batt_lvl, com + DATASTRB, 2);
+
+    BATT_LVL_CB(com[CIDB], batt_lvl);
+
+    break;
+
+  case IMU_DATA:
+
+    IMU_DATA_CB(com[CIDB], com + DATASTRB);
+
+    break;
+
+  case ODOM_DATA:
+
+    long l, r;
+
+    memcpy(&l, com + DATASTRB, 4);
+    memcpy(&r, com + DATASTRB + 4, 4);
+
+    ODOM_DATA_CB(com[CIDB], l, r);
+
+    break;
+  }
+}
+
+void *serialDataIn(void *vargp) {
+
+  int *fd = (int *)vargp;
+
+  const uint8_t magic_start1 = 0xEE;
+  const uint8_t magic_start2 = 0xAE;
+  const uint8_t magic_end = 0xFC;
+
+  enum PACKET_STATE {
+    WAITING_FOR_NEW_COM,
+    FIRST_BYTE_VERIFIED,
+    SECOND_BYTE_VERIFIED,
+    LENGTH_INPUT,
+    READING_PAYLOAD,
+    READING_COMPLETE,
+    COM_RECEIVED
+  };
+
+  int inL = 0;
+
+  unsigned char com_recv[25];
+  unsigned char in_buf[25];
+  unsigned char inChar;
+  int in_buf_i = 0;
+  uint16_t com_len;
+
+  PACKET_STATE curState = WAITING_FOR_NEW_COM;
+
+  while (sig) {
+
+    inL = read(*fd, &inChar, 1);
+
+    switch (curState) {
+
+    case WAITING_FOR_NEW_COM:
+
+      if (inChar == magic_start1) {
+        curState = FIRST_BYTE_VERIFIED;
+      }
+
+      break;
+
+    case FIRST_BYTE_VERIFIED:
+
+      if (inChar == magic_start2) {
+        curState = SECOND_BYTE_VERIFIED;
+      } else {
+        curState = WAITING_FOR_NEW_COM;
+      }
+
+      break;
+
+    case SECOND_BYTE_VERIFIED:
+
+      com_len = inChar;
+      curState = LENGTH_INPUT;
+
+      break;
+
+    case LENGTH_INPUT:
+
+      com_len = com_len | (inChar << 8);
+
+      if (com_len < 20) {
+        in_buf_i = 0;
+        curState = READING_PAYLOAD;
+      } else {
+        curState = WAITING_FOR_NEW_COM;
+      }
+
+      break;
+
+    case READING_PAYLOAD:
+
+      in_buf[in_buf_i] = inChar;
+      in_buf_i++;
+
+      if (in_buf_i == com_len) {
+        curState = READING_COMPLETE;
+      }
+      break;
+
+    case READING_COMPLETE:
+
+      in_buf_i = 0;
+
+      if (inChar == magic_end) {
+
+        memcpy(com_recv, in_buf, com_len);
+
+        runRecvCom(com_len, com_recv);
+      }
+
+      curState = WAITING_FOR_NEW_COM;
+
+      break;
+    }
+  }
+}
+}
+
+void hex2bin(const char *in, size_t len, unsigned char *out) {
+
+  static const unsigned char TBL[] = {
+      0,  1,  2,  3,  4,  5,  6,  7,  8,  9,  58, 59, 60, 61,
+      62, 63, 64, 10, 11, 12, 13, 14, 15, 71, 72, 73, 74, 75,
+      76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89,
+      90, 91, 92, 93, 94, 95, 96, 10, 11, 12, 13, 14, 15};
+
+  static const unsigned char *LOOKUP = TBL - 48;
+
+  const char *end = in + len;
+
+  while (in < end)
+    *(out++) = LOOKUP[*(in++)] << 4 | LOOKUP[*(in++)];
+}
+
+int main(int argc, char **argv) {
+  int l = 0;
+  int fd = open_port(argv[1]);
+  if (fd < 0)
+    return 0;
+  usleep(2000000);
+  pthread_t thread_id;
+  pthread_create(&thread_id, NULL, tRead, (void *)&fd);
+
+  unsigned char buffer[25];
+  unsigned char in_string[50];
+
+  while (sig) {
+    fgets(in_string, 50, stdin);
+    if (in_string[0] == 'q') {
+      sig = 0;
+      break;
+    }
+
+    l = strlen(in_string) / 2;
+
+    hex2bin(in_string, 2 * l, buffer);
+
+    int n = write(fd, buffer, l);
+    printf("%d written\n", n);
+    for (int i = 0; i < n; i++)
+      printf("%x ", buffer[i]);
+    printf("\n");
+  }
+
+  pthread_join(thread_id, NULL);
+  // int res = read(fd, buffer, 25);
+
+  //  if(res > 0)
+  //    for(int i = 0; i < res; i++)
+  //      printf("%x ", buffer[i]);
+  // printf("\n");
+  close(fd);
+}