Diff: main.cpp

Revision:

5:29971215ef50

Parent:

4:4d9c0999a90a

Child:

7:c423a6579c3c

diff -r 4d9c0999a90a -r 29971215ef50 main.cpp
--- a/main.cpp	Mon Oct 21 07:51:27 2019 +0000
+++ b/main.cpp	Sat Oct 26 04:30:54 2019 +0000
@@ -30,6 +30,7 @@
 DigitalOut pwm(D4);
 DigitalOut stop(D10);
 DigitalOut led1(LED1);
+//DigitalOut aaaaa(D9);
 Timer timer;
 Timer timer2;
 
@@ -50,20 +51,20 @@
 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;
-int hidari1;
+bool right1;
+bool right2;
+bool left1;
+bool left2;
+bool select;
+bool start;
+bool circle;
+bool cross;
+bool triangle;
+bool square;
+bool ue1;
+bool shita;
+bool migi1;
+bool hidari1;
 int tushin_check;
 int old_select=0;
 int i=1;
@@ -90,6 +91,9 @@
 int s=5;
 bool x=0;
 bool y=0;
+bool kaisyu_arm=0;
+char get_data_ps3[8];
+
 
 
 //関数プロトタイプ宣言
@@ -112,11 +116,18 @@
 
 //メイン関数
 int main()
-{
+{   //pc.printf("hi\n");
     led=1;
     pwm=1;
+    i2c.frequency(100000);
+    slave.baud(115200);
+    pc.baud(115200);
     initialization();
+
+    //pc.printf("ha\n");
     while(true) {
+        //pc.printf("hello\n");
+        //aaaaa=1;
         emergency();
         get_data();
         mode_change();
@@ -142,8 +153,8 @@
         send_data(0x40,data10);
         //send_data(0x30,data11);
 
-
-
+        //aaaaa=0;
+        //pc.printf("world\n");
 
     }
 }
@@ -194,7 +205,7 @@
     send_data(0x18,data_servo);
     data_servo=0x04;
     send_data(0x18,data_servo);*/
-    timer2.reset();
+    //timer2.reset();
     
 }
 
@@ -202,6 +213,9 @@
 //データ読み込み
 void get_data()
 {
+    int w;
+    //int switch_judge=0;
+    /*
     Ry=ps3.getRightJoystickYaxis();
     Rx=ps3.getRightJoystickXaxis();
     Ly=ps3.getLeftJoystickYaxis();
@@ -219,8 +233,75 @@
     shita=ps3.getButtonState(sita);
     hidari1=ps3.getButtonState(hidari);
     migi1=ps3.getButtonState(migi);
+    */
+    for(w=0;w<8;w++){
+    get_data_ps3[w]=ps3.PS3Data[w];
+    }
+   // get_data_ps3[0]=(*(+(button>>4)) >> (button & 0x0f)) & 1;
+   
+   switch(get_data_ps3[2] & 0x0c){
+        case 0x0c:
+        select=true;
+        migi1=false;
+        hidari1=false;
+        break;
+        case 0x04:
+        select=false;
+        migi1=true;
+        hidari1=false;
+        break;
+        case 0x08:
+        select=false;
+        migi1=false;
+        hidari1=true;
+        break;
+        default:
+        select=false;
+        migi1=false;
+        hidari1=false;
+        }
+        
+       switch(get_data_ps3[2] & 0x03){
+        case 0x03:
+        start=true;
+        ue1=false;
+        shita=false;
+        break;
+        case 0x01:
+        start=false;
+        ue1=true;
+        shita=false;
+        break;
+        case 0x02:
+        start=false;
+        ue1=false;
+        shita=true;
+        break;
+        default:
+        start=false;
+        ue1=false;
+        shita=false;
+        }
+
+   triangle=bool(get_data_ps3[2] & 0x10);
+   cross=bool(get_data_ps3[2] & 0x20);
+   circle=bool(get_data_ps3[2] & 0x40);
+   square=bool(get_data_ps3[1] & 0x01);
+   left1=bool(get_data_ps3[1] & 0x02);
+   left2=bool(get_data_ps3[1] & 0x04);
+   right1=bool(get_data_ps3[1] & 0x08);
+   right2=bool(get_data_ps3[1] & 0x10);
+    Ry=(int)get_data_ps3[6]*-1+64;
+    Rx=(int)get_data_ps3[5]-64;
+    
+
+    //pc.printf("a\n");
+    if(slave.readable()==1){
+        //pc.printf("c\n");
     get_data_rs232=slave.getc();
-    
+        //pc.printf("d\n");
+    }
+    //pc.printf("b\n");
 
 
     if(get_data_rs232) {
@@ -231,6 +312,7 @@
     //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);
+    //pc.printf("%d,%d,%d\n",x,y,p);
     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 {
@@ -491,6 +573,7 @@
                 data_servo=0x01;
                 send_data(0x18,data_servo);
                 o=2;
+                kaisyu_arm=1;
             } else if(o==2) {
                 data_servo=0x02;
                 send_data(0x18,data_servo);
@@ -514,8 +597,9 @@
             }
         }
     }
-    /*
-     if(triangle!=triangle){
+    
+    if(q==0){
+     if(triangle!=old_triangle){
             old_triangle=triangle;
             if(triangle==1){
                 if(y==0){
@@ -528,7 +612,8 @@
                     y=0;
                 }
             }
-        }    */
+        }   
+    } 
 }
 
 
@@ -645,12 +730,14 @@
 //回収機構switch文version
 void kaisyu(){
     if(q==0){
+        if(kaisyu_arm!=0){
         if(old_square!=square){
             old_square=square;
             if(square==1){
                 if(x==0){
                     p=1;
                     x=1;
+                    timer2.start();
                 }else if(x==1){
                     p=4;
                     x=0;
@@ -661,10 +748,11 @@
          switch(p){
             
             case 1:
+            y=0;
             data_servo=0x07;
             send_data(0x18,data_servo);
-            timer2.start();
-            if(timer2.read()>1.2f){
+            //timer2.start();
+            if(timer2.read()>1.5f){
                 timer2.stop();
                 timer2.reset();
                 p=2;
@@ -682,6 +770,7 @@
             break;
             
             case 3:
+            /*
             if(triangle!=old_triangle){
                 old_triangle=triangle;
                 if(triangle==1){
@@ -695,7 +784,7 @@
                         y=0;
                     }
                 }
-            }
+            }*/
 
             break;
             
@@ -714,6 +803,7 @@
             break;
             
           }    
+        }
     }
 }
 
@@ -789,6 +879,7 @@
             old_migi1=migi1;
             if(migi1==1&&select==0){
                 r=1;
+                timer2.start();
             }
         }
         
@@ -796,10 +887,11 @@
             case 1:
             data_servo=0x07;
             send_data(0x18,data_servo);
-            timer2.start();
+            //timer2.start();
             if(timer2.read()>1.5f){
                 timer2.stop();
                 timer2.reset();
+                timer2.start();
                 r=2;
             }
             break;
@@ -807,7 +899,7 @@
             case 2:
             data7=0x00;
             send_data(0x24,data7);
-            timer2.start();
+            //timer2.start();
             if(timer2.read()>0.3f){
                 timer2.stop();
                 timer2.reset();
@@ -820,7 +912,6 @@
             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);
@@ -831,7 +922,6 @@
             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);
@@ -842,7 +932,6 @@
             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);