Diff: main.cpp

Revision:

0:dea3c70f4911

Child:

1:4062af6224fa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 21 07:35:27 2021 +0000
@@ -0,0 +1,91 @@
+#include "mbed.h"
+#include "rotary_inc.hpp"
+#include "matrix.h"
+#include "BNO055.h"
+#include <cmath>
+
+//Serial pc(USBTX, USBRX);
+
+RotaryInc enc1(PC_3,PC_2,2*M_PI,256,4);
+RotaryInc enc2(PC_5,PA_12,2*M_PI,256,4);
+RotaryInc enc3(PC_1,PC_0,2*M_PI,256,4);
+RotaryInc enc4(PA_13,PC_4,2*M_PI,256,4);
+
+BNO055 bno(PB_3, PB_10);
+
+Timer t;
+
+Matrix<double> Vr(3, 1);
+Matrix<double> J(3, 4);
+Matrix<double> W(4, 1);
+
+int main(){
+    double r = 50.6;
+    double R = 2.0*r;
+    double theta = 0.0;
+    double gyro = 0.0;
+    double fgyro = 0.0;
+    double x = 0.0;
+    double y = 0.0;
+    double vx = 0.0;
+    double vy = 0.0;
+    double old1 = 0.0, old2 = 0.0, old3 = 0.0, old4 = 0.0;
+    double cor1 = 0.0, cor2 = 0.0, cor3 = 0.0, cor4 = 0.0;
+    bno.setmode(OPERATION_MODE_IMUPLUS);
+        bno.get_angles();/*
+        while(fgyro == 0){
+        bno.get_angles();
+        fgyro = bno.euler.yaw * (M_PI / 180.0);
+            }
+            fgyro = 0.0;*/
+    while(fgyro - (bno.euler.yaw * (M_PI / 180.0)) != 0.0){
+        fgyro = bno.euler.yaw * (M_PI / 180.0);
+        bno.get_angles();
+    }
+    t.start();                                           
+    double start;
+    while(1){
+        start = t.read();
+        
+        cor1 = 2*M_PI*enc1.get()/1024.0;
+        cor2 = 2*M_PI*enc2.get()/1024.0;
+        cor3 = 2*M_PI*enc3.get()/1024.0;
+        cor4 = 2*M_PI*enc4.get()/1024.0;
+        
+        W.set(  4,
+                (old1-cor1)/0.01,//enc1.getSpeed(),
+                (old2-cor2)/0.01,//enc2.getSpeed(),
+                (old3-cor3)/0.01,//enc3.getSpeed(),
+                (old4-cor4)/0.01//enc4.getSpeed()
+        );
+        
+        old1 = cor1;
+        old2 = cor2;
+        old3 = cor3;
+        old4 = cor4;
+        
+        //W.show();
+        bno.get_angles();
+        gyro = bno.euler.yaw * (M_PI / 180.0);
+        theta = abs(fgyro - gyro);  
+        J.set(  12,
+                -sin(theta + M_PI/4.0), -sin(theta + 3.0*M_PI/2.0), -sin(theta + 5.0*M_PI/4.0), -sin(theta + 7.0*M_PI/4.0),
+                 cos(theta + M_PI/4.0),  cos(theta + 3.0*M_PI/2.0),  cos(theta + 5.0*M_PI/4.0),  cos(theta + 7.0*M_PI/4.0),
+                1.0/(2.0*R),            1.0/(2.0*R),        1.0/(2.0*R),                1.0/(2.0*R)
+        );
+        //J.show();
+
+        Vr = J*W*(r/2);
+        //Vr.show();
+        
+        x += ((vx + Vr[0][0]) * 0.01) / 2.0;
+        y += ((vy + Vr[1][0]) * 0.01) / 2.0;
+        vx = Vr[0][0];
+        vy = Vr[1][0];
+        
+        pc.printf("x : %lf y : %lf  : %lf theta : %lf\r\n", x, y, Vr[2][0], theta);
+        //pc.printf("vx : %lf vy : %lf\r\n",Vr[0][0], Vr[1][0]);
+        
+        while(t.read() < start + 0.01);
+    }
+}
\ No newline at end of file