Diff: LambdaController.cpp

Revision:

0:728c138f0486

Child:

1:3785af7e715e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LambdaController.cpp	Sat Nov 12 03:55:50 2016 +0000
@@ -0,0 +1,237 @@
+#include "LambdaController.h"
+
+LambdaController::LambdaController(PinName tx, PinName rx, PinName hzpin) :
+    device(tx, rx), hzjudge(hzpin)
+{   
+    is_axis_initialized = false;
+
+    buffer_head = -1;
+    device.baud(19200);
+    device.attach(this, &LambdaController::data_receive_isr, Serial::RxIrq);
+    
+    hzjudge.mode(PullUp);
+
+    error_count = 0;
+    error_id = NoDataError;
+                                 
+    error_timer.start();
+}
+
+void LambdaController::data_receive_isr()
+{
+    char data, *ptr;
+    int i;
+
+    while (device.readable())
+    {
+        data = device.getc();
+
+        if (data == 0x3A)
+        {
+            buffer_head = 0;
+        }
+
+        if (hzjudge)
+        {
+            if (buffer_head < 21 && buffer_head != -1)
+            {
+                if (data != ',')
+                {
+                    if (buffer_head == 0)
+                    {
+                        received_buffer[buffer_head] = 0x3A;
+                    }   
+                    else
+                    {
+                        if (buffer_head % 2 == 1)
+                        {
+                            received_buffer[buffer_head / 2 + 1] = strtol(&data, &ptr, 16) << 4;
+                        }
+                        else if (buffer_head % 2 == 0)
+                        {
+                            received_buffer[buffer_head / 2] |= strtol(&data, &ptr, 16);
+                        }
+                    }
+                    buffer_head++;
+                }
+            }
+            
+            if (buffer_head == 21)
+            {
+                if (received_buffer[0] == 0x3A)
+                {
+                    for (i = 0; i < 11; i++)
+                    {
+                        received_data[i] = received_buffer[i];
+                    }
+                    
+                    if (!is_axis_initialized)
+                    {
+                        initialize_axis();
+                        is_axis_initialized = true;
+                    }
+    
+                    error_count = 0;
+                    error_id = NoError;
+                    error_timer.reset();
+                }
+                else
+                {
+                    buffer_head = -1;
+                    error_id = InvalidDataError;
+                }
+            }   
+        }
+        else
+        {
+            if (buffer_head < 11 && buffer_head != -1)
+            {
+                received_buffer[buffer_head] = data;
+                buffer_head++;
+            }
+            else
+            {
+                error_id = InvalidDataError;
+                buffer_head = -1;
+            }
+    
+            if (buffer_head == 11)
+            {
+                if(received_buffer[0] == 0x3A)
+                {
+                    for (i = 0; i < 11; i++)
+                    {
+                        received_data[i] = received_buffer[i];
+                    }
+    
+                    if (!is_axis_initialized)
+                    {
+                        initialize_axis();
+                        is_axis_initialized = true;
+                    }
+    
+                    error_count = 0;
+                    error_id = NoError;
+                    error_timer.reset();
+                }
+                else
+                {
+                    buffer_head = -1;
+                    error_id = InvalidDataError;
+                }
+            }
+        }
+
+        if(data == EOF)
+        {
+            error_id = SerialBusyError;
+            break;
+        }
+    }
+}
+
+uint16_t LambdaController::get_all_switch()
+{
+    return (received_data[1] << 8) | received_data[2];
+}
+
+bool LambdaController::get_switch(int num)
+{
+    bool status = false;
+    if (num < 11 && 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)
+{
+    uint8_t *data;
+    int value;
+
+    if((int)id < 4 && (int)id >= 0)
+    {
+        data = &received_data[(int) id * 2 + 3];
+        value = (data[0] << 8) | (data[1]);
+    }
+    else
+    {
+        value = -1;
+    }
+
+    return value;
+}
+
+double LambdaController::get_axis(AxisId id)
+{
+    double value = 0;
+    int raw;
+    raw = get_raw_axis(id);
+
+    if(raw != -1)
+    {
+        value = (double) (raw - 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;
+        }
+    }
+    else
+    {
+        value = 0.0;
+    }
+
+    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)
+{
+    int raw;
+    raw = get_raw_axis(id);
+
+    if(raw != -1)
+    {
+        axis_center[(int) id] = raw;
+    }
+}
+
+LambdaController::ErrorFactor LambdaController::get_error()
+{   
+    if (error_timer >= timeout_time)
+    {
+        error_id = TimeoutError;
+    }
+    
+    return error_id;
+}
\ No newline at end of file