Revision 4:4d9c0999a90a, committed 2019-10-21

Comitter:

shina

Date:

Mon Oct 21 07:51:27 2019 +0000

Parent:

3:8c4e42cba9cb

Child:

5:29971215ef50

Commit message:

NHK 2019 manual program 10/21

Changed in this revision

--- a/main.cpp	Thu Sep 12 09:03:59 2019 +0000
+++ b/main.cpp	Mon Oct 21 07:51:27 2019 +0000
@@ -9,7 +9,7 @@
 0x16:左前
 0x18:サーボ制御回路
 0x20:右ラック
-0x22:左ラック
+0x22:左ラック.
 0x24:回収機構
 0x26:右ファン
 0x28:左ファン
@@ -30,7 +30,8 @@
 DigitalOut pwm(D4);
 DigitalOut stop(D10);
 DigitalOut led1(LED1);
-
+Timer timer;
+Timer timer2;
 
 //変数
 char data1;//右上
@@ -78,13 +79,17 @@
 int o=1;
 int old_right1=0;
 int old_left1=0;
-int p=2;
+int p=5;
 int q=0;
 int old_ue1=0;
 int old_shita=0;
 int old_left2=0;
 int old_triangle=0;
 int old_cross=0;
+int r=6;
+int s=5;
+bool x=0;
+bool y=0;
 
 
 //関数プロトタイプ宣言
@@ -93,15 +98,14 @@
 void change_data();
 void change_pwm();
 void change_servo();
-//void change_rack();
 void change_fan();
 void send_data(char address,char data);
 void emergency();
 void change_rack_2();
-void sequence_kaisyu();
-//void kaisyu();
-void sequence_hakidashi();
-//void hakidashi();
+//void sequence_kaisyu();
+void kaisyu();
+//void sequence_kaisyu_2();
+void hakidashi();
 void mode_change();
 
 
@@ -117,20 +121,19 @@
         get_data();
         mode_change();
         change_pwm();
+        change_data();
         change_servo();
-        change_data();
-        //change_rack();
         change_rack_2();
-        sequence_kaisyu();
-        //kaisyu();
-        sequence_hakidashi();
-        //hakidashi();
+        //sequence_kaisyu();
+        kaisyu();
+        //sequence_kaisyu_2();
+        hakidashi();
         change_fan();
         send_data(0x10,data1);
         send_data(0x12,data2);
         send_data(0x14,data3);
         send_data(0x16,data4);
-        send_data(0x18,data_servo);
+        //send_data(0x18,data_servo);
         send_data(0x20,data5);
         send_data(0x22,data6);
         //send_data(0x24,data7);
@@ -160,6 +163,7 @@
     data9=0x80;
     data10=0x80;
     data11=0x80;
+    data_servo=0x00;
 
     send_data(0x10,data1);
     send_data(0x12,data2);
@@ -172,6 +176,8 @@
     send_data(0x28,data9);
     send_data(0x40,data10);
     send_data(0x30,data11);
+    send_data(0x18,data_servo);
+    
 
     data10=0x3f;
     send_data(0x40,data10);
@@ -184,7 +190,11 @@
         send_data(0x40,data10);
     }
     
-    
+    /*data_servo=0x03;
+    send_data(0x18,data_servo);
+    data_servo=0x04;
+    send_data(0x18,data_servo);*/
+    timer2.reset();
     
 }
 
@@ -210,6 +220,7 @@
     hidari1=ps3.getButtonState(hidari);
     migi1=ps3.getButtonState(migi);
     get_data_rs232=slave.getc();
+    
 
 
     if(get_data_rs232) {
@@ -217,7 +228,9 @@
     } 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);
