Diff: LambdaController.cpp

Revision:

0:1b682aa7d935

Child:

1:e270b8b60070

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LambdaController.cpp	Tue Aug 23 05:10:44 2016 +0000
@@ -0,0 +1,126 @@
+#include "LambdaController.h"
+
+LambdaController::LambdaController(PinName tx, PinName rx) :
+		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);
+
+	error_count = 0;
+	error_id = NoDataError;
+	error_detection_timer.attach(this, &LambdaController::error_detection_isr,
+			0.03);
+}
+
+void LambdaController::data_receive_isr() {
+	int data;
+	int i;
+
+	while (twe_lite.readable()) {
+		data = twe_lite.getc();
+
+		if (data == 0x3A) {
+			buffer_head = 0;
+		}
+		
+		if(data == EOF){
+			buffer_head = -1;
+			error_id = InvalidDataError;
+		}	
+
+		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();
+			}
+
+			error_count = 0;
+			error_id = NoError;
+		} else {
+			buffer_head = -1;
+			error_id = InvalidDataError;
+		}
+	}
+}
+
+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];
+}
+
+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;
+}
+
+int LambdaController::get_raw_axis(AxisId id) {
+	int *data;
+	int value;
+
+	data = &received_data[(int) id * 2 + 2];
+	value = (data[0] << 8) | (data[1]);
+
+	return value;
+}
+
+double LambdaController::get_axis(AxisId id) {
+	double value = 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;
+	}
+
+	return value;
+}
+
+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);
+}
+
+LambdaController::ErrorFactor LambdaController::get_error() {
+	return error_id;
+}
+
+void GetCon::debug(const char *format, ...)
+{
+    va_list arg;
+
+    va_start(arg, format);
+    device.vprintf(format, arg);
+    va_end(arg);
+}
\ No newline at end of file