Revision 1:99294241f2ba, committed 2019-09-05

Comitter:

shina

Date:

Thu Sep 05 09:40:50 2019 +0000

Parent:

0:c7e17c2fd542

Child:

2:830db42bae7e

Commit message:

NHK2019manualprogram

Changed in this revision

--- a/main.cpp	Tue Jun 25 08:53:27 2019 +0000
+++ b/main.cpp	Thu Sep 05 09:40:50 2019 +0000
@@ -1,19 +1,19 @@
 /////////////////////////////////////
 /*NHK2019*/
-/*Aチーム手動機プログラム*/
-/*担当者: 品川敦哉*/
+//Aチーム手動機プログラム
 /*
 アドレス
 0x10:右前
-0x12:左前
+0x12:左後ろ
 0x14:右後ろ
-0x16:左後ろ
+0x16:左前
 0x18:サーボ制御回路
 0x20:右ラック
 0x22:左ラック
 0x24:回収機構
 0x26:右ファン
 0x28:左ファン
+0x30:吐き出し機構
 */
 /////////////////////////////////////
 
@@ -31,6 +31,7 @@
 DigitalOut stop(D10);
 DigitalOut led1(LED1);
 
+
 //変数
 char data1;//右上
 char data2;//左上
@@ -42,16 +43,22 @@
 char data7;//回収機構
 char data8;//右ファン
 char data9;//左ファン
+char data10;//パトランプ
+char data11;//吐き出し機構
 char get_data_rs232=0x80;
 int Ry;
 int Rx;
+int Ly;
 int right1;
+int right2;
 int left1;
 int left2;
 int select;
 int start;
 int circle;
 int cross;
+int triangle;
+int square;
 int ue1;
 int shita;
 int migi1;
@@ -63,25 +70,42 @@
 int j=1;
 int old_circle=0;
 int k=1;
-int old_cross=0;
-int l=1;
+int old_square=0;
+int old_migi1=0;
+int old_hidari1=0;
+int n=1;
+int old_right2=0;
+int o=1;
+int old_right1=0;
+int old_left1=0;
+int p=2;
+int q=0;
+
+
 
 
 //関数プロトタイプ宣言
+void initialization();
 void get_data();
 void change_data();
 void change_pwm();
 void change_servo();
 void change_rack();
-void change_kaisyu();
 void change_fan();
 void send_data(char address,char data);
 void emergency();
-void get_limit_data();
+void change_rack_2();
+void sequence_kaisyu();
+//void kaisyu();
+void sequence_hakidashi();
+//void hakidashi();
+
+
 
 //メイン関数
 int main(){
     led=1;
+    initialization();
     while(true){
         emergency();
         get_data();
@@ -89,9 +113,12 @@
         change_servo();
         change_data();
         change_rack();
-        change_kaisyu();
+        change_rack_2();
+        sequence_kaisyu();
+       //kaisyu();
+       sequence_hakidashi();
+       //hakidashi();
         change_fan();
-        get_limit_data();
         send_data(0x10,data1);
         send_data(0x12,data2);
         send_data(0x14,data3);
@@ -99,29 +126,78 @@
         send_data(0x18,data_servo);
         send_data(0x20,data5);
         send_data(0x22,data6);
-        send_data(0x24,data7);
+        //send_data(0x24,data7);
         send_data(0x26,data8);
         send_data(0x28,data9);
+        send_data(0x40,data10);
+        //send_data(0x30,data11);
+        
+
+        
+        
         }
     }
     
