Revision 3:8c4e42cba9cb, committed 2019-09-12

Comitter:

shina

Date:

Thu Sep 12 09:03:59 2019 +0000

Parent:

2:e462c8257384

Commit message:

This program is about manual robot.

Changed in this revision

--- a/main.cpp	Thu Sep 05 10:28:09 2019 +0000
+++ b/main.cpp	Thu Sep 12 09:03:59 2019 +0000
@@ -80,8 +80,11 @@
 int old_left1=0;
 int p=2;
 int q=0;
-
-
+int old_ue1=0;
+int old_shita=0;
+int old_left2=0;
+int old_triangle=0;
+int old_cross=0;
 
 
 //関数プロトタイプ宣言
@@ -90,7 +93,7 @@
 void change_data();
 void change_pwm();
 void change_servo();
-void change_rack();
+//void change_rack();
 void change_fan();
 void send_data(char address,char data);
 void emergency();
@@ -99,6 +102,7 @@
 //void kaisyu();
 void sequence_hakidashi();
 //void hakidashi();
+void mode_change();
 
 
 
@@ -106,14 +110,16 @@
 int main()
 {
     led=1;
+    pwm=1;
     initialization();
     while(true) {
         emergency();
         get_data();
+        mode_change();
         change_pwm();
         change_servo();
         change_data();
-        change_rack();
+        //change_rack();
         change_rack_2();
         sequence_kaisyu();
         //kaisyu();
@@ -167,7 +173,19 @@
     send_data(0x40,data10);
     send_data(0x30,data11);
 
-
+    data10=0x3f;
+    send_data(0x40,data10);
+    
+    if(q==0){
+        data10=0xdf;
+        send_data(0x40,data10);
+    }else if(q==1){
+        data10=0xcf;
+        send_data(0x40,data10);
+    }
+    
+    
+    
 }
 
 
@@ -230,12 +248,33 @@
 }
 
 
+//モードチェンジ
+void mode_change(){
+    if(old_select!=select){
+        old_select=select;
+        if(select==1){
+            if(q==0){
+                //送風モード
+                q=1;
+                data10=0xcf;
+                send_data(0x40,data10);
+            }else if(q==1){
+                //回収モード
+                q=0;
+                data10=0xdf;
+                send_data(0x40,data10);
+            }
+        }
+    }
+}
+
 //データ変化(メカナム)
 void change_data()
 {
 
     if(Ry!=0||Rx!=0) {
         square=0;
+        migi1=0;
     }
 
     if(Ry>30&&i==0) {
@@ -256,10 +295,22 @@
             data4=0x00;
         }
     } else if(Ry<-30&&i==0) {
-        data1=0xff;
-        data2=0xff;
-        data3=0xff;
-        data4=0xff;
+            if(right1==1){
+                data1=0xdf;
+                data2=0xff;
+                data3=0xdf;
+                data4=0xff;
+            }else if(left1==1){
+                data1=0xff;
+                data2=0xdf;
+                data3=0xff;
+                data4=0xdf;
+            }else{
+                data1=0xff;
+                data2=0xff;
+                data3=0xff;
+                data4=0xff;
+            }
     } else if(Rx>30&&i==0) {
         if(right1==1) {
             data1=0xff;
@@ -322,10 +373,22 @@
             data4=0x3f;
         }
     } else if(Ry<-30&&i==1) {
-        data1=0xbf;
-        data2=0xbf;
-        data3=0xbf;
-        data4=0xbf;
+        if(right1==1){
+            data1=0x5f;
+            data2=0xbf;
+            data3=0x5f;
+            data4=0xbf;
+        }else if(left1==1){
+            data1=0xbf;
+            data2=0x5f;
+            data3=0xbf;
+            data4=0x5f;
+        }else{
+            data1=0xbf;
+            data2=0xbf;
+            data3=0xbf;
+            data4=0xbf;
+        }
     } else if(Rx>30&&i==1) {
         if(right1==1) {
             data1=0xbf;
@@ -382,14 +445,18 @@
 //pwm変化
 void change_pwm()
 {
-    if(select!=old_select) {
-        old_select=select;
-        if(select==1) {
+    if(cross!=old_cross) {
+        old_cross=cross;
+        if(cross==1) {
             if(i==1) {
                 pwm=0;
+                data10=0xbf;
+                send_data(0x40,data10);
                 i=0;
             } else if(i==0) {
                 pwm=1;
+                data10=0x3f;
+                send_data(0x40,data10);
                 i=1;
             }
         }
@@ -444,6 +511,7 @@
 }
 
 //ラック
+/*
 void change_rack()
 {
 
@@ -481,10 +549,12 @@
     }
 
 }
+*/
 
 //ラック左右
 void change_rack_2()
 {
+    /*
     if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) {
         if(get_data_rs232 & 0x01) {
             data5=0x10;
@@ -511,12 +581,46 @@
         data5=0x80;
         data6=0x80;
     }
+    */
+    
+    if(ue1!=old_ue1){
+        old_ue1=ue1;
+        if(ue1==1){
+            data5=0x00;
+            data6=0xff;
+        }
+    }else if(shita!=old_shita){
+        old_shita=shita;
+        if(shita==1){
+            data5=0xff;
+            data6=0x00;
+        }
+    }
+    
+    if((get_data_rs232 & 0x01)&&data5==0x00){
+        data5=0x10;
+        send_data(0x20,data5);
+    }else if((get_data_rs232 & 0x04)&&data5==0xff){
+        data5=0x10;
+        send_data(0x20,data5);
+    }
+    
+    if((get_data_rs232 & 0x02)&&data6==0xff){
+        data6=0x10;
+        send_data(0x22,data6);
+    }else if((get_data_rs232 & 0x08)&&data6==0x00){
+        data6=0x10;
+        send_data(0x22,data6);
+    }
+    
+    
 }
 
 //回収機構
 
 void sequence_kaisyu()
 {
+    if(q==0){
     if(square!=old_square) {
         old_square=square;
         if(square==1) {
@@ -541,9 +645,8 @@
             }
             data7=0x10;
             send_data(0x24,data7);
-            data_servo=0x07;
-            send_data(0x18,data_servo);
 
+            }
         }
     }
 }
@@ -583,28 +686,47 @@
 
 void sequence_hakidashi()
 {
+    if(q==0){
     if(migi1!=old_migi1) {
         old_migi1=migi1;
-        if(migi1==1) {
-            /*  data_servo=0x07;
+        if(migi1==1&&select==0) {
+              data_servo=0x07;
               send_data(0x18,data_servo);
-              wait(1.5);
+              wait(1);
+              data7=0x00;
+              send_data(0x24,data7);
+              wait(0.5);
+              data7=0x10;
+              send_data(0x24,data7);
               data11=0xff;
-              while(get_data_rs232!=0x36){
+              while(!(get_data_rs232 & 0x40)){
                   send_data(0x30,data11);
                   get_data_rs232=slave.getc();
-                  }
-               data11=0x10;
-               send_data(0x30,data11);*/
-            data11=0xff;
-            send_data(0x30,data11);
+              }
+              data11=0x10;
+              send_data(0x38,data11);
+              data11=0x00;
+              while(!(get_data_rs232 & 0x80)){
+                  send_data(0x30,data11);
+                  get_data_rs232=slave.getc();
+              }
+              data11=0x10;
+              send_data(0x30,data11);
+              data7=0xff;
+              while(!(get_data_rs232 & 0x20)){
+                  send_data(0x24,data7);
+                  get_data_rs232=slave.getc();
+              }
+              data7=0x10;
+              send_data(0x24,data7);
+ 
         }
-    } else if(hidari1!=old_hidari1) {
+    } /*else if(hidari1!=old_hidari1) {
         if(hidari1==1) {
             data11=0x00;
             send_data(0x30,data11);
         }
-    }  /*   else if(hidari1==1){
+    }     else if(hidari1==1){
                     if(get_data_rs232!=0x38){
                         data11=0x38;
                         send_data(0x30,data11);
@@ -612,11 +734,11 @@
                             data11=0x10;
                             send_data(0x30,data11;
                         }
-        }*/ /*else{
+        } */ /*else{
                  data11=0x80;
                  send_data(0x30,data11);
                  }*/
-
+    /*
     if((get_data_rs232 & 0x80)&&(data11==0x00)) {
         data11=0x10;
         send_data(0x30,data11);
@@ -624,6 +746,7 @@
         data11=0x10;
         send_data(0x30,data11);
 
+    }*/
     }
 }
 
@@ -654,16 +777,42 @@
 //ファン
 void change_fan()
 {
-    if(left2==1) {
+    if(q==1){   
+    if(left2==1&&(old_triangle==triangle)) {
         data8=0xff;
         data9=0xff;
         data10=0xff;
-    } else if(left2==0) {
+    } else if(left2==0&&(old_triangle==triangle)) {
         data8=0x00;
         data9=0x00;
         data10=0x00;
+    }else if(left2==0&&(old_triangle!=triangle)&&select==0){
+        old_triangle=triangle;
+        if(triangle==1){
+            data8=0xff;
+            data9=0xff;
+            data10=0xff;
+            data_servo=0x03;
+            send_data(0x26,data8);
+            send_data(0x28,data9);
+            send_data(0x40,data10);
+            wait(1);
+            send_data(0x18,data_servo);
+            wait(1);
+            data8=0x00;
+            data9=0x00;
+            data10=0x00;
+            send_data(0x26,data8);
+            send_data(0x28,data9);
+            send_data(0x40,data10);
+            wait(0.5);
+            data_servo=0x04;
+            send_data(0x18,data_servo);
+        }
     }
 }
+    
+}
 
 
 
@@ -673,11 +822,9 @@
 {
     tsushin=1;
     tushin_check=0;
-    i2c.frequency(100000);
     i2c.start();
     i2c.write(address);
     tushin_check=i2c.write(data);
     i2c.stop();
-    wait(0.003);
 }