Diff: main.cpp

Revision:

0:e32543d6e1e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Aug 22 13:02:21 2021 +0000
@@ -0,0 +1,277 @@
+/*INCLUDE*/
+#include "SDFileSystem.h"
+#include "mbed.h"
+#include "rotary_inc.hpp"
+#include "matrix.h"
+#include "BNO055.h"
+#include "NK_PID.hpp"
+#include <cmath>
+#include <vector>
+#include "scrp_slave.hpp"
+
+ScrpSlave sendpwm(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
+
+struct pos{
+    double x;
+    double y;
+};
+/*FUNCTIONS*/
+void route_read();
+void odmetry();
+void move(double vx,double vy,double vw);
+void getpoint(pos *p,int n);
+void getvelocity(double *v,int n);
+
+/*DECLARATION*/
+SDFileSystem sd(PB_15, PB_14, PB_13, PB_2,"sd");
+
+RotaryInc cenc0(PC_10,PC_11,2*M_PI,256,4);
+RotaryInc cenc1(PC_4,PA_13,2*M_PI,256,4);
+RotaryInc cenc2(PA_14,PA_15,2*M_PI,256,4);
+RotaryInc cenc3(PC_2,PC_3,2*M_PI,256,4);
+
+RotaryInc senc0(PA_6,PA_7,2*M_PI,256,4);
+RotaryInc senc1(PA_8,PA_9,2*M_PI,256,4);
+RotaryInc senc2(PC_0,PC_1,2*M_PI,256,4);
+RotaryInc senc3(PA_12,PC_5,2*M_PI,256,4);
+
+std::vector<double> t_info,x_info,y_info,v_info,theta_info;
+
+/*
+01
+32
+*/
+
+Nk_pid mot0(0.0067,0.001,0,0.01);
+Nk_pid mot1(0.0067,0.001,0,0.01);
+Nk_pid mot2(0.0067,0.001,0,0.01);
+Nk_pid mot3(0.0067,0.001,0,0.01);
+
+BNO055 bno(PB_3, PB_10);
+
+Timer LOOPdt;
+unsigned long long int t;
+
+pos a,b,c;
+double v = 0;
+double cwv[4];
+double swv[4];
+double theta = 0;
+double r = 50.6;
+double R = 2.0*r;
+double cR = 322.5;
+int n = 0;
+
+PwmOut asd(PC_6);
+
+int main(){
+asd = 1;
+/*SETUP*/
+LOOPdt.start();
+/*
+*/
+mot0.SetLimit(0.5,0.00,-0.5,-0.00);
+mot1.SetLimit(0.5,0.00,-0.5,-0.00);
+mot2.SetLimit(0.5,0.00,-0.5,-0.00);
+mot3.SetLimit(0.5,0.00,-0.5,-0.00);
+/*0.07*/
+    
+//route_read();
+odmetry();
+/*LOOP*/
+wait(3);
+while(true){
+    
+    odmetry();
+    /*
+    if(sqrt( (a.x-c.x)*(a.x-c.x)+(a.y-c.y)*(a.y-c.y) ) < 30){
+        getpoint(&a,n);
+        getvelocity(&v,n);
+        if(n<x_info.size()) n++;
+        printf("pass\n");
+    }
+    double theta = atan2(a.y-c.y,a.x-c.x);
+    move(v*cos(theta),v*sin(theta),0);*/
+    //move(0.0,2000.0,0.0);
+    //motor0.pwmout(0.2);
+    //printf("%lf\n",cwv[0]*r);
+    
+    sendpwm.send1(255, 1 , -0.3 * 100);
+    while(LOOPdt.read_us()-t <= 10*1000/100*50);
+    sendpwm.send1(255, 2 , 0.3 * 100);/*
+    while(LOOPdt.read_us()-t <= 10*1000/100*70);
+    sendpwm.send1(255, 3 , 0.3 * 100);
+    while(LOOPdt.read_us()-t <= 10*1000/100*90);
+    sendpwm.send1(255, 4 , 0.3 * 100);*/
+    while(LOOPdt.read_us()-t <= 10*1000);
+    t = LOOPdt.read_us();
+}
+
+}
+
+void route_read() {
+    static double x, y, v, theta;
+    wait(1);
+    pc.printf("Hello World!\n");
+    wait(0.1);
+    FILE *fp;
+    do {
+        fp = fopen("/sd/route.txt", "r");
+        wait(0.2);
+    } while(fp==NULL);
+    pc.printf("Good!\n");
+    while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta) != EOF) {
+        pc.printf("%lf %lf %lf %lf\n",x, y, v, theta);
+        x_info.push_back(x);
+        y_info.push_back(y);
+        v_info.push_back(v);
+        theta_info.push_back(theta);
+    }
+    pc.printf("checking...\n");
+    for(int k=0;k<x_info.size();k++){
+        pc.printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]);
+    }
+    fclose(fp);
+}
+
+void odmetry(){
+    
+    static Matrix<double> Vr(3, 1);
+    static Matrix<double> J(3, 4);
+    static Matrix<double> W(4, 1);
+    static double gyro = 0.0;
+    static double x = 0.0;
+    static double y = 0.0;
+    static double vx = 0.0;
+    static double vy = 0.0;
+    static double sold0 = 0.0, sold1 = 0.0, sold2 = 0.0, sold3 = 0.0;
+    static double cold0 = 0.0, cold1 = 0.0, cold2 = 0.0, cold3 = 0.0;
+    double cor0 = 0.0, cor1 = 0.0, cor2 = 0.0, cor3 = 0.0;
+    double side0 = 0.0, side1 = 0.0, side2 = 0.0, side3 = 0.0;
+    static bool setup = false;
+    if(!setup){
+        //bno.setmode(OPERATION_MODE_IMUPLUS);
+        bno.reset();
+        bno.setmode(OPERATION_MODE_IMUPLUS);
+        bno.get_angles();
+        /*while(fgyro - (bno.euler.yaw * (M_PI / 180.0)) != 0.0){
+            fgyro = bno.euler.yaw * (M_PI / 180.0);
+            bno.get_angles();
+        }*/
+        setup = true;
+    }
+    
+    cor0 = M_PI*cenc0.get()/1024.0;
+    cor1 = M_PI*cenc1.get()/1024.0;
+    cor2 = M_PI*cenc2.get()/1024.0;
+    cor3 = M_PI*cenc3.get()/1024.0;
+    cwv[0] = (cor0-cold0)/0.01;
+    cwv[1] = (cor1-cold1)/0.01;
+    cwv[2] = (cor2-cold2)/0.01;
+    cwv[3] = (cor3-cold3)/0.01;
+    side0 = M_PI*senc0.get()/1024.0;
+    side1 = M_PI*senc1.get()/1024.0;
+    side2 = M_PI*senc2.get()/1024.0;
+    side3 = M_PI*senc3.get()/1024.0;
+    swv[0] = (side0-sold0)/0.01;
+    swv[1] = (side1-sold1)/0.01;
+    swv[2] = (side2-sold2)/0.01;
+    swv[3] = (side3-sold3)/0.01;
+    
+    cold0 = cor0;
+    cold1 = cor1;
+    cold2 = cor2;
+    cold3 = cor3;
+    
+    sold0 = side0;
+    sold1 = side1;
+    sold2 = side2;
+    sold3 = side3;
+    
+    W.set(  4,
+            cwv[0],
+            cwv[1],
+            cwv[2],
+            cwv[3]
+    );
+    /*
+    W.set(  4,
+            swv[0],
+            swv[1],
+            swv[2],
+            swv[3]
+    );*/
+    
+    //W.show();
+    bno.get_angles();
+    gyro = bno.euler.yaw * (M_PI / 180.0);
+    theta = (2*M_PI-gyro);
+    /*
+    J.set(  12,
+            -sin(-theta + M_PI/4.0), -sin(-theta + M_PI/2.0 + M_PI/4.0), -sin(-theta + M_PI + M_PI/4.0), -sin(-theta + 3.0*M_PI/2.0 + M_PI/4.0),
+             -cos(-theta + M_PI/4.0),  -cos(-theta + M_PI/2.0 + M_PI/4.0),  -cos(-theta + M_PI + M_PI/4.0),  -cos(-theta + 3.0*M_PI/2.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.set(  12,
+            -sin(-theta - 2*M_PI/4), -sin(-theta + M_PI/2.0 - 2*M_PI/4), -sin(-theta + M_PI - 2*M_PI/4), -sin(-theta + 3.0*M_PI/2.0 - 2*M_PI/4),
+             -cos(-theta - 2*M_PI/4),  -cos(-theta + M_PI/2.0 - 2*M_PI/4),  -cos(-theta + M_PI - 2*M_PI/4),  -cos(-theta + 3.0*M_PI/2.0 - 2*M_PI/4),
+            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];
+    c.x = x;
+    c.y = y;
+    pc.printf("x : %lf y : %lf  vr : %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]);
+        
+}
+
+void getpoint(pos *p,int n){
+    p->x = x_info[n];
+    p->y = y_info[n];
+}
+
+void getvelocity(double *v,int n){
+    *v = v_info[n];
+}
+
+void move(double vx,double vy,double vw){
+    
+    static double k = sqrt(2.0)/2.0;;
+    static double target[4];
+    
+    target[0]= vx*k*(sin(theta)-cos(theta)) - vy*k*(sin(theta)+cos(theta)) + cR*vw;
+    target[1]= -vx*k*(sin(theta)+cos(theta)) - vy*k*(sin(theta)-cos(theta)) + cR*vw;
+    target[2]= -vx*k*(sin(theta)-cos(theta)) + vy*k*(sin(theta)+cos(theta)) + cR*vw;
+    target[3]= vx*k*(sin(theta)+cos(theta)) + vy*k*(sin(theta)-cos(theta)) + cR*vw;
+    
+    /*
+      target[1]=(k*vy-k*vx)+cR*vw;
+      target[0]=(k*vx+k*vy)*-1;
+      target[3]=(k*vx-k*vy)+cR*vw;
+      target[2]=(k*vx+k*vy)+cR*vw;*/
+    mot0.SetParam(cwv[0]*r/100.0,target[0]/100.0);
+    mot1.SetParam(cwv[1]*r/100.0,target[1]/100.0);
+    mot2.SetParam(cwv[2]*r/100.0,target[2]/100.0);
+    mot3.SetParam(cwv[3]*r/100.0,target[3]/100.0);
+    
+    //printf("%lf\n",mot0.GetGain(1.0));
+    //motor0.pwmout(mot0.GetGain(1.0)
+    sendpwm.send1(255, 1 ,(mot0.GetGain(1.0) * 100));
+    while(LOOPdt.read_us()-t <= 10*1000/100*40);
+    sendpwm.send1(255, 2 ,(mot1.GetGain(1.0) * 100));
+    while(LOOPdt.read_us()-t <= 10*1000/100*70);
+    sendpwm.send1(255, 3 ,(mot2.GetGain(1.0) * 100));
+    while(LOOPdt.read_us()-t <= 10*1000/100*90);
+    sendpwm.send1(255, 4 ,(mot3.GetGain(1.0) * 100));
+    //printf("%lf,%lf\n",cwv[0]*r,target[0]);
+}
\ No newline at end of file