Diff: main.cpp

Revision:

4:0038da6cdc9a

Parent:

2:4e7ec0816a91

Child:

5:aee4416b0d4f

--- a/main.cpp	Fri Oct 22 09:42:22 2021 +0000
+++ b/main.cpp	Sun Oct 24 04:48:27 2021 +0000
@@ -4,14 +4,18 @@
 #include "BNO055.h"
 #include <cmath>
 #include <vector>
+#include <ros.h>
+#include <std_msgs/Int16.h>
 
 #include "mbed.h"
 #define POINTS 900
+#define ISLAND 50
 
 
 /*--------------FUNCTION--------------*/
 bool init();
 void route_read();
+void spinonce();
 void getPoint(Position *p,int n);
 void getVelocity(double *v,int n);
 bool incircle(Position pos,Position target,double error);
@@ -19,122 +23,147 @@
 void gyroB(double theta);
 bool checkMOT12(int rx_data, int &tx_data);
 bool checkMOT34(int rx_data, int &tx_data);
+void getPixel(const std_msgs::Int16& pix);
 
 
 /*--------------VARIABLE--------------*/
 
 std::vector<double> t_info,x_info,y_info,v_info,theta_info;
-Position pos,NextP,subP;
+Position pos,vel,NextP,subP;
 int n=1;
 double v=0.0;
 double theta = 0.0;
 double gyro = 0.0;
+int pixel = 0.0;
 bool gyro_1 = false,gyro_2 = false;
 long long g_pulse;
 bool MOT12 = false,MOT34 = false;
 
 BNO055 bno(PB_3, PB_10);
 
+ros::NodeHandle nh;
+Ticker rosspin;
+
 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);
 
+ros::Subscriber<std_msgs::Int16> sub("pixel", &getPixel);
+
 int main(){
     
-PwmOut ok(PC_6);
+    PwmOut ok(PC_6);
 
 /*--------------SETUP--------------*/
-scrp.addCMD(7,checkMOT12);
-scrp.addCMD(8,checkMOT34);
-AKASHIKOSEN.setCWID(1,2,3,4);
-AKASHIKOSEN.setSWID(5,6,7,8);
+
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.subscribe(sub);
+    rosspin.attach_us(&spinonce,100);
+        
+    scrp.addCMD(7,checkMOT12);
+    scrp.addCMD(8,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.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.8,0.00,0.0,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
-//RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
-/*
-RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5);
-RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5);
-RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5);
-RBT.addPID(0.0048,0.00,0.00,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);
-*/
-MOT12 = false,MOT34 = false;
-
-while(init());
-printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34);
-route_read();
-
-wait(1);
-ok = 1.0;
-
-bool setup = false;
-if(!setup){
-    bno.reset();
-    bno.setmode(OPERATION_MODE_IMUPLUS);
-    setup = true;
-}
+    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.8,0.00,0.0,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
+    RBT.addPID(0.01,0.00,0.0,0)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
+    //RBT.addPID(0.5,0.001,0.2,5)->setLimit(2.0*M_PI/8.0,-2.0*M_PI/8.0);
+    /*
+    RBT.addPID(0.0048,0.00,0.0000,1)->setLimit(0.5,-0.5);
+    RBT.addPID(0.0048,0.00,0.0000,2)->setLimit(0.5,-0.5);
+    RBT.addPID(0.0048,0.00,0.00,3)->setLimit(0.5,-0.5);
+    RBT.addPID(0.0048,0.00,0.00,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);
+    */
+    MOT12 = false,MOT34 = false;
+    
+    while(init());
+    printf("SCRP_CHECKING MOT12:%d,MOT34:%d\n",MOT12,MOT34);
+    route_read();
+    
+    wait(1);
+    ok = 1.0;
     
-RBT.START();
-RBT.setPosition(0.0,0.0,0.0);
-/*--------------LOOP--------------*/
-while(true){
-    pos = RBT.getStatus(true);
-    
-    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,40.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);
-        NextP.theta -= M_PI/2.0;
-        getVelocity(&v,n-1);
-        if(n < POINTS)getPoint(&subP,n+1);
-        if(v > 0.0) v *= 2.0;
-        if(v >= 1400) v = 1400;
-        //v = 20.0;
-        if(n == POINTS) v = 20.0;
-        printf("%d:pass\n",n);
+    bool setup = false;
+    if(!setup){
+        bno.reset();
+        bno.setmode(OPERATION_MODE_IMUPLUS);
+        setup = true;
     }
-    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,false);
-    //RBT.sendVelocity(v*cos(etheta),v*sin(etheta),RBT.PIDs[5]->getmv());
-    //printf("%lf,%lf\n",pos.theta,NextP.theta);
-    //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();
-}
+        
+    RBT.START();
+    RBT.setPosition(0.0,0.0,0.0);
+    /*--------------LOOP--------------*/
+    while(true){
+        pos = RBT.getStatus(true);
+        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,40.0) and n < POINTS and n != 1000 and n != ISLAND) 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);
+            NextP.theta -= M_PI/2.0;
+            getVelocity(&v,n-1);
+            if(n < POINTS)getPoint(&subP,n+1);
+            if(v > 0.0) v *= 1.5;
+            if(v >= 1400) v = 1400;
+            //v = 20.0;
+            if(n == POINTS) v = 20.0;
+            printf("%d:pass\n",n);
+        }
+        
+        double etheta = atan2(NextP.y-pos.y,NextP.x-pos.x);
+        RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false);
+        vel.theta = RBT.PIDs[5]->getmv();
+        
+        if(n == ISLAND and incircle(pos,NextP,10.0) and abs(pixel) > 10){
+            RBT.PIDs[0]->Update(pixel,0,0.02,false);
+            vel.theta = RBT.PIDs[0]->getmv();
+        }
+        else {RBT.sendPWM(1,9);}
+        
+        if(n == POINTS and incircle(pos,NextP,1.0)) v = 0.0;
+        if(n == POINTS and fabs(NextP.theta - pos.theta) < 0.1) vel.theta = 0.0;
+        
+        vel.x = v*cos(etheta);
+        vel.y = v*sin(etheta);
+        
+        RBT.sendVelocity(vel.x,vel.y,vel.theta);
+        printf("x:%lf,y:%lf,theta:%lf,n:%d\n",pos.x,pos.y,pos.theta,n);
+        RBT.LOOP();
+    }
 }
 
 void route_read() {
@@ -219,4 +248,12 @@
     wait(0.25);
     scrp.send1(255,6,1);
     return (!MOT12 or !MOT34);
-}
\ No newline at end of file
+}
+void spinonce()
+{
+    nh.spinOnce();
+}
+
+void getPixel(const std_msgs::Int16& pix){
+    pixel = pix.data;
+    }