diff --git a/kilolib.c b/kilolib.c
index aa8717d..9942c3b 100644
--- a/kilolib.c
+++ b/kilolib.c
@@ -195,6 +195,7 @@ void kilo_loop(void (*program)(void)) {
             case MOVING:
                 if (cur_motion == MOVE_STOP) {
                     set_motors(0,0);
+                    prev_motion = MOVE_STOP;
                 } else {
                     if (cur_motion != prev_motion) {
                         prev_motion = cur_motion;
@@ -207,6 +208,7 @@ void kilo_loop(void (*program)(void)) {
                             _delay_ms(15);
                             set_motors(0, kilo_turn_right);
                         } else {
+                            set_motors(0, 0xFF);
                             set_motors(0xFF, 0xFF);
                             _delay_ms(15);
                             set_motors(kilo_straight_left, kilo_straight_right);
@@ -264,7 +266,6 @@ static inline void process_message() {
             }
             break;
         case CALIB:
-            reset();
             switch(calibmsg->mode) {
                 case CALIB_SAVE:
                     if (kilo_state == MOVING) {
@@ -285,23 +286,23 @@ static inline void process_message() {
                 case CALIB_TURN_LEFT:
                     if (cur_motion != MOVE_LEFT || kilo_turn_left != calibmsg->turn_left) {
                         prev_motion = MOVE_STOP;
-                        kilo_turn_left = calibmsg->turn_left;
                         cur_motion = MOVE_LEFT;
+                        kilo_turn_left = calibmsg->turn_left;
                     }
                     break;
                 case CALIB_TURN_RIGHT:
                     if (cur_motion != MOVE_RIGHT || kilo_turn_right != calibmsg->turn_right) {
                         prev_motion = MOVE_STOP;
-                        kilo_turn_right = calibmsg->turn_right;
                         cur_motion = MOVE_RIGHT;
+                        kilo_turn_right = calibmsg->turn_right;
                     }
                     break;
                 case CALIB_STRAIGHT:
                     if (cur_motion != MOVE_STRAIGHT || kilo_straight_right != calibmsg->straight_right || kilo_straight_left != calibmsg->straight_left) {
                         prev_motion = MOVE_STOP;
+                        cur_motion = MOVE_STRAIGHT;
                         kilo_straight_left = calibmsg->straight_left;
                         kilo_straight_right = calibmsg->straight_right;
-                        cur_motion = MOVE_STRAIGHT;
                     }
                     break;
             }
diff --git a/ohc.c b/ohc.c
index e77256b..274884b 100644
--- a/ohc.c
+++ b/ohc.c
@@ -2,11 +2,10 @@
 #include <avr/interrupt.h> // for ISR
 #include <util/delay.h>    // for _delay_ms
 #include <string.h>        // for memcpy
-#include "message.h"
+#include "ohc.h"           // for message definitions
 #include "message_crc.h"
 #include "message_send.h"
 #include "bootldr.h"
-#include "ohc.h"
 
 uint8_t packet_buffer[PACKET_SIZE];
 uint8_t packet_head = 0;
diff --git a/ohc.h b/ohc.h
index cc53791..aac9111 100644
--- a/ohc.h
+++ b/ohc.h
@@ -2,6 +2,21 @@
 #define __OHC_H__
 
 #include <stdint.h>
+#include "message.h"
+
+// 01010101
+#define PACKET_HEADER 0x55
+#define PAGE_SIZE 128
+#define PACKET_SIZE (PAGE_SIZE+4)
+enum {
+    PACKET_STOP,
+    PACKET_LEDTOGGLE,
+    PACKET_FORWARDMSG,
+    PACKET_FORWARDRAWMSG,
+    PACKET_BOOTPAGE,
+    PACKET_GPSFRAME,
+    PACKET_FORWARDMSGSINGLE,
+};
 
 typedef struct {
     uint16_t id;
@@ -29,18 +44,4 @@ enum {
     CALIB_STRAIGHT
 };
 
-// 01010101
-#define PACKET_HEADER 0x55
-#define PAGE_SIZE 128
-#define PACKET_SIZE (PAGE_SIZE+4)
-enum {
-    PACKET_STOP,
-    PACKET_LEDTOGGLE,
-    PACKET_FORWARDMSG,
-    PACKET_FORWARDRAWMSG,
-    PACKET_BOOTPAGE,
-    PACKET_GPSFRAME,
-    PACKET_FORWARDMSGSINGLE,
-};
-
 #endif//__OHC_H__