Diff: main.cpp

Revision:

7:2e578dbe3a81

Parent:

6:e7b9b1d46852

Child:

8:49f527ad271c

--- a/main.cpp	Mon Oct 25 08:29:01 2021 +0000
+++ b/main.cpp	Tue Oct 26 15:55:52 2021 +0000
@@ -9,7 +9,7 @@
 
 #include "mbed.h"
 #define POINTS 900
-#define ISLAND 50
+#define ISLAND 10
 
 
 /*--------------FUNCTION--------------*/
@@ -31,6 +31,7 @@
 std::vector<double> t_info,x_info,y_info,v_info,theta_info;
 Position pos,vel,NextP,subP;
 int n=1;
+int cnt = 0;
 double v=0.0;
 double theta = 0.0;
 double gyro = 0.0;
@@ -59,10 +60,14 @@
     DigitalOut hoge(PB_2);
     hoge = x;  
       
+    PwmOut servo(PA_0);
     PwmOut ok(PC_6);
 
 /*--------------SETUP--------------*/
 
+    servo.period_ms(2048);
+    servo.pulsewidth_us(1500);
+    
     nh.getHardware()->setBaud(115200);
     nh.initNode();
     nh.subscribe(sub);
@@ -159,12 +164,15 @@
         RBT.PIDs[5]->Update(pos.theta,NextP.theta,0.02,false);
         vel.theta = RBT.PIDs[5]->getmv();
         
-        if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 10){
+
+        if(n == ISLAND and incircle(pos,NextP,40.0) and abs(pixel) > 40){
             RBT.PIDs[0]->Update(pixel,0,0.02,false);
             v = 0.0;
             vel.theta = RBT.PIDs[0]->getmv();
+            servo.pulsewidth_us(1500);
+            cnt = 0;
         }
-        else if(n == ISLAND and incircle(pos,NextP,40.0)){v = 0.0;vel.theta = 0.0;/*RBT.sendPWM(1,9);*/}
+        else if(n == ISLAND and incircle(pos,NextP,40.0)){cnt++;v = 0.0;vel.theta = 0.0;if((double)cnt > 3.0/0.02) servo.pulsewidth_us(1200);}
         
         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;