Diff: main.cpp

Revision:

3:ccd2dd7d3814

Parent:

2:fe26901d9d49

Child:

4:7e2aa008f84b

--- a/main.cpp	Sat Mar 12 01:21:35 2022 +0000
+++ b/main.cpp	Sat Mar 19 02:12:50 2022 +0000
@@ -5,20 +5,26 @@
 ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807ffff);
 Serial pc(SERIAL_TX,SERIAL_RX);
 DigitalIn limitA(PB_6,PullUp);
-DigitalIn limitB(PB_12,PullUp);
+DigitalIn limitB(PB_4,PullUp);
+
+DigitalIn limitA2(PC_2,PullUp);
+DigitalIn limitB2(PC_3,PullUp);
+
 Air catch_no(PA_9);
-Air catch_no2(PA_7);
+Air catch_no2(PB_8);
 //Air catch_no3(PA_6);
 
-DigitalIn limitC(PB_4,PullUp);
+DigitalIn limitC(PB_5,PullUp);
 Air hassya(PA_8);
 Servo myservo(PA_11);
 Timer timer;
-int tim=-6000;
+int tim=-2000;
 bool limit_switch = true;
 int date=0;
+int date2=0;
 int air_date=0;
 int air_date2=0;
+int limit_air=0;
 int a=0;
 
 
@@ -52,22 +58,20 @@
         tim=timer.read_ms();
         a=0;        
     }
-    if(a==1 || !limitC==0){
-            hassya.move(0);
-            driveMotorS2(0);
-            Servo(0);
+    if(!limitC==0){
             a=1;
-    }else if(timer.read_ms()-tim<5000){
+    }
+    if(timer.read_ms()-tim<1000){
             hassya.move(0);
             driveMotorS2(0);
             Servo(1);
-    }else if(timer.read_ms()-tim<6000){
+    }else if(timer.read_ms()-tim<2000){
             hassya.move(1);
             driveMotorS2(0);
             Servo(0);
-    }else if(timer.read_ms()-tim>6000 && !limitC==1){
+    }else if(timer.read_ms()-tim>2000 && a==0){
             hassya.move(1);
-            driveMotorS2(-0.06);
+            driveMotorS2(-0.15);
             Servo(0);
     }else{
             hassya.move(0);
@@ -79,8 +83,8 @@
 
 //↓モーター
 void driveMotorS(double pwm){
-    PwmOut up_pin_A1(PA_1);
-    PwmOut up_pin_B1(PA_0);
+    PwmOut up_pin_A1(PA_0);
+    PwmOut up_pin_B1(PA_1);
     up_pin_A1.period_us(2048);
     up_pin_B1.period_us(2048);
     if (pwm==0) {
@@ -94,6 +98,25 @@
         up_pin_B1= -pwm;
     }
 }
+
+void driveMotorS3(double pwm){
+    PwmOut up_pin_A3(PC_9);
+    PwmOut up_pin_B3(PC_8);
+    up_pin_A3.period_us(2048);
+    up_pin_B3.period_us(2048);
+    if (pwm==0) {
+        up_pin_A3 = 0;
+        up_pin_B3 = 0;
+    } else if (0 < pwm) {
+        up_pin_A3 = pwm;
+        up_pin_B3 = 0;
+    } else {
+        up_pin_A3 = 0;
+        up_pin_B3= -pwm;
+    }
+}
+
+
 //↓昇降
 //速さはテキトー
 bool Up(int rx_data,int &tx_data){
@@ -102,7 +125,7 @@
     date=1;
     }
     if(rx_data==-1){
-    date=-1    ;
+    date=-1;
     }
     if(date==1 && !limitA){
         driveMotorS(0.4);
@@ -113,6 +136,36 @@
     }
     return true;
 }
+
+//↓昇降
+//速さはテキトー
+bool Up2(int rx_data,int &tx_data){
+    
+    if(rx_data==1){
+    date2=1;
+    }
+    if(rx_data==-1){
+    date2=-1;
+    }
+    if(!limitA2){
+        limit_air=1;
+    }
+    if(!limitB2){
+        limit_air=-1;
+    }
+    
+    if(date2==1 && !limitA2){
+        driveMotorS3(0.6);
+        limit_air=0;
+    }else if(date2==-1 &&!limitB2){
+        driveMotorS3(-0.4);
+        limit_air=0;
+    }else{
+        driveMotorS3(0);
+    }
+    return true;
+}
+
 //↓回収用
 bool CATCH_NO(int rx_data,int &tx_data){
     if(tx_data==1){
@@ -125,12 +178,12 @@
 }
 
 bool CATCH_NO2(int rx_data,int &tx_data){
-    air_date+=tx_data;
-    catch_no2.move(air_date%2);
-    //catch_no3.move(rx_data);
+    air_date2+=tx_data;
+    catch_no2.move(air_date2%2);
     return true;
-    }
     
+}
+
     
 bool limit2(int id,int ppp){
     if(ppp==1){
@@ -153,7 +206,8 @@
     slave.addCMD(22,Hassya);
     slave.addCMD(20,CATCH_NO);
     slave.addCMD(21,Up);
-    //slave.addCMD(22,CATCH_NO2);
+    slave.addCMD(23,CATCH_NO2);
+    slave.addCMD(24,Up2);
     while(limit_switch){