Diff: ActionEncoder.txt

Revision:

0:502b364c9f1d

diff -r 000000000000 -r 502b364c9f1d ActionEncoder.txt
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ActionEncoder.txt	Sun Oct 21 19:38:09 2018 +0000
@@ -0,0 +1,265 @@
+#include "mbed.h"
+#include "millis.h"
+
+/*
+ * ActionEncoder.cpp
+ *
+ *  Created on: 7 Mar 2018
+ *      Author: tung
+ */
+
+#include "ActionEncoder.hpp"
+#include "Timer.h"
+
+Serial Action(D8,D2 ); // tx, rx
+Serial pc(USBTX, USBRX);
+
+
+
+union {
+    uint8_t data[24];
+    float val[6];
+} posture;
+int count=0;
+int i=0;
+int done=0;
+float xangle=0;
+float yangle=0;
+float zangle=0;
+float d_angle=0;
+float pos_x=0;
+float pos_y=0;
+float angleSpeed=0;
+float temp_zangle=0;
+int   LastRead=0;
+bool newDataArrived=false;
+
+char buffer[8];
+
+
+void ActionEncoder_init()
+{
+    count=0;
+    i=0;
+    done=0;
+    xangle=0;
+    yangle=0;
+    zangle=0;
+    d_angle=0;
+    pos_x=0;
+    pos_y=0;
+    angleSpeed=0;
+    temp_zangle=0;
+    LastRead=0;
+    newDataArrived=false;
+
+
+}
+
+void readEncoder(char c)
+{
+
+
+    //sprintf(buffer,"%02X",c);
+    //sprintf(buffer,"%X",c);
+    //pc.printf(buffer);
+    //pc.printf("\r\n");
+    
+    //sprintf(buffer,"%d",count);
+    //pc.printf(buffer);
+    //pc.printf("\r\n");
+    switch(count) {
+        case 0:
+            if (c==0x0d) count++;
+            //else count=0;
+            break;
+        case 1:
+            if(c==0x0a) {
+                i=0;
+                count++;
+            } else if(c==0x0d) {}
+            //else count=0;
+            break;
+        case 2:
+            posture.data[i]=c;
+            i++;
+            if(i>=24) {
+                i=0;
+                count++;
+            }
+            break;
+        case 3:
+            if(c==0x0a)count++;
+            //else count=0;
+            break;
+        case 4:
+            if(c==0x0d) {
+                d_angle=posture.val[0]-temp_zangle;
+                if (d_angle<-180) {
+                    d_angle=d_angle+360;
+                } else if (d_angle>180) {
+                    d_angle=d_angle-360;
+                }
+                zangle+=d_angle;
+                temp_zangle=posture.val[0];
+                xangle=posture.val[1];
+                yangle=posture.val[2];
+                pos_x=posture.val[3];
+                pos_y=posture.val[4];
+                angleSpeed=posture.val[5];
+                newDataArrived=true;
+                
+                pc.printf("GET\r\n");
+                sprintf(buffer, "%f", pos_x);
+                pc.printf(buffer);
+                pc.printf("  ");
+                sprintf(buffer, "%f", pos_y);
+                pc.printf(buffer);
+                pc.printf("  ");
+                sprintf(buffer, "%f", zangle);
+                pc.printf(buffer);
+                pc.printf("\r\n");
+            }
+            count=0;
+            done=1;
+            LastRead=millis();
+            break;
+        default:
+            //count=0;
+            break;
+    }
+}
+
+bool updated()
+{
+    if (done) {
+        done=false;
+        return true;
+    } else {
+        return false;
+    }
+
+}
+
+float getXangle()
+{
+    return xangle;
+}
+
+float getYangle()
+{
+    return yangle;
+}
+
+float getZangle()
+{
+    return zangle;
+}
+
+float getXpos()
+{
+    return pos_x;
+}
+
+float getYpos()
+{
+    return pos_y;
+}
+
+float getAngleSpeed()
+{
+    return angleSpeed;
+}
+
+bool isAlive()
+{
+    if ((millis()-LastRead)<100) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool newDataAvailable()
+{
+    if (newDataArrived) {
+        newDataArrived=false;
+        return true;
+    } else return false;
+}
+
+char* reset()
+{
+    return "ACT0";
+}
+
+char* calibrate()
+{
+    return "ACTR";
+}
+//////////////
+
+void action_get()
+{
+
+    /*
+
+    // Loop just in case more than one character is in UART's receive FIFO buffer
+    // Stop if buffer full
+      while ((device.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
+          rx_buffer[rx_in] = device.getc();
+    // Uncomment to Echo to USB serial to watch data flow
+    //        monitor_device.putc(rx_buffer[rx_in]);
+          rx_in = (rx_in + 1) % buffer_size;
+      }
+      */
+
+    readEncoder(Action.getc());
+
+}
+/////////////////////////
+
+
+
+
+
+int main()
+{
+    //Action.attach(&action_get, Serial::RxIrq);
+    Action.baud(115200);
+    Action.format(8,SerialBase::None,1); 
+    
+    
+    ActionEncoder_init();
+
+    while(1) {
+
+        while (Action.readable()==1 ) 
+        {
+            //pc.printf("readable\r\n");
+            char c = Action.getc();
+            
+            readEncoder(c);
+            //pc.putc(c);
+            //pc.printf("\r\n");
+          
+          
+          /*
+                sprintf(buffer, "%f", pos_x);
+                pc.printf(buffer);
+                sprintf(buffer, "%f", pos_y);
+                pc.printf(buffer);
+                sprintf(buffer, "%f", zangle);
+                pc.printf(buffer);
+                pc.printf("\r\n");*/
+            
+
+        }
+
+    }
+
+
+
+}
+
+