Diff: main.cpp

Revision:

3:0de505949008

Parent:

2:af928306ca36

--- a/main.cpp	Mon Aug 14 09:47:07 2017 +0000
+++ b/main.cpp	Fri Jan 12 06:30:54 2018 +0000
@@ -9,6 +9,7 @@
 #include <std_msgs/Float32.h>
 #include <std_msgs/Empty.h>
 #include "QEI.h"
+#include "math.h"
 
 using namespace std_msgs;
 using namespace ros;
@@ -16,6 +17,7 @@
 DigitalOut myled(LED1);
 
 #define BAUD 115200
+#define M_PI 3.14159265358979323846  /* pi */
 
 // GYO
 Serial gy25(PA_9, PA_10, 115200);
@@ -23,6 +25,11 @@
 char rxC[8];
 char ctBuffer[128];
 
+
+float pX1 = 0, pX2 = 0, pY1 = 0, pY2 = 0, pG1 = 0, pG2 = 0, pG3 = 0, pXaxis = 0, pYaxis = 0;
+float g0 = 0, Xn = 0, Yn = 0, Gn = 0, pX = 0, pY = 0, pg0 = 0;
+bool clockwise = false;
+
 void rxIRQ(){
     // get the Byte0: 0xAA Preamble Flags
     rxC[0] = gy25.getc();
@@ -46,7 +53,7 @@
 
 // ENCODER
 #define N 1000
-#define C 10     // circumference(cm)
+#define C 18     // 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);
@@ -56,10 +63,12 @@
 ros::NodeHandle  nh;
 
 // instantiate the publisher
-Float32 X1, X2, Y1, Y2, G1, G2, G3;
+Float32 Xaxis, Yaxis, X1, X2, Y1, Y2, G1, G2, G3;
+Publisher pub_X("Xaxis", &Xaxis);
+Publisher pub_Y("Yaxis", &Yaxis);
 Publisher pub_X1("X1", &X1);
+Publisher pub_Y1("Y1", &Y1);
 Publisher pub_X2("X2", &X2);
-Publisher pub_Y1("Y1", &Y1);
 Publisher pub_Y2("Y2", &Y2);
 Publisher pub_G1("G1", &G1);
 Publisher pub_G2("G2", &G2);
@@ -77,6 +86,8 @@
     En_X2.reset();
     En_Y1.reset();
     En_Y2.reset();
+    Xn = 0;
+    Yn = 0;
 }
 
 // for reset
@@ -94,16 +105,18 @@
     
     nh.subscribe(sub);
     
+    nh.advertise(pub_X);
+    nh.advertise(pub_Y);
     nh.advertise(pub_X1);
+    nh.advertise(pub_Y1);
     nh.advertise(pub_X2);
-    nh.advertise(pub_Y1);
     nh.advertise(pub_Y2);
     nh.advertise(pub_G1);
+    nh.advertise(pub_G2);
     nh.advertise(pub_G3);
-    nh.advertise(pub_G2);
 }
 
-float pX1 = 0, pX2 = 0, pY1 = 0, pY2 = 0, pG1 = 0, pG2 = 0, pG3 = 0;
+
 
 int main() {
     nodeInit();
@@ -115,14 +128,61 @@
     button.fall(&pressed);
     
     while (1) {
-        X1.data = (float)En_X1.getPulses()/(4*N) * C;
-        Y1.data = (float)En_Y1.getPulses()/(4*N) * C;
+        X1.data = -(float)En_X1.getPulses()/(4*N) * C;
+        Y1.data = -(float)En_Y2.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];
+        Y2.data = (float)En_Y1.getPulses()/(4*N) * C;
+        
+        float dX = ((X1.data - pX1) + (X2.data - pX2)) / 2;
+        float dY = ((Y1.data - pY1) + (Y2.data - pY2)) / 2;
+        
+        if(pG1 < 45 && pG1 >= 0 && floor(g[0]) > 315){
+            clockwise = true;
+        }
+        else if(pG1 > 315 && floor(g[0]) >= 0 && floor(g[0]) < 45){
+            clockwise = false; 
+        }
+        
+        if(clockwise){
+            g0 = g[0];
+            //g0 = g[0] - 360;
+        }
+        else{
+            g0 = g[0];
+        }
         
+        //float arc = 15 * g0 / 180 * M_PI;
+        //float Xa = X1.data - arc;
+        //float Ya = Y1.data - arc;
+ //       float dX = X - pX;
+//        float dY = Y - pY;
+        float dg0 = g0 - pg0;
+        
+        float x = (Xn + dX * cos(Gn + dg0 / 360 * M_PI) + dY * cos(Gn + M_PI/2 + dg0 / 360 * M_PI));
+        float y = (Yn + dX * sin(Gn + dg0 /360 * M_PI) + dY * sin(Gn + M_PI/2 + dg0 /360 * M_PI));
+        Gn = Gn + dg0 / 180 * M_PI;
+        pg0 = g0;
+        Xn = x;
+        Yn = y;
+        //x = (int)(x*10);
+        //y = (int)(y*10);
+        //Xaxis.data = (float) x;
+        //Yaxis.data = (float) y;
+        Xaxis.data = (int)x;
+        Yaxis.data = (int)y;
+//        pX = dX;
+//        pY = dY;
+        //Xaxis.data = (int) (Xa * cos(g0 / 180 * M_PI) - Ya * sin(g0 / 180 * M_PI));
+        //Yaxis.data = (int) (Ya * cos(g0 / 180 * M_PI) + Xa * sin(g0 / 180 * M_PI));
+        
+        pub_X.publish( &Xaxis );
+        pub_Y.publish( &Yaxis );
+        
+        G1.data = floor(g[0]);
+        G2.data = dX;
+        G3.data = dY;
+        
+
         if(pX1 != X1.data){
             pX1 = X1.data;
             pub_X1.publish( &X1 );