Diff: LambdaController.cpp

Revision:

4:c35227d14896

Parent:

3:9f80eee7aa78

Child:

5:57c6d5db3d52

--- a/LambdaController.cpp	Tue Aug 23 08:30:41 2016 +0000
+++ b/LambdaController.cpp	Wed Aug 24 01:35:09 2016 +0000
@@ -22,7 +22,7 @@
 
     while (twe_lite.readable()) {
         data = twe_lite.getc();
-        
+
         if (data == 0x3A) {
             buffer_head = 0;
         }
@@ -53,11 +53,11 @@
                 error_id = InvalidDataError;
             }
         }
-        
+
         if(data == EOF) {
             error_id = SerialBusyError;
             break;
-    	}
+        }
     }
 }
 
@@ -96,8 +96,12 @@
     uint8_t *data;
     int value;
 
-    data = &received_data[(int) id * 2 + 2];
-    value = (data[0] << 8) | (data[1]);
+    if((int)id < 4 && (int)id >= 0) {
+        data = &received_data[(int) id * 2 + 2];
+        value = (data[0] << 8) | (data[1]);
+    } else {
+        value = -1;
+    }
 
     return value;
 }
@@ -105,15 +109,21 @@
 double LambdaController::get_axis(AxisId id)
 {
     double value = 0;
+    int raw;
+    raw = get_raw_axis(id);
 
-    value = (double) (get_raw_axis(id) - axis_center[(int) id]) / 512.0;
+    if(raw != -1) {
+        value = (double) (raw - axis_center[(int) id]) / 512.0;
 
-    if (value > -axis_threshold && value < axis_threshold) {
+        if (value > -axis_threshold && value < axis_threshold) {
+            value = 0.0;
+        } else if (value < -1.0) {
+            value = -1.0;
+        } else if (value > 1.0) {
+            value = 1;
+        }
+    } else {
         value = 0.0;
-    } else if (value < -1.0) {
-        value = -1.0;
-    } else if (value > 1.0) {
-        value = 1;
     }
 
     return value;
@@ -129,7 +139,12 @@
 
 void LambdaController::initialize_axis(AxisId id)
 {
-    axis_center[(int) id] = get_raw_axis(id);
+    int raw;
+    raw = get_raw_axis(id);
+
+    if(raw != -1) {
+        axis_center[(int) id] = raw;
+    }
 }
 
 LambdaController::ErrorFactor LambdaController::get_error()
@@ -145,5 +160,6 @@
     va_start(arg, format);
     twe_lite.vprintf(format, arg);
     va_end(arg);
-	*/
+    */
 }
+