+    //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);
+    //pc.printf("0x%x\n",get_data_rs232);
+    //pc.printf("%d\n",x);
     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 {
@@ -256,6 +269,10 @@
             if(q==0){
                 //送風モード
                 q=1;
+        /*        data_servo=0x03;
+                send_data(0x18,data_servo);
+                data_servo=0x04;
+                send_data(0x18,data_servo);*/
                 data10=0xcf;
                 send_data(0x40,data10);
             }else if(q==1){
@@ -468,88 +485,53 @@
 {
     if(right2!=old_right2) {
         old_right2=right2;
-        //ハンガー機構
+        //ハンガー機構&バスタオル機構initialize処理
         if(right2==1) {
             if(o==1) {
                 data_servo=0x01;
-                //send_data(0x18,data_servo);
+                send_data(0x18,data_servo);
                 o=2;
             } else if(o==2) {
                 data_servo=0x02;
-                //send_data(0x18,data_servo);
+                send_data(0x18,data_servo);
                 o=3;
             }
-            //data_servo=0x01;
         }
-    } else if(circle!=old_circle) {
+    } 
+    
+     if(circle!=old_circle) {
         old_circle=circle;
         //バスタオル挟む機構
         if(circle==1) {
             if(k==1) {
-                data_servo=0x03;
-                //send_data(0x18,data_servo);
+                data_servo=0xf3;
+                send_data(0x18,data_servo);
                 k=0;
             } else if(k==0) {
-                data_servo=0x04;
-                //send_data(0x18,data_servo);
+                data_servo=0xf4;
+                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;
-                            n=0;
-                        }else if(n==0){
-                            data_servo=0x06;
-                            n=1;
-                        }
-                    }
-            }*/
+    }
+    /*
+     if(triangle!=triangle){
+            old_triangle=triangle;
+            if(triangle==1){
+                if(y==0){
+                    data_servo=0x08;
+                    send_data(0x18,data_servo);
+                    y=1;
+                }else if(y==1){
+                    data_servo=0x07;
+                    send_data(0x18,data_servo);
+                    y=0;
+                }
+            }
+        }    */
 }
 
-//ラック
-/*
-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 & 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()
@@ -585,13 +567,18 @@
     
     if(ue1!=old_ue1){
         old_ue1=ue1;
-        if(ue1==1){
+        if(ue1==1&&start==0){
             data5=0x00;
             data6=0xff;
+            
+            /*data_servo=0x03;
+            send_data(0x18,data_servo);
+            data_servo=0x04;
+            send_data(0x18,data_servo);*/
         }
     }else if(shita!=old_shita){
         old_shita=shita;
-        if(shita==1){
+        if(shita==1&&start==0){
             data5=0xff;
             data6=0x00;
         }
@@ -617,163 +604,261 @@
 }
 
 //回収機構
-
+/*
 void sequence_kaisyu()
 {
     if(q==0){
     if(square!=old_square) {
         old_square=square;
-        if(square==1) {
+        if(square==1&&select==0) {
+            if(x==0){
+                data_servo=0x07;
+                send_data(0x18,data_servo);
+                wait(1.2);
+                data7=0x00;
+                get_data_rs232=get_data_rs232 & 0b11101111;
+                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(1.2);
+                data7=0xff;
+                get_data_rs232=get_data_rs232 & 0b11011111;
+                while(!(get_data_rs232 & 0x20)){
+                    send_data(0x24,data7);
+                    get_data_rs232=slave.getc();
+                }
+                data7=0x10;
+                send_data(0x24,data7);
+                x=0;
+              }
+            }  
+        }
+    }
+}
+*/
 
+//回収機構switch文version
+void kaisyu(){
+    if(q==0){
+        if(old_square!=square){
+            old_square=square;
+            if(square==1){
+                if(x==0){
+                    p=1;
+                    x=1;
+                }else if(x==1){
+                    p=4;
+                    x=0;
+                }
+            }
+        }
+        
+         switch(p){
+            
+            case 1:
+            data_servo=0x07;
+            send_data(0x18,data_servo);
+            timer2.start();
+            if(timer2.read()>1.2f){
+                timer2.stop();
+                timer2.reset();
+                p=2;
+            }
+            break;
+            
+            case 2:
+            data7=0x00;
+            send_data(0x24,data7);
+            if(get_data_rs232 & 0x10){
+                data7=0x10;
+                send_data(0x24,data7);
+                p=3;
+            }
+            break;
+            
+            case 3:
+            if(triangle!=old_triangle){
+                old_triangle=triangle;
+                if(triangle==1){
+                    if(y==0){
+                        data_servo=0x08;
+                        send_data(0x18,data_servo);
+                        y=1;
+                    }else if(y==1){
+                        data_servo=0x07;
+                        send_data(0x18,data_servo);
+                        y=0;
+                    }
+                }
+            }
+
+            break;
+            
+            case 4:
+            data7=0xff;
+            send_data(0x24,data7);
+            if(get_data_rs232 & 0x20){
+                data7=0x10;
+                send_data(0x24,data7);
+                p=5;
+            }
+            break;
+            
+            default:
+                p=5;
+            break;
+            
+          }    
+    }
+}
+
+/*
+void sequence_kaisyu_2(){
+    if(q==0){
+        if(old_migi1!=migi1){
+            if(migi1==1){
+                p=1;
+            }
+        }
+       
+       switch(p){
+            case 1:
             data_servo=0x07;
             send_data(0x18,data_servo);
-            wait(1.5);
+            timer.start();
+            if(timer.read>1.2f){
+                p=2;
+                timer.stop();
+                timer.reset();
+            }
+            break;
+            
+            case 2:
             data7=0x00;
-            while(!(get_data_rs232 & 0x10)) {
+            send_data(0x24,data7);
+            if(get_data_rs232 & 0x10){
+                timer.stop();
+                timer.reset();
+                data7=0x10;
                 send_data(0x24,data7);
-                get_data_rs232=slave.getc();
+                p=3;
             }
-            data7=0x10;
-            send_data(0x24,data7);
+            break();
+            
+            case 3:
             data_servo=0x08;
             send_data(0x18,data_servo);
-            wait(2);
+            timer.start();
+            if(timer.read()>1.5f()){
+                timer.stop();
+                timer.reset();
+                p=4;
+            }
+            break();
+            
+            case 4:
             data7=0xff;
-            while(!(get_data_rs232 & 0x20)) {
+            send_data(0x24,data7);
+            if(get_data_rs232 & 0x20){
+                data7=0x10;
                 send_data(0x24,data7);
-                get_data_rs232=slave.getc();
+                p=5;
+            }
+            break;
+            
+            default:
+            break();
+            
+            
+        } 
+        
+    }
+}
+*/
+
+
+
+void hakidashi(){
+    if(q==0){
+        if(old_migi1!=migi1){
+            old_migi1=migi1;
+            if(migi1==1&&select==0){
+                r=1;
+            }
+        }
+        
+        switch(r){
+            case 1:
+            data_servo=0x07;
+            send_data(0x18,data_servo);
+            timer2.start();
+            if(timer2.read()>1.5f){
+                timer2.stop();
+                timer2.reset();
+                r=2;
             }
-            data7=0x10;
+            break;
+            
+            case 2:
+            data7=0x00;
             send_data(0x24,data7);
-
+            timer2.start();
+            if(timer2.read()>0.3f){
+                timer2.stop();
+                timer2.reset();
+                data7=0x10;
+                send_data(0x24,data7);
+                r=3;
+            }
+            break;
+            
+            case 3:
+            data11=0xff;
+            send_data(0x30,data11);
+            //get_data_rs232=get_data_rs232 & 0b10111111;
+            if(get_data_rs232 & 0x40){
+                data11=0x10;
+                send_data(0x30,data11);
+                r=4;
             }
+            break;
+            
+            case 4:
+            data11=0x00;
+            send_data(0x30,data11);
+            //get_data_rs232=get_data_rs232 & 0b01111111;
+            if(get_data_rs232 & 0x80){
+                data11=0x10;
+                send_data(0x30,data11);
+                r=5;
+            }
+            break;
+            
+            case 5:
+            data7=0xff;
+            send_data(0x24,data7);
+            //get_data_rs232=get_data_rs232 & 0b11011111;
+            if(get_data_rs232 & 0x20){
+                data7=0x10;
+                send_data(0x24,data7);
+                r=6;
+            }
+            break;
+            
+            default:
+            r=6;
+            break;
         }
     }
 }
 
 
 
-/*void kaisyu(){
-    if(square!=old_square){
-        if(square==1){
-            p=1;
-            data_servo=0x07;
-            send_data(0x18,data_servo);
-            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){
-             data7=0x10;
-             p=0;
-             data_servo=0x08;
-             send_data(0x18,data_servo);
-             wait(1.5);
-         }else if(p==0&&get_data_rs232!=0x34){
-             data7=0xff;
-         }else if(p==0&&get_data_rs232==0x34){
-             data7=0x10;
-             p=2;
-         }
-
-    }
-*/
-
-
-void sequence_hakidashi()
-{
-    if(q==0){
-    if(migi1!=old_migi1) {
-        old_migi1=migi1;
-        if(migi1==1&&select==0) {
-              data_servo=0x07;
-              send_data(0x18,data_servo);
-              wait(1);
-              data7=0x00;
-              send_data(0x24,data7);
-              wait(0.5);
-              data7=0x10;
-              send_data(0x24,data7);
-              data11=0xff;
-              while(!(get_data_rs232 & 0x40)){
-                  send_data(0x30,data11);
-                  get_data_rs232=slave.getc();
-              }
-              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) {
-        if(hidari1==1) {
-            data11=0x00;
-            send_data(0x30,data11);
-        }
-    }     else if(hidari1==1){
-                    if(get_data_rs232!=0x38){
-                        data11=0x38;
-                        send_data(0x30,data11);
-                        }else if(get_data_rs232==0x38){
-                            data11=0x10;
-                            send_data(0x30,data11;
-                        }
-        } */ /*else{
-                 data11=0x80;
-                 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(){
-    if(migi1==1&&hidari1==0){
-        data_servo=0x07;
-        if(get_data_rs232==0x36){
-            data11=0x10;
-        }else if(get_data_rs232!=0x38){
-            data11=0xff;
-            }
-    }else if(migi1==0&&hidari1==1){
-        data_servo=0x08;
-        if(get_data_rs232==0x38){
-            data11=0x10;
-        }else if(get_data_rs232!=0x38){
-            data11=0x00;
-        }
-    }else{
-        data11=0x80;
-    }
-}
-
-*/
-
 //ファン
 void change_fan()
 {
@@ -786,17 +871,17 @@
         data8=0x00;
         data9=0x00;
         data10=0x00;
-    }else if(left2==0&&(old_triangle!=triangle)&&select==0){
+    }else if(left2==0&&(old_triangle!=triangle)&&select==0&&(get_data_rs232 & 0b00000011)){
         old_triangle=triangle;
         if(triangle==1){
             data8=0xff;
             data9=0xff;
             data10=0xff;
-            data_servo=0x03;
+            data_servo=0xf3;
             send_data(0x26,data8);
             send_data(0x28,data9);
             send_data(0x40,data10);
-            wait(1);
+            wait(0.5);
             send_data(0x18,data_servo);
             wait(1);
             data8=0x00;
@@ -806,7 +891,7 @@
             send_data(0x28,data9);
             send_data(0x40,data10);
             wait(0.5);
-            data_servo=0x04;
+            data_servo=0xf4;
             send_data(0x18,data_servo);
         }
     }
@@ -814,9 +899,6 @@
     
 }
 
-
-
-
 //i2c
 void send_data(char address,char data)
 {
@@ -828,3 +910,4 @@
     i2c.stop();
 }
 
+