diff --git a/kraftsensor.c b/kraftsensor.c
index 823daac..648840b 100644
--- a/kraftsensor.c
+++ b/kraftsensor.c
@@ -29,10 +29,7 @@ void kraftsensor_init(){
 }
 
 void do_kraftsensor(){
-    static int32_t last_read;
-    static int32_t old_value;
     uint16_t m_data[4];
-    int32_t kraftsensor_read;
 
     /* read 2 16bit values and merge to 32bit signed integer */
     readReg(1,0,2);
@@ -45,31 +42,12 @@ void do_kraftsensor(){
         int32_t tmp = (uint32_t)m_data[1]<<16;
         tmp |= m_data[0];
 
-        //kraftsensor_value = tmp;
         /* conversion magic to milliNewton */
         kraftsensor_value = (((tmp /* + 539363*/)*9.81)/177.380)+kraftsensor_zero_offset;
-
-        //if(abs(kraftsensor_read - old_value) > 10000){
-        //    if(abs(last_read - kraftsensor_read) > 10000){
-        //        kraftsensor_value = old_value;
-        //        //printf("delta: %ld\tvalue:%ld\n", kraftsensor_read - old_value, kraftsensor_read);
-        //        //printf("spike\n");
-        //    }
-        //    else{
-        //        kraftsensor_value = kraftsensor_read;
-        //        //printf("jump\n");
-        //    }
-        //}
-        //else{
-        //    kraftsensor_value = kraftsensor_read;
-        //}
-        //last_read = kraftsensor_read;
-        //old_value = kraftsensor_value;
     }
-
 }
 
-uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
+int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
 
     uint8_t breaker = timeout;
     while(!receiveOkay && breaker) { //wait for client response, time out after 1s
@@ -94,6 +72,23 @@ uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout){
     return 0;
 }
 
+int8_t wait_write(uint8_t timeout){
+    uint8_t breaker = timeout;
+    while(!receiveOkay && breaker) { //wait for client response, time out after 1s
+        breaker--;
+        _delay_ms(1);
+        if(breaker==0)
+            return -1;
+    }
+
+    if(receiveOkay) { //if this fails, there was either no response or a crc error
+        if(rxbuffer[1]&0x80) { //client responded with an error code
+            return rxbuffer[1]&0x80;
+        }
+    }
+    return 0;
+}
+
 void readReg(uint8_t slaveid, uint16_t address, uint8_t amount) {
     _delay_ms(2);
     rxbuffer[0]=slaveid;
diff --git a/kraftsensor.h b/kraftsensor.h
index b0acbe1..e1af24d 100644
--- a/kraftsensor.h
+++ b/kraftsensor.h
@@ -13,7 +13,7 @@ extern int32_t kraftsensor_zero_offset;
 void timer2_init();
 void kraftsensor_init();
 void do_kraftsensor(void);
-uint8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout);
+int8_t wait_receive(uint8_t len, uint16_t dest[], uint8_t timeout);
 void readReg(uint8_t slaveid, uint16_t address, uint8_t amount);
 void writeReg(uint8_t slaveid, uint16_t address, uint16_t value);