Diff: main.cpp

Revision:

0:62729afe5606

Child:

1:ea4150fafa3b

diff -r 000000000000 -r 62729afe5606 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 10 09:07:54 2017 +0000
@@ -0,0 +1,114 @@
+/*
+ * rosserial Publisher for encoder
+ * Publish: distance(cm)
+ */
+
+#include"mbed.h"
+#include <ros.h>
+#include <std_msgs/Float32.h>
+#include "QEI.h"
+
+// GYO
+Serial gy25(PA_9, PA_10, 115200);
+float g[3];
+char rxC[8];
+char ctBuffer[128];
+
+void rxIRQ(){
+    // get the Byte0: 0xAA Preamble Flags
+    rxC[0] = gy25.getc();
+    // fetch the 8-byte data when 0xAA is found
+    if (rxC[0] == 0xAA) {
+        for (int i = 1; i < 8; i++) {
+            rxC[i] = gy25.getc();
+        }
+        // if the last byte is 0x55(Frame end flag), starts the calculation
+        if (rxC[7] == 0x55) {
+            for (int i = 0, j =1; i < 3; i++, j+=2) {
+                //Angle = ( (HIGH << 8) | LOW ) / 100
+                g[i] = (float)(rxC[j]<<8 | rxC[j+1])/100;
+                // adjustment
+                if (g[i] > 475)
+                    g[i] = g[i] - 476 + 180;
+            }
+        }
+    }
+}
+
+// ENCODER
+#define N 1000
+#define C 10     // circumference(cm)
+QEI En_X1(PC_10, PC_11, PC_12, N, QEI::X4_ENCODING); // A, B, Z, pulses/revolution, mode
+QEI En_X2(PC_1, PC_2, PC_3, N, QEI::X4_ENCODING);
+QEI En_Y1(PB_3, PB_4, PB_5, N, QEI::X4_ENCODING);
+QEI En_Y2(PA_6, PA_7, PA_8, N, QEI::X4_ENCODING);
+
+// instantiate the node handle
+ros::NodeHandle  nh;
+
+// instantiate the publisher
+std_msgs::Float32 X1, X2, Y1, Y2, G1, G2, G3;
+ros::Publisher pub_X1("X1", &X1);
+ros::Publisher pub_X2("X2", &X2);
+ros::Publisher pub_Y1("Y1", &Y1);
+ros::Publisher pub_Y2("Y2", &Y2);
+ros::Publisher pub_G1("G1", &G1);
+ros::Publisher pub_G2("G2", &G2);
+ros::Publisher pub_G3("G3", &G3);
+
+// initializing node
+void nodeInit() {
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    
+    nh.advertise(pub_X1);
+    nh.advertise(pub_X2);
+    nh.advertise(pub_Y1);
+    nh.advertise(pub_Y2);
+    nh.advertise(pub_G1);
+    nh.advertise(pub_G3);
+    nh.advertise(pub_G2);
+}
+
+// BUTTON for reset
+InterruptIn button(USER_BUTTON);
+
+void pressed() {
+    gy25.putc(0xA5);
+    gy25.putc(0x55);
+    gy25.putc(0xA5);
+    gy25.putc(0x52);
+    En_X1.reset();
+    En_X2.reset();
+    En_Y1.reset();
+    En_Y2.reset();
+}
+
+int main() {
+    nodeInit();
+    
+    // gyo
+    gy25.format(8,SerialBase::None,1);
+    gy25.attach(rxIRQ, Serial::RxIrq);
+    // reset button
+    button.fall(&pressed);
+    
+    while (1) {
+        X1.data = (float)En_X1.getPulses()/(4*N) * C;
+        Y1.data = (float)En_Y1.getPulses()/(4*N) * C;
+        X2.data = (float)En_X2.getPulses()/(4*N) * C;
+        Y2.data = (float)En_Y2.getPulses()/(4*N) * C;
+        G1.data = g[0];
+        G2.data = g[1];
+        G3.data = g[2];
+        pub_X1.publish( &X1 );
+        pub_Y1.publish( &Y1 );
+        pub_X2.publish( &X2 );
+        pub_Y2.publish( &Y2 );
+        pub_G1.publish( &G1 );
+        pub_G2.publish (&G2 );
+        pub_G3.publish( &G3 );
+        nh.spinOnce();
+        wait_ms(50); // 20hz
+    }
+}
\ No newline at end of file