Diff: main.cpp

Revision:

0:f80bb69e638e

Child:

1:aa9cc4250220

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 13 08:47:48 2021 +0000
@@ -0,0 +1,206 @@
+#define USESCRP
+#include "core.hpp"
+#include "SDFileSystem.h"
+#include "BNO055.h"
+#include <cmath>
+#include <vector>
+
+#include "mbed.h"
+#define POINTS 900
+
+
+/*--------------FUNCTION--------------*/
+void init();
+void route_read();
+void getPoint(Position *p,int n);
+void getVelocity(double *v,int n);
+bool incircle(Position pos,Position target,double error);
+void gyroA(double theta);
+void gyroB(double theta);
+bool checkMOT12(int rx_data, int &tx_data);
+bool checkMOT34(int rx_data, int &tx_data);
+
+
+/*--------------VARIABLE--------------*/
+
+std::vector<double> t_info,x_info,y_info,v_info,theta_info;
+Position pos,NextP,subP;
+int n=1;
+double v=0.0;
+double theta = 0.0;
+double gyro = 0.0;
+bool gyro_1 = false,gyro_2 = false;
+long long g_pulse;
+bool MOT12 = false,MOT34 = false;
+
+BNO055 bno(PB_3, PB_10);
+
+SDFileSystem sd(PB_15, PB_14, PB_13, PB_1,"sd");// mosi, miso, sclk, cs
+ScrpSlave scrp(PC_12,PD_2 ,PH_1 ,SERIAL_TX,SERIAL_RX,0x0807f800);
+Robot AKASHIKOSEN(50.8,25.4,322.5,259.75);
+Core RBT(&AKASHIKOSEN,&scrp,OMNI4,0.02);
+
+int main(){
+
+/*--------------SETUP--------------*/
+scrp.addCMD(6,checkMOT12);
+scrp.addCMD(7,checkMOT34);
+AKASHIKOSEN.setCWID(1,2,3,4);
+AKASHIKOSEN.setSWID(5,6,7,8);
+    
+RBT.addENC(PC_4,PA_13,512,4,1);
+RBT.addENC(PA_14,PA_15,512,4,2);
+RBT.addENC(PC_2,PC_3,512,4,3);
+RBT.addENC(PC_10,PC_11,512,4,4);
+RBT.addENC(PA_7,PA_6,512,4,5);
+RBT.addENC(PA_9,PA_8,512,4,6);
+RBT.addENC(PC_1,PC_0,512,4,7);
+RBT.addENC(PC_5,PA_12,512,4,8);
+
+RBT.addPID(0.0048,0.001,0.00008,1)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.001,0.00008,2)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.001,0.00008,3)->setLimit(0.5,-0.5);
+RBT.addPID(0.0048,0.001,0.00008,4)->setLimit(0.5,-0.5);
+RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
+
+init();
+
+route_read();
+
+wait(1);
+
+
+bool setup = false;
+if(!setup){
+    bno.reset();
+    bno.setmode(OPERATION_MODE_IMUPLUS);
+    setup = true;
+}
+    
+RBT.START();
+/*--------------LOOP--------------*/
+while(true){
+    pos = RBT.getStatus();
+    
+    bno.get_angles();
+    gyro = 2.0*M_PI - bno.euler.yaw * (M_PI / 180.0);
+    gyroA(gyro);
+    gyroB(gyro);
+    
+    theta = 2.0*M_PI*(int)(g_pulse/4.0);
+    if(g_pulse > 0.0) theta += gyro;
+    else if(g_pulse < 0.0) theta += gyro-2.0*M_PI;
+    else if(g_pulse == 0 and gyro < M_PI/2.0) theta += gyro;
+    else if(g_pulse == 0 and gyro > 3.0*M_PI/2.0) theta += gyro-2.0*M_PI;
+    
+    RBT.setPosition(pos.x,pos.y,theta);
+    
+    //RBT.setVelocity(0.0,0.0,0.0);
+    if(n<POINTS and 
+        ( (incircle(pos,NextP,30.0) and n < POINTS and n != 1000) or 
+            ( incircle(pos,NextP,10.0) and n == 1000 ) or
+            ( sqrt( (NextP.x-pos.x)*(NextP.x-pos.x)+(NextP.y-pos.y)*(NextP.y-pos.y) ) > sqrt( (subP.x-pos.x)*(subP.x-pos.x)+(subP.y-pos.y)*(subP.y-pos.y) ) and n < POINTS)
+        ) 
+    )
+    {
+        n++;
+        getPoint(&NextP,n);
+        getVelocity(&v,n-1);
+        if(n < POINTS)getPoint(&subP,n+1);
+        if(v > 0.0) v *= 1.7;
+        if(v >= 1400) v = 1400;
+        //v = 200.0;
+        if(n == POINTS) v = 50.0;
+        printf("%d:pass\n",n);
+    }
+    if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0;
+    double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x);
+    RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02);
+    RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getGain());
+    //RBT.sendVelocity(200.0,200.0,0.0);
+    printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
+    RBT.LOOP();
+}
+}
+
+void route_read() {
+    static double x, y, v, theta_;
+    wait(1);
+    printf("Hello World!\n");
+    wait(0.1);
+    FILE *fp;
+    do {
+        fp = fopen("/sd/route.txt", "r");
+        wait(0.2);
+    } while(fp==NULL);
+    printf("Good!\n");
+    while(fscanf(fp,"%lf %lf %lf %lf",&x, &y, &v, &theta_) != EOF) {
+        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_);
+    }
+    printf("checking...\n");
+    for(int k=0;k<x_info.size();k++){
+        printf("%lf %lf %lf %lf\n",x_info[k],y_info[k],v_info[k],theta_info[k]);
+    }
+    fclose(fp);
+}
+
+void getPoint(Position *p,int n){
+    p->x = x_info[n];
+    p->y = y_info[n];
+    p->theta = theta_info[n];
+}
+
+void getVelocity(double *v,int n){
+    *v = v_info[n];
+}
+
+bool incircle(Position pos,Position target,double error){
+    return (sqrt( (target.x-pos.x)*(target.x-pos.x)+(target.y-pos.y)*(target.y-pos.y) ) < error);
+}
+void gyroA(double theta){
+    static bool prev = false;
+    if(0 <= theta and theta < M_PI) gyro_1 = false;
+    else gyro_1 = true;
+    if(gyro_1 and !prev){
+        if(gyro_2) g_pulse++;
+        else if(g_pulse != 0) g_pulse--;
+    }
+    else if(!gyro_1 and prev){
+        if(gyro_2) g_pulse--;
+        else if(g_pulse != 0)g_pulse++;
+    }
+    prev = gyro_1;
+}
+void gyroB(double theta){
+    static bool prev = false;
+    if((3.0*M_PI/2 <= theta and theta <= 2*M_PI) or (theta < M_PI/2.0)) gyro_2 = false;
+    else gyro_2 = true;
+    if(gyro_2 and !prev){
+        if(gyro_1) g_pulse--;
+        else g_pulse++;
+    }
+    else if(!gyro_2 and prev){
+        if(gyro_1) g_pulse++;
+        else g_pulse--;
+    }
+    prev = gyro_2;
+}
+
+bool checkMOT12(int rx_data, int &tx_data){
+    MOT12 = true;
+    return true;
+}
+bool checkMOT34(int rx_data, int &tx_data){
+    MOT34 = true;
+    return true;
+}
+
+void init(){
+    MOT12 = false,MOT34 = false;
+    scrp.send1(255,5,1);
+    while(!MOT12 or !MOT34);
+}
\ No newline at end of file