Diff: LambdaController.cpp

Revision:

3:9f80eee7aa78

Parent:

2:82c94d2c727a

Child:

4:c35227d14896

--- a/LambdaController.cpp	Tue Aug 23 05:30:07 2016 +0000
+++ b/LambdaController.cpp	Tue Aug 23 08:30:41 2016 +0000
@@ -1,135 +1,149 @@
 #include "LambdaController.h"
 
 LambdaController::LambdaController(PinName tx, PinName rx) :
-		twe_lite(tx, rx) {
-	is_axis_initialized = false;
+    twe_lite(tx, rx)
+{
+    is_axis_initialized = false;
 
-	buffer_head = -1;
-	twe_lite.baud(115200);
-	twe_lite.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
+    buffer_head = -1;
+    twe_lite.baud(115200);
+    twe_lite.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
 
-	error_count = 0;
-	error_id = NoDataError;
-	error_detection_timer.attach(this, &LambdaController::error_detection_isr,
-			0.03);
+    error_count = 0;
+    error_id = NoDataError;
+    error_detection_timer.attach(this, &LambdaController::error_detection_isr,
+                                 timeout_time);
 }
 
-void LambdaController::data_receive_isr() {
-	int data;
-	int i;
+void LambdaController::data_receive_isr()
+{
+    int data;
+    int i;
 
-	while (twe_lite.readable()) {
-		data = twe_lite.getc();
+    while (twe_lite.readable()) {
+        data = twe_lite.getc();
+        
+        if (data == 0x3A) {
+            buffer_head = 0;
+        }
+
+        if (buffer_head < 10 && buffer_head != -1) {
+            received_buffer[buffer_head] = data;
+            buffer_head++;
+        } else {
+            error_id = InvalidDataError;
+            buffer_head = -1;
+        }
 
-		if (data == 0x3A) {
-			buffer_head = 0;
-		}
-		
-		if(data == EOF){
-			buffer_head = -1;
-			error_id = InvalidDataError;
-		}
-		
-		if (buffer_head < 10 && buffer_head != -1) {
-			received_buffer[buffer_head] = data;
-			buffer_head++;
-		} else {
-			error_id = InvalidDataError;
-			buffer_head = -1;
-		}
+        if (buffer_head == 10) {
+            if(received_buffer[0] == 0x3A) {
+                for (i = 0; i < 10; i++) {
+                    received_data[i] = received_buffer[i];
+                }
+
+                if (!is_axis_initialized) {
+                    initialize_axis();
+                    is_axis_initialized = true;
+                }
 
-		if (buffer_head == 10 && received_buffer[0] == 0x3A) {
-			for (i = 0; i < 10; i++) {
-				received_data[i] = received_buffer[i];
-			}
-
-			if (!is_axis_initialized) {
-				initialize_axis();
-				is_axis_initialized = true;
-			}
-
-			error_count = 0;
-			error_id = NoError;
-		} else {
-			buffer_head = -1;
-			error_id = InvalidDataError;
-		}
-	}
+                error_count = 0;
+                error_id = NoError;
+            } else {
+                buffer_head = -1;
+                error_id = InvalidDataError;
+            }
+        }
+        
+        if(data == EOF) {
+            error_id = SerialBusyError;
+            break;
+    	}
+    }
 }
 
-void LambdaController::error_detection_isr() {
-	if (error_count < error_threshold) {
-		error_count++;
-	} else {
-		error_id = TimeoutError;
-	}
+void LambdaController::error_detection_isr()
+{
+    if (error_count < error_threshold) {
+        error_count++;
+    } else {
+        error_id = TimeoutError;
+    }
 }
 
-uint8_t LambdaController::get_all_switch() {
-	return received_data[1];
+uint8_t LambdaController::get_all_switch()
+{
+    return received_data[1];
 }
 
-bool LambdaController::get_switch(int num) {
-	bool status = false;
-	if (num < 8 && num >= 0) {
-		if (get_all_switch() & (1 << num)) {
-			status = true;
-		} else {
-			status = false;
-		}
-	} else {
-		status = false;
-	}
+bool LambdaController::get_switch(int num)
+{
+    bool status = false;
+    if (num < 8 && num >= 0) {
+        if (get_all_switch() & (1 << num)) {
+            status = true;
+        } else {
+            status = false;
+        }
+    } else {
+        status = false;
+    }
 
-	return status;
+    return status;
 }
 
-int LambdaController::get_raw_axis(AxisId id) {
-	uint8_t *data;
-	int value;
+int LambdaController::get_raw_axis(AxisId id)
+{
+    uint8_t *data;
+    int value;
 
-	data = &received_data[(int) id * 2 + 2];
-	value = (data[0] << 8) | (data[1]);
+    data = &received_data[(int) id * 2 + 2];
+    value = (data[0] << 8) | (data[1]);
 
-	return value;
+    return value;
 }
 
-double LambdaController::get_axis(AxisId id) {
-	double value = 0;
+double LambdaController::get_axis(AxisId id)
+{
+    double value = 0;
 
-	value = (double) (get_raw_axis(id) - axis_center[(int) id]) / 512.0;
+    value = (double) (get_raw_axis(id) - axis_center[(int) id]) / 512.0;
 
-	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;
-	}
+    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;
+    }
 
-	return value;
+    return value;
 }
 
-void LambdaController::initialize_axis(void) {
-	int i;
-	for (i = 0; i < 4; i++) {
-		initialize_axis((AxisId) i);
-	}
+void LambdaController::initialize_axis(void)
+{
+    int i;
+    for (i = 0; i < 4; i++) {
+        initialize_axis((AxisId) i);
+    }
 }
 
-void LambdaController::initialize_axis(AxisId id) {
-	axis_center[(int) id] = get_raw_axis(id);
+void LambdaController::initialize_axis(AxisId id)
+{
+    axis_center[(int) id] = get_raw_axis(id);
 }
 
-LambdaController::ErrorFactor LambdaController::get_error() {
-	return error_id;
+LambdaController::ErrorFactor LambdaController::get_error()
+{
+    return error_id;
 }
 
 void LambdaController::debug(const char *format, ...)
 {
+    /*
     va_list arg;
 
     va_start(arg, format);
     twe_lite.vprintf(format, arg);
     va_end(arg);
-}
\ No newline at end of file
+	*/
+}