+    
+//初期化
+void initialization(){
+    data1=0x80;
+    data2=0x80;
+    data3=0x80;
+    data4=0x80;
+    data5=0x80;
+    data6=0x80;
+    data7=0x10;
+    data8=0x80;
+    data9=0x80;
+    data10=0x80;
+    data11=0x80;
+    
+    send_data(0x10,data1);
+    send_data(0x12,data2);
+    send_data(0x14,data3);
+    send_data(0x16,data4);
+    send_data(0x20,data5);
+    send_data(0x22,data6);
+    send_data(0x24,data7);
+    send_data(0x26,data8);
+    send_data(0x28,data9);
+    send_data(0x40,data10);
+    send_data(0x30,data11);
+    
+    
+    }    
+
+    
 //データ読み込み
 void get_data(){
     Ry=ps3.getRightJoystickYaxis();
     Rx=ps3.getRightJoystickXaxis();
+    Ly=ps3.getLeftJoystickYaxis();
     circle=ps3.getButtonState(maru);
     cross=ps3.getButtonState(batu);
+    triangle=ps3.getButtonState(sankaku);
+    square=ps3.getButtonState(sikaku);
     left1=ps3.getButtonState(L1);
     left2=ps3.getButtonState(L2);
     right1=ps3.getButtonState(R1);
+    right2=ps3.getButtonState(R2);
     select=ps3.getSELECTState();
     start=ps3.getSTARTState();
     ue1=ps3.getButtonState(ue);
     shita=ps3.getButtonState(sita);
     hidari1=ps3.getButtonState(hidari);
     migi1=ps3.getButtonState(migi);
-    pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,left1,left2,right1,select,start,ue1,shita,migi1,hidari1,tushin_check,i);
-    if(Ry==0&&Rx==0&&left1==0&&right1==0&&left2==0&&select==0&&start==0&&shita==0&&ue1==0&&migi1==0&&hidari1==0){
+    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){
+        led1=1;
+        }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){
         data_check=0;
         }else{
             data_check=1;
@@ -136,8 +212,6 @@
     if(start==1){
         if(j==1){
     //緊急停止
-            data_servo=0x00;
-            send_data(0x18,data_servo);
             led=0;
             stop=1;
             j=0;
@@ -154,65 +228,142 @@
     
 //データ変化(メカナム)
 void change_data(){
+    
+        if(Ry!=0||Rx!=0){
+        square=0;
+        }
+    
     if(Ry>30&&i==0){
-        data1=0xff;
-        data2=0x00;
-        data3=0xff;
-        data4=0x00;
+            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){
-        data1=0x00;
-        data2=0xff;
-        data3=0x00;
-        data4=0xff;
-        }else if(Rx>30&&i==0){
-        data1=0xff;
-        data2=0xff;
-        data3=0x00;
-        data4=0x00;
-        }else if(Rx<-30&&i==0){
-        data1=0x00;
-        data2=0x00;
-        data3=0xff;
-        data4=0xff;
-        }else if(right1==1&&i==0){
-        data1=0x00;
-        data2=0x00;
-        data3=0x00;
-        data4=0x00;
-        }else if(left1==1&&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(right1==1&&i==0){
+        data1=0xff;
+        data2=0x00;
+        data3=0xff;
+        data4=0x00;
+        }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){
         data1=0xbf;
-        data2=0x3f;
+        data2=0xbf;
         data3=0xbf;
-        data4=0x3f;
-        }else if(Ry<-30&&i==1){
-        data1=0x3f;
-        data2=0xbf;
-        data3=0x3f;
         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=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){
-        data1=0x3f;
+        data1=0xbf;
         data2=0x3f;
-        data3=0x3f;
+        data3=0xbf;
         data4=0x3f;
         }else if(left1==1&&i==1){
-        data1=0xbf;
+        data1=0x3f;
         data2=0xbf;
-        data3=0xbf;
+        data3=0x3f;
         data4=0xbf;
         }else{
         data1=0x80;
@@ -222,6 +373,7 @@
         }
     }
 
+
 //pwm変化    
 void change_pwm(){
 if(select!=old_select){
@@ -240,106 +392,275 @@
 
 //サーボモーター
 void change_servo(){
-    if(j!=0){
-        if(circle!=old_circle){
+        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=0x01;
+                            data_servo=0x03;
+                            //send_data(0x18,data_servo);
                             k=0;
                         }else if(k==0){
-                            data_servo=0x02;
-                            k=1;
+                            data_servo=0x04;
+                            //send_data(0x18,data_servo);
+                            k=1;   
                         }
                     }
-            }else if(cross!=old_cross){
-                    if(cross==1){
-                        if(l==1){
-                            data_servo=0x03;
-                            l=0;
-                        }else if(l==0){
-                            data_servo=0x04;
-                            l=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;
                         }
                     }
-            }
-        }
+            }*/
      }
     
 //ラック
 void change_rack(){
-    if(ue1==1&&shita==0){
-        data5=0x00;
-        data6=0x00;
-        }else if(ue1==0&&shita==1){
-            data5=0xff;
+        
+        
+        //右
+        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;
+                }
+        //左
+        }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;
+            
+        }
+
+}
+
+//ラック左右
+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){
+            data5=0x10;
             data6=0xff;
-            }else{
-                data5=0x80;
-                data6=0x80;
-                }
+        }else if(get_data_rs232==0x24){
+            data5=0x00;
+            data6=0x10;
+        }else{
+            data5=0x00;
+            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){
+            data5=0x10;
+            data6=0x00;
+        }else if(get_data_rs232==0x30){
+            data5=0xff;
+            data6=0x10;
+        }else{
+            data5=0xff;
+            data6=0x00;
+        }
+    }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
+        data5=0x80;
+        data6=0x80;
     }
+}
 
 //回収機構
-void change_kaisyu(){
+
+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 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(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);*/
+              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);
+                        }else if(get_data_rs232==0x38){
+                            data11=0x10;
+                            send_data(0x30,data11;
+                        }
+        }*/ /*else{
+                 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);
+                
+            }   
+     }
+  
+
+/*
+void hakidashi(){
     if(migi1==1&&hidari1==0){
-        data7=0xff;
-        }else if(migi1==0&&hidari1==1){
-            data7=0x00;
-            }else{
-                data7=0x80;
-                }
-    }
-    
+        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(){
     if(left2==1){
         data8=0xff;
         data9=0xff;
+        data10=0xff;
         }else if(left2==0){
             data8=0x00;
             data9=0x00;
+            data10=0x00;
             }
     }
   
-//リミットスイッチデータ取得  
-void get_limit_data(){
-    get_data_rs232=slave.getc();
-        if(get_data_rs232==0x20){
-              if(ue1==1){
-                data5=0x80;
-                data6=0x80;
-                }
-        }else if(get_data_rs232==0x22){
-            if(ue1==1){
-                data5=0x80;
-                }
-        }else if(get_data_rs232==0x24){
-            if(ue1==1){
-                data6=0x80;
-                }
-        }else if(get_data_rs232==0x26){
-            if(shita==1){
-                data5=0x80;
-                data6=0x80;
-                }
-        }else if(get_data_rs232==0x28){
-            if(shita==1){
-                data5=0x80;
-                }
-        }else if(get_data_rs232==0x30){
-            if(shita==1){
-                data6=0x80;
-                }
-        }else if(get_data_rs232==0x32){
-            if(migi1==1){
-                data7=0x80;
-                }
-        }else if(get_data_rs232==0x34){
-            if(hidari1==1){
-                data7=0x80;
-                }
-        }
-    }
+
+
     
 //i2c
 void send_data(char address,char data){
@@ -351,4 +672,5 @@
     tushin_check=i2c.write(data);
     i2c.stop();
     wait(0.003);
-    }
\ No newline at end of file
+    }
+