Diff: main.cpp

Revision:

1:1f9a3449bbb9

Parent:

0:c60fccf1ea71

--- a/main.cpp	Fri Apr 01 00:20:40 2022 +0000
+++ b/main.cpp	Fri Apr 08 09:31:13 2022 +0000
@@ -7,9 +7,9 @@
  
  
 #define goal_x -17700.0
-#define goal_y -23800.0
+#define goal_y -23800.0//-23800.0
 #define goal_x2 -8000000
-#define goal_y2 -8000000
+#define goal_y2 -80000000
 #define period_r 8000.0
 NeoPixelOut npx(PB_4,7);
 I2C i2d(PB_3,PB_10);
@@ -36,7 +36,7 @@
 int check_tepu=0;
  
 bool limi=true;
- 
+bool liset_abc=false;
 
  
 DigitalIn limitB(PB_12,PullUp);
@@ -102,7 +102,7 @@
     return true;
 }
 bool pro2(){  //アングルとスピードを同時に決める　下から3桁で角度を決める(0~360)　それ以外でスピードを決める　これなに？私もわからん
-    double anglert=-135.000000000;
+    double anglert=-135.000000000+180.0;
     Led4.write(1);
     for(int i=0;i<4;i++){
     target[i]=(direct_yy*sin(M_PI/180.0*(90*i - gyro.yaw - anglert))-direct_xx*cos(M_PI/180.0*(90*i - gyro.yaw - anglert))+direct_turn)*30.0;
@@ -216,8 +216,17 @@
     check_tepu+=1;
     return direct_tu(0,a);
 }
+bool  abc14(int a,int &b){
+    if(a==1){
+        liset_abc=true;
+    }else{
+        liset_abc=false;
+    }
+    return true;
+}
 
  
+ 
 int main() {
    Led1.write(1);
    Led2.write(0);
@@ -231,7 +240,7 @@
     slave.addCMD(31,abc11);
     slave.addCMD(32,abc12);
     slave.addCMD(33,abc13);
-    
+    slave.addCMD(34,abc14);
     int i;    
     double x_period=0;
     double y_period=0;
@@ -384,6 +393,12 @@
         */
         x_period+=(((speed_pwm[0]-speed_pwm[2])*cos(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*cos(M_PI/180.0*(angle+90.0))*(-1.0))/2);
         y_period+=(((speed_pwm[0]-speed_pwm[2])*sin(M_PI/180.0*angle)+(speed_pwm[1]-speed_pwm[3])*sin(M_PI/180.0*(angle+90.0))*(-1.0))/2);
+        if(liset_abc==true){
+            x_period=0;
+            y_period=0;
+            gyro.reset(0);
+            
+        }
         slave.port2.printf("%d\n",1);
         while(name.read_ms()<30){}
         slave.send1(255,16,x_period*1);