Diff: main.cpp

Revision:

1:d3985e5af20f

Parent:

0:d7ecfe1f9840

Child:

2:fe26901d9d49

diff -r d7ecfe1f9840 -r d3985e5af20f main.cpp
--- a/main.cpp	Sat Mar 05 04:57:40 2022 +0000
+++ b/main.cpp	Fri Mar 11 00:45:18 2022 +0000
@@ -7,13 +7,17 @@
 DigitalIn limitA(PB_6,PullUp);
 DigitalIn limitB(PB_12,PullUp);
 Air catch_no(PA_9);
+//Air catch_no2(PA_7);
+//Air catch_no3(PA_6);
 
 DigitalIn limitC(PB_4,PullUp);
 Air hassya(PA_8);
-Servo myservo(PB_0);
+Servo myservo(PA_11);
 Timer timer;
-int tim=-3000;
-
+int tim=-6000;
+bool limit_switch = true;
+int date=0;
+int air_date=0;
 
 
 void driveMotorS2(double pwm){
@@ -21,7 +25,7 @@
     PwmOut up_pin_B(PB_14);
     up_pin_A.period_us(2048);
     up_pin_B.period_us(2048);
-    if (!pwm) {
+    if (pwm==0) {
         up_pin_A = 0;
         up_pin_B = 0;
     } else if (0 < pwm) {
@@ -41,40 +45,44 @@
     }
 }
 //↓発射用
+int a=0;
 bool Hassya(int rx_data,int &tx_data){
-    if(rx_data == 1 && limitC==0){
+    if(rx_data==1 /*&& a==1*/){
         tim=timer.read_ms();
+        a=0;        
     }
-    if(timer.read_ms()-tim<1000){
+    if(a==1 && !limitA==0){
+        hassya.move(0);
+        driveMotorS2(0);
+        Servo(0);
+        a=1;
+    }else if(timer.read_ms()-tim<5000){
+            hassya.move(0);
+            driveMotorS2(0);
+            Servo(1);
+    }else if(timer.read_ms()-tim<6000){
             hassya.move(1);
             driveMotorS2(0);
-            Servo(1);
-    }else if(timer.read_ms()-tim<2000){
+            Servo(0);
+    }else if(timer.read_ms()-tim>6000 && !limitA==1){
+            hassya.move(1);
+            driveMotorS2(-0.06);
+            Servo(0);
+    }else {
             hassya.move(0);
             driveMotorS2(0);
             Servo(0);
-            
-    }else if(timer.read_ms()-tim>2000 && limitC==1){
-            hassya.move(0);
-            driveMotorS2(0.06);
-            Servo(0);
-           
-    }else {
-            hassya.move(1);
-            driveMotorS2(0);
-            Servo(0);
-           
     }
     return true;
 }
 
 //↓モーター
 void driveMotorS(double pwm){
-    PwmOut up_pin_A1(PA_0);
-    PwmOut up_pin_B1(PA_1);
+    PwmOut up_pin_A1(PA_1);
+    PwmOut up_pin_B1(PA_0);
     up_pin_A1.period_us(2048);
     up_pin_B1.period_us(2048);
-    if (!pwm) {
+    if (pwm==0) {
         up_pin_A1 = 0;
         up_pin_B1 = 0;
     } else if (0 < pwm) {
@@ -88,10 +96,17 @@
 //↓昇降
 //速さはテキトー
 bool Up(int rx_data,int &tx_data){
-    if(rx_data==1 && !limitA){
-        driveMotorS(0.3);
-    }else if(rx_data==-1 && !limitB){
-        driveMotorS(-0.3);
+    
+    if(rx_data==1){
+    date=1;
+    }
+    if(rx_data==-1){
+    date=-1    ;
+    }
+    if(date==1 && !limitA){
+        driveMotorS(0.6);
+    }else if(date==-1 && !limitB){
+        driveMotorS(-0.6);
     }else{
         driveMotorS(0);
     }
@@ -99,14 +114,48 @@
 }
 //↓回収用
 bool CATCH_NO(int rx_data,int &tx_data){
-    catch_no.move(rx_data);
+    if(tx_data==1){
+        air_date+=1;    
+    }else if(tx_data==2&&air_date==0){
+        air_date=1;    
+    }
+    catch_no.move(air_date%2);
+    return true;
+    }
+
+bool CATCH_NO2(int rx_data,int &tx_data){
+    //catch_no2.move(rx_data);
+    //catch_no3.move(rx_data);
     return true;
     }
+    
+    
+bool limit2(int id,int ppp){
+    if(ppp==1){
+        limit_switch=false;
+        driveMotorS(0);
+        driveMotorS2(0);
+    }
+    return true;
+}
+
+bool  limit(int a,int &b){
+    return limit2(0,a);
+}
+
+
 
 int main(){
     timer.start();
-    slave.addCMD(17,Hassya);
+    slave.addCMD(1,limit);
+    slave.addCMD(22,Hassya);
     slave.addCMD(20,CATCH_NO);
     slave.addCMD(21,Up);
-    while(true);
+    //slave.addCMD(22,CATCH_NO2);
+    while(limit_switch){
+        
+     
+    }
+    driveMotorS(0);
+    driveMotorS2(0);
 }
\ No newline at end of file