Diff: main.cpp

Revision:

2:e462c8257384

Parent:

1:99294241f2ba

Child:

3:8c4e42cba9cb

--- a/main.cpp	Thu Sep 05 09:40:50 2019 +0000
+++ b/main.cpp	Thu Sep 05 10:28:09 2019 +0000
@@ -45,7 +45,7 @@
 char data9;//左ファン
 char data10;//パトランプ
 char data11;//吐き出し機構
-char get_data_rs232=0x80;
+char get_data_rs232=0x00;
 int Ry;
 int Rx;
 int Ly;
@@ -103,10 +103,11 @@
 
 
 //メイン関数
-int main(){
+int main()
+{
     led=1;
     initialization();
-    while(true){
+    while(true) {
         emergency();
         get_data();
         change_pwm();
@@ -115,9 +116,9 @@
         change_rack();
         change_rack_2();
         sequence_kaisyu();
-       //kaisyu();
-       sequence_hakidashi();
-       //hakidashi();
+        //kaisyu();
+        sequence_hakidashi();
+        //hakidashi();
         change_fan();
         send_data(0x10,data1);
         send_data(0x12,data2);
@@ -131,16 +132,17 @@
         send_data(0x28,data9);
         send_data(0x40,data10);
         //send_data(0x30,data11);
-        
+
+
+
 
-        
-        
-        }
     }
-    
-    
+}
+
+
 //初期化
-void initialization(){
+void initialization()
+{
     data1=0x80;
     data2=0x80;
     data3=0x80;
@@ -152,7 +154,7 @@
     data9=0x80;
     data10=0x80;
     data11=0x80;
-    
+
     send_data(0x10,data1);
     send_data(0x12,data2);
     send_data(0x14,data3);
@@ -164,13 +166,14 @@
     send_data(0x28,data9);
     send_data(0x40,data10);
     send_data(0x30,data11);
-    
-    
-    }    
+
+
+}
 
-    
+
 //データ読み込み
-void get_data(){
+void get_data()
+{
     Ry=ps3.getRightJoystickYaxis();
     Rx=ps3.getRightJoystickXaxis();
     Ly=ps3.getLeftJoystickYaxis();
@@ -189,241 +192,245 @@
     hidari1=ps3.getButtonState(hidari);
     migi1=ps3.getButtonState(migi);
     get_data_rs232=slave.getc();
-    
+
 
-     if(get_data_rs232==0x20||get_data_rs232==0x22||get_data_rs232==0x24||get_data_rs232==0x32||get_data_rs232==0x34){
+    if(get_data_rs232) {
         led1=1;
-        }else{
-            led1=0;
-            }
+    } else {
+        led1=0;
+    }
     pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,Ly,left1,left2,right1,right2,select,start,ue1,shita,triangle,square,migi1,hidari1,tushin_check,i);
-    if(Ry==0&&Rx==0&&Ly==0&&left1==0&&right1==0&&right2==0&&left2==0&&select==0&&start==0&&shita==0&&triangle==0&&ue1==0&&migi1==0&&hidari1==0&&square==0&&circle==0&&cross==0){
+    if(Ry==0&&Rx==0&&Ly==0&&left1==0&&right1==0&&right2==0&&left2==0&&select==0&&start==0&&shita==0&&triangle==0&&ue1==0&&migi1==0&&hidari1==0&&square==0&&circle==0&&cross==0) {
         data_check=0;
-        }else{
-            data_check=1;
-            }
-    
+    } else {
+        data_check=1;
     }
-    
+
+}
+
 //緊急停止
-void emergency(){
-    if(start!=old_start){
-    old_start=start;
-    if(start==1){
-        if(j==1){
-    //緊急停止
-            led=0;
-            stop=1;
-            j=0;
-        }else if(j==0){
-    //緊急停止解除
-            led=1;
-            stop=0;
-            j=1;
+void emergency()
+{
+    if(start!=old_start) {
+        old_start=start;
+        if(start==1) {
+            if(j==1) {
+                //緊急停止
+                led=0;
+                stop=1;
+                j=0;
+            } else if(j==0) {
+                //緊急停止解除
+                led=1;
+                stop=0;
+                j=1;
             }
         }
-    }  
+    }
 }
 
-    
+
 //データ変化(メカナム)
-void change_data(){
-    
-        if(Ry!=0||Rx!=0){
+void change_data()
+{
+
+    if(Ry!=0||Rx!=0) {
         square=0;
+    }
+
+    if(Ry>30&&i==0) {
+        if(right1==1) {
+            data1=0xcf;
+            data2=0x00;
+            data3=0xcf;
+            data4=0x00;
+        } else if(left1==1) {
+            data1=0x00;
+            data2=0xcf;
+            data3=0x00;
+            data4=0xcf;
+        } else {
+            data1=0x00;
+            data2=0x00;
+            data3=0x00;
+            data4=0x00;
         }
-    
-    if(Ry>30&&i==0){
-            if(right1==1){
-                data1=0xcf;
-                data2=0x00;
-                data3=0xcf;
-                data4=0x00;
-            }else if(left1==1){
-                data1=0x00;
-                data2=0xcf;
-                data3=0x00;
-                data4=0xcf;
-            }else{
-                data1=0x00;
-                data2=0x00;
-                data3=0x00;
-                data4=0x00;
-            }
-        }else if(Ry<-30&&i==0){
+    } else if(Ry<-30&&i==0) {
         data1=0xff;
         data2=0xff;
         data3=0xff;
         data4=0xff;
-        }else if(Rx>30&&i==0){
-            if(right1==1){
-                data1=0xff;
-                data2=0xdf;
-                data3=0xcf;
-                data4=0x00;
-            }else if(left1==1){
-                data1=0xdf;
-                data2=0xff;
-                data3=0x00;
-                data4=0xcf;
-            }else{
-        data1=0xff;
-        data2=0xff;
-        data3=0x00;
-        data4=0x00;
-         }
-        }else if(Rx<-30&&i==0){
-             if(right1==1){
-                data1=0xcf;
-                data2=0x00;
-                data3=0xff;
-                data4=0xdf;
-            }else if(left1==1){
-                data1=0x00;
-                data2=0xcf;
-                data3=0xdf;
-                data4=0xff;
-            }else{
-        data1=0x00;
-        data2=0x00;
-        data3=0xff;
-        data4=0xff;
+    } else if(Rx>30&&i==0) {
+        if(right1==1) {
+            data1=0xff;
+            data2=0xdf;
+            data3=0xcf;
+            data4=0x00;
+        } else if(left1==1) {
+            data1=0xdf;
+            data2=0xff;
+            data3=0x00;
+            data4=0xcf;
+        } else {
+            data1=0xff;
+            data2=0xff;
+            data3=0x00;
+            data4=0x00;
         }
-        }else if(right1==1&&i==0){
+    } else if(Rx<-30&&i==0) {
+        if(right1==1) {
+            data1=0xcf;
+            data2=0x00;
+            data3=0xff;
+            data4=0xdf;
+        } else if(left1==1) {
+            data1=0x00;
+            data2=0xcf;
+            data3=0xdf;
+            data4=0xff;
+        } else {
+            data1=0x00;
+            data2=0x00;
+            data3=0xff;
+            data4=0xff;
+        }
+    } else if(right1==1&&i==0) {
         data1=0xff;
         data2=0x00;
         data3=0xff;
         data4=0x00;
-        }else if(left1==1&&i==0){
+    } else if(left1==1&&i==0) {
         data1=0x00;
         data2=0xff;
         data3=0x00;
         data4=0xff;
-        }else if(Ry>30&&i==1){
-                 if(right1==1){
-                    data1=0x4f;
-                    data2=0x3f;
-                    data3=0x4f;
-                    data4=0x3f;
-                }else if(left1==1){
-                    data1=0x3f;
-                    data2=0x4f;
-                    data3=0x3f;
-                    data4=0x4f;
-                }else{
-                    data1=0x3f;
-                    data2=0x3f;
-                    data3=0x3f;
-                    data4=0x3f;
+    } else if(Ry>30&&i==1) {
+        if(right1==1) {
+            data1=0x4f;
+            data2=0x3f;
+            data3=0x4f;
+            data4=0x3f;
+        } else if(left1==1) {
+            data1=0x3f;
+            data2=0x4f;
+            data3=0x3f;
+            data4=0x4f;
+        } else {
+            data1=0x3f;
+            data2=0x3f;
+            data3=0x3f;
+            data4=0x3f;
         }
-        }else if(Ry<-30&&i==1){
+    } else if(Ry<-30&&i==1) {
         data1=0xbf;
         data2=0xbf;
         data3=0xbf;
         data4=0xbf;
-        }else if(Rx>30&&i==1){
-             if(right1==1){
-                data1=0xbf;
-                data2=0x5f;
-                data3=0x4f;
-                data4=0x3f;
-            }else if(left1==1){
-                data1=0x5f;
-                data2=0xbf;
-                data3=0x3f;
-                data4=0x4f;
-            }else{
-        data1=0xbf;
-        data2=0xbf;
-        data3=0x3f;
-        data4=0x3f;
+    } else if(Rx>30&&i==1) {
+        if(right1==1) {
+            data1=0xbf;
+            data2=0x5f;
+            data3=0x4f;
+            data4=0x3f;
+        } else if(left1==1) {
+            data1=0x5f;
+            data2=0xbf;
+            data3=0x3f;
+            data4=0x4f;
+        } else {
+            data1=0xbf;
+            data2=0xbf;
+            data3=0x3f;
+            data4=0x3f;
         }
-        }else if(Rx<-30&&i==1){
-             if(right1==1){
-                data1=0x4f;
-                data2=0x3f;
-                data3=0xbf;
-                data4=0x5f;
-            }else if(left1==1){
-                data1=0x3f;
-                data2=0x4f;
-                data3=0x5f;
-                data4=0xbf;
-            }else{
-        data1=0x3f;
-        data2=0x3f;
-        data3=0xbf;
-        data4=0xbf;
+    } else if(Rx<-30&&i==1) {
+        if(right1==1) {
+            data1=0x4f;
+            data2=0x3f;
+            data3=0xbf;
+            data4=0x5f;
+        } else if(left1==1) {
+            data1=0x3f;
+            data2=0x4f;
+            data3=0x5f;
+            data4=0xbf;
+        } else {
+            data1=0x3f;
+            data2=0x3f;
+            data3=0xbf;
+            data4=0xbf;
         }
-        }else if(right1==1&&i==1){
+    } else if(right1==1&&i==1) {
         data1=0xbf;
         data2=0x3f;
         data3=0xbf;
         data4=0x3f;
-        }else if(left1==1&&i==1){
+    } else if(left1==1&&i==1) {
         data1=0x3f;
         data2=0xbf;
         data3=0x3f;
         data4=0xbf;
-        }else{
+    } else {
         data1=0x80;
         data2=0x80;
         data3=0x80;
         data4=0x80;
-        }
     }
+}
 
 
-//pwm変化    
-void change_pwm(){
-if(select!=old_select){
-    old_select=select;
-    if(select==1){
-    if(i==1){
-        pwm=0;
-        i=0;
-        }else if(i==0){
-            pwm=1;
-            i=1;
+//pwm変化
+void change_pwm()
+{
+    if(select!=old_select) {
+        old_select=select;
+        if(select==1) {
+            if(i==1) {
+                pwm=0;
+                i=0;
+            } else if(i==0) {
+                pwm=1;
+                i=1;
             }
         }
     }
 }
 
 //サーボモーター
-void change_servo(){
-        if(right2!=old_right2){
-                    old_right2=right2;
-                    //ハンガー機構
-                    if(right2==1){
-                        if(o==1){
-                            data_servo=0x01;
-                            //send_data(0x18,data_servo);
-                            o=2;
-                        }else if(o==2){
-                            data_servo=0x02;
-                            //send_data(0x18,data_servo);
-                            o=3;
-                        }
-                        //data_servo=0x01;
-                    }
-            }else if(circle!=old_circle){
-                    old_circle=circle;
-                    //バスタオル挟む機構
-                    if(circle==1){
-                        if(k==1){
-                            data_servo=0x03;
-                            //send_data(0x18,data_servo);
-                            k=0;
-                        }else if(k==0){
-                            data_servo=0x04;
-                            //send_data(0x18,data_servo);
-                            k=1;   
-                        }
-                    }
-            }/*else if(hidari1!=old_hidari1){
+void change_servo()
+{
+    if(right2!=old_right2) {
+        old_right2=right2;
+        //ハンガー機構
+        if(right2==1) {
+            if(o==1) {
+                data_servo=0x01;
+                //send_data(0x18,data_servo);
+                o=2;
+            } else if(o==2) {
+                data_servo=0x02;
+                //send_data(0x18,data_servo);
+                o=3;
+            }
+            //data_servo=0x01;
+        }
+    } else if(circle!=old_circle) {
+        old_circle=circle;
+        //バスタオル挟む機構
+        if(circle==1) {
+            if(k==1) {
+                data_servo=0x03;
+                //send_data(0x18,data_servo);
+                k=0;
+            } else if(k==0) {
+                data_servo=0x04;
+                //send_data(0x18,data_servo);
+                k=1;
+            }
+        }
+    }/*else if(hidari1!=old_hidari1){
                     old_hidari1=hidari1;
-                
+
                     if(hidari1==1&&select==0){
                         if(n==1){
                             data_servo=0x05;
@@ -434,77 +441,73 @@
                         }
                     }
             }*/
-     }
-    
+}
+
 //ラック
-void change_rack(){
-        
-        
-        //右
-        if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
-            if(get_data_rs232!=0x20&&get_data_rs232!=0x22){
-                data5=0x00;
-            }else if(get_data_rs232==0x20||get_data_rs232==0x22){
-                data5=0x10;
-                }
-        }else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)){
-            if(get_data_rs232!=0x26&&get_data_rs232!=0x28){
-                data5=0xff;
-            }else if(get_data_rs232==0x26||get_data_rs232==0x28){
-                data5=0x10;
-                }
+void change_rack()
+{
+
+
+    //右
+    if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
+        if(!(get_data_rs232 & 0x01)) {
+            data5=0x00;
+        } else if(get_data_rs232 & 0x01) {
+            data5=0x10;
+        }
+    } else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)) {
+        if(!(get_data_rs232 & 0x04)) {
+            data5=0xff;
+        } else if(get_data_rs232 & 0x04) {
+            data5=0x10;
+        }
         //左
-        }else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)){
-            if(get_data_rs232!=0x20&&get_data_rs232!=0x24){
-                data6=0xff;
-            }else if(get_data_rs232==0x20||get_data_rs232==0x24){
-                data6=0x10;
-                }
-        }else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)){
-            if(get_data_rs232!=0x26&&get_data_rs232!=0x30){
-                data6=0x00;
-            }else if(get_data_rs232==0x26||get_data_rs232==0x30){
-                data6=0x10;
-                }
-        }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
-            data5=0x80;
-            data6=0x80;
-            
+    } else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)) {
+        if(!(get_data_rs232 & 0x02)) {
+            data6=0xff;
+        } else if(get_data_rs232 & 0x02) {
+            data6=0x10;
         }
+    } else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)) {
+        if(!(get_data_rs232 & 0x08)) {
+            data6=0x00;
+        } else if(get_data_rs232 & 0x08) {
+            data6=0x10;
+        }
+    } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
+        data5=0x80;
+        data6=0x80;
+
+    }
 
 }
 
 //ラック左右
-void change_rack_2(){
-    if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32){
-        if(get_data_rs232==0x20){
-            data5=0x10;
-            data6=0x10;
-        }else if(get_data_rs232==0x22){
+void change_rack_2()
+{
+    if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) {
+        if(get_data_rs232 & 0x01) {
             data5=0x10;
-            data6=0xff;
-        }else if(get_data_rs232==0x24){
+        } else {
             data5=0x00;
+        }
+        if(get_data_rs232 & 0x02) {
             data6=0x10;
-        }else{
-            data5=0x00;
+        } else {
             data6=0xff;
         }
-    }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32){
-        if(get_data_rs232==0x26){
-            data5=0x10;
-            data6=0x10;
-        }else if(get_data_rs232==0x28){
+    } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32) {
+        if(get_data_rs232 & 0x04) {
             data5=0x10;
-            data6=0x00;
-        }else if(get_data_rs232==0x30){
+        } else {
             data5=0xff;
+        }
+        if(get_data_rs232 & 0x08) {
             data6=0x10;
-        }else{
-            data5=0xff;
+        } else {
             data6=0x00;
         }
-    }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
+    } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
         data5=0x80;
         data6=0x80;
     }
@@ -512,36 +515,37 @@
 
 //回収機構
 
-void sequence_kaisyu(){
-     if(square!=old_square){
-         old_square=square;
-         if(square==1){
-             
-             data_servo=0x07;
-             send_data(0x18,data_servo);
-             wait(1.5);
-             data7=0x00;
-             while(get_data_rs232!=0x32){
-             send_data(0x24,data7);
-             get_data_rs232=slave.getc();
-             }
-             data7=0x10;
-             send_data(0x24,data7);
-             data_servo=0x08;
-             send_data(0x18,data_servo);
-             wait(2);
-             data7=0xff;
-             while(get_data_rs232!=0x34){
-             send_data(0x24,data7);
-             get_data_rs232=slave.getc();
-             }
-             data7=0x10;
-             send_data(0x24,data7);
-             data_servo=0x07;
-             send_data(0x18,data_servo);
+void sequence_kaisyu()
+{
+    if(square!=old_square) {
+        old_square=square;
+        if(square==1) {
 
-         }
-      }
+            data_servo=0x07;
+            send_data(0x18,data_servo);
+            wait(1.5);
+            data7=0x00;
+            while(!(get_data_rs232 & 0x10)) {
+                send_data(0x24,data7);
+                get_data_rs232=slave.getc();
+            }
+            data7=0x10;
+            send_data(0x24,data7);
+            data_servo=0x08;
+            send_data(0x18,data_servo);
+            wait(2);
+            data7=0xff;
+            while(!(get_data_rs232 & 0x20)) {
+                send_data(0x24,data7);
+                get_data_rs232=slave.getc();
+            }
+            data7=0x10;
+            send_data(0x24,data7);
+            data_servo=0x07;
+            send_data(0x18,data_servo);
+
+        }
+    }
 }
 
 
@@ -555,9 +559,9 @@
             wait(1.5);
             }
         }
-        
+
         //get_data_rs232=slave.getc();
-        
+
      if(p==1&&get_data_rs232_2!=0x32){
          data7=0x00;
          }else if(p==1&&get_data_rs232_2==0x32){
@@ -572,34 +576,35 @@
              data7=0x10;
              p=2;
          }
-        
+
     }
 */
 
 
-void sequence_hakidashi(){
-     if(migi1!=old_migi1){
-         old_migi1=migi1;
-         if(migi1==1){
-           /*  data_servo=0x07;
-             send_data(0x18,data_servo);
-             wait(1.5);
-             data11=0xff;
-             while(get_data_rs232!=0x36){
-                 send_data(0x30,data11);
-                 get_data_rs232=slave.getc();
-                 }
-              data11=0x10;
-              send_data(0x30,data11);*/
+void sequence_hakidashi()
+{
+    if(migi1!=old_migi1) {
+        old_migi1=migi1;
+        if(migi1==1) {
+            /*  data_servo=0x07;
+              send_data(0x18,data_servo);
+              wait(1.5);
               data11=0xff;
-              send_data(0x30,data11);
-             }
-         }else if(hidari1!=old_hidari1){
-             if(hidari1==1){
-                 data11=0x00;
-                 send_data(0x30,data11);
-                 }
-         }  /*   else if(hidari1==1){
+              while(get_data_rs232!=0x36){
+                  send_data(0x30,data11);
+                  get_data_rs232=slave.getc();
+                  }
+               data11=0x10;
+               send_data(0x30,data11);*/
+            data11=0xff;
+            send_data(0x30,data11);
+        }
+    } else if(hidari1!=old_hidari1) {
+        if(hidari1==1) {
+            data11=0x00;
+            send_data(0x30,data11);
+        }
+    }  /*   else if(hidari1==1){
                     if(get_data_rs232!=0x38){
                         data11=0x38;
                         send_data(0x30,data11);
@@ -611,17 +616,17 @@
                  data11=0x80;
                  send_data(0x30,data11);
                  }*/
-                 
-            if(get_data_rs232==0x38&&data11==0x00){
-                data11=0x10;
-                send_data(0x30,data11);
-            }  else if(get_data_rs232==0x36&&data11==0xff){
-                data11=0x10;
-                send_data(0x30,data11);
-                
-            }   
-     }
-  
+
+    if((get_data_rs232 & 0x80)&&(data11==0x00)) {
+        data11=0x10;
+        send_data(0x30,data11);
+    }  else if((get_data_rs232 & 0x40)&&(data11==0xff)) {
+        data11=0x10;
+        send_data(0x30,data11);
+
+    }
+}
+
 
 /*
 void hakidashi(){
@@ -641,29 +646,31 @@
         }
     }else{
         data11=0x80;
-    }   
+    }
 }
 
 */
 
 //ファン
-void change_fan(){
-    if(left2==1){
+void change_fan()
+{
+    if(left2==1) {
         data8=0xff;
         data9=0xff;
         data10=0xff;
-        }else if(left2==0){
-            data8=0x00;
-            data9=0x00;
-            data10=0x00;
-            }
+    } else if(left2==0) {
+        data8=0x00;
+        data9=0x00;
+        data10=0x00;
     }
-  
+}
 
 
-    
+
+
 //i2c
-void send_data(char address,char data){
+void send_data(char address,char data)
+{
     tsushin=1;
     tushin_check=0;
     i2c.frequency(100000);
@@ -672,5 +679,5 @@
     tushin_check=i2c.write(data);
     i2c.stop();
     wait(0.003);
-    }
-    
+}
+