Diff: main.cpp

Revision:

2:9dae549ae1b4

Parent:

1:8a67adccebd9

diff -r 8a67adccebd9 -r 9dae549ae1b4 main.cpp
--- a/main.cpp	Wed Sep 18 14:14:01 2019 +0000
+++ b/main.cpp	Thu Sep 19 14:31:17 2019 +0000
@@ -3,12 +3,14 @@
 #include "moterdrive.h"
 #include "DualShockMod.h"
 
-#define PERIOD 50
+#define PERIOD 40
 
 #define hosei1 1.070    //left front
 #define hosei2 1.000    //right front
-#define hosei3 1.000    //back
+#define hosei3 0.960    //back
 
+#define LIFTPOWER_UP 0.2
+#define LIFTPOWER_DOWN 0.2
 
 DigitalOut  my_led(LED1);
 PwmOut      md1_pwm1(PB_2);
@@ -31,14 +33,18 @@
 DigitalOut  md2_ccw2(PA_10);
 DigitalOut  md2_dis2(PB_5);
 
-DigitalIn   Limit1(PC_9);
-DigitalIn   Limit2(PC_8);
+DigitalIn   Limit1(PC_9);  //back
+DigitalIn   Limit2(PC_8);  //right
 DigitalIn   Limit3(PC_6);
 DigitalIn   Limit4(PC_5);
 
-DigitalIn   photo1(PA_6);
-DigitalIn   photo2(PA_7);
-DigitalIn   photo3(PB_6); 
+DigitalIn   photo1(PA_13);
+DigitalIn   photo2(PA_14);
+DigitalIn   photo3(PA_15); 
+
+DigitalIn button1(USER_BUTTON);
+DigitalOut led1(D14);
+DigitalOut led2(D15);
 
 //serial通信
 Serial pc(SERIAL_TX, SERIAL_RX);
@@ -55,11 +61,14 @@
     
     typedef enum{
         WAIT,
+        INIT1,
+        INIT2,
+        WAIT2,
         HARI1,
         HARI2,
         HARI3,
         HARI4,
-        WAIT2,
+        HARI_END,
         SYM1,
         SYM2,
         SYM3,
@@ -67,7 +76,9 @@
         SYM5,
         SYM6,
         SYM7,
-        END   
+        SYM8,
+        END,
+        DEBUG   
     }SEQENCE;
     SEQENCE seq = WAIT;
 
@@ -75,11 +86,12 @@
         STOP,
         FFAST,
         FSLOW,
+        BFAST,
+        BSLOW,
         RIGHT,
         LEFT,
         RROLL,
         LROLL,
-        BACK,
         UP,
         DOWN 
     }MOVEDIR;
@@ -98,7 +110,7 @@
     md2_pwm1.period_us(100);
     md2_pwm2.period_us(100);
     
-    int pho1, pho2, pho3, lim1, lim2, lim3, lim4 = {0};
+    int pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1,k = {0};
     
     while(1){
         
@@ -109,7 +121,7 @@
         lim2 = Limit2;
         lim3 = Limit3;
         lim4 = !Limit4;                        
-        
+        btn1 = !button1;
 
         
 //        pc.printf("ENC = %d \n\r", qei.getPulses());
@@ -118,44 +130,90 @@
 //        posX = ;
 //        posY = ;     
 
+        if(move == STOP) led1 = 1;
+            else led1 = 0;          
             
+        if(lim4 == 1) seq = DEBUG;
 
         switch(seq){
             case WAIT:
-                move = FFAST;
+                seq = INIT1;
+             
+             break;
             
-                if(posY <= -1000 ){
-                    move = FSLOW;
-                    if(posY <= -1080)
+            case INIT1:
+              move = DOWN;
+               
+               if(pho1 == 1){
+                    move = STOP;
+                    seq = INIT2;
+                    }
+
+             break;
+             
+             case INIT2:
+              timer.start();
+              move = UP;
+              if(timer.read_ms() > 2500){
+                    move = STOP;
+                    timer.reset();
+                    timer.stop();                    
+                seq = WAIT2;
+             }
+              break;
+             
+            case WAIT2:
+                move = STOP;
+                
+                 if(btn1 == 1){
                     seq = HARI1;
-                    }   
-                     
+                    }
+                 if(lim2 == 1){
+                    seq = SYM1;
+                     }   
                 break;
             
             case HARI1:
-                move = BACK;
-                if(posY <= -800)
-                
+                move = FFAST;
+            
+                if(posY > 1000 ){
+                    move = FSLOW;
+                    if(posY > 1080)
+                    seq = HARI2;
+                    }
               break;
               
-              
             case HARI2:
                 move = DOWN;
                 
+                if(pho1 == 1){
+                    move = STOP;
+                    seq = HARI3;
+                    }    
               break;
               
             case HARI3:
-               move = BACK;                    
+               move = BFAST; 
+               
+               if(posY < 800){ 
+                    seq = HARI4;
+                    timer.reset();
+                }  
              break;
              
             case HARI4:
+               timer.start();
                move = UP;
-               if(0){
-                    seq = WAIT2;}
+               
+               if(timer.read_ms() > 2000){
+                    move = STOP;
+                    timer.reset();
+                    seq = HARI_END;
+               }
                     
              break;
              
-            case WAIT2:
+            case HARI_END:
                move = STOP;
 
              break;
@@ -163,63 +221,86 @@
             case SYM1:
                 move = FFAST;
                 
-
+                if(posY > 750 ){
+                    move = FSLOW;
+                    if(posY > 800)
+                    seq = SYM2;
+                    }
               break;
               
             case SYM2:
                 move = RIGHT;
                 
-                if(0){
+                if(posX > 850){
                     seq = SYM3;
-                  
                     }   
               break;
               
             case SYM3:
-                move = FFAST;
+                move = BSLOW;
                 
-                if(0){
+                if(lim1 == 1){
+                    move = FFAST;
                     seq = SYM4;
-                  
+                    k = posY;
                     }   
               break; 
             
             case SYM4:
-                move = LEFT;
+                move = FFAST;
                 
-                if(0){
+                if( posY - k >360){
                     seq = SYM5;
-                  
-                    }   
+                    }
               break; 
               
             case SYM5:
-                move = LROLL;
+                move = LEFT;
                 
-                if(0){
+                if(posX < 300){
                     seq = SYM6;
-                  
-                    }   
+                    } 
               break;   
               
             case SYM6:
-                move = DOWN;
+                move = LROLL;
                 
-                if(0){
+                timer.start();
+                if(timer.read_ms() > 5000){
+                    move = STOP;
+                    timer.reset();
+                    timer.stop(); 
                     seq = SYM7;
-                  
                     }   
               break; 
               
             case SYM7:
-                move = BACK;
+                 move = DOWN;
                 
-                if(0){
-                    seq = END;
-                  
+                if(pho1 == 1){
+                    move = STOP;
+                    seq = SYM8;
                     }   
               break; 
               
+            case SYM8:
+                move = BFAST;
+                timer.start();
+                if(timer.read_ms() > 3000){
+                    move = STOP;
+                    timer.reset();
+                    timer.stop(); 
+                
+                    seq = END;  
+                    }   
+              break; 
+              
+            case DEBUG:
+                if(lim1 == 1) move = UP;
+                else if (lim2 == 1) move = DOWN;
+                else move = STOP;
+            break;
+              
             }
             
             
@@ -247,37 +328,44 @@
                 moter(4,STOP,0);
              break;
             
-            case BACK:
+            case BFAST:
                 moter(1,ccw,1);
                 moter(2,cw,1);
                 moter(3,STOP,0);
                 moter(4,STOP,0);
              break;
+             
+            case BSLOW:
+                moter(1,ccw,0.5);
+                moter(2,cw,0.5);
+                moter(3,STOP,0);
+                moter(4,STOP,0);
+             break;
             
             case RROLL:
-                moter(1,ccw,0.3);
-                moter(2,ccw,0.3);
+                moter(1,cw,0.3);
+                moter(2,cw,0.3);
                 moter(3,ccw,0.3);
                 moter(4,STOP,0);
              break;
             
             case LROLL:
-                moter(1,cw,0.3);
-                moter(2,cw,0.3);
+                moter(1,ccw,0.3);
+                moter(2,ccw,0.3);
                 moter(3,cw,0.3);
                 moter(4,STOP,0);
              break;
             
             case RIGHT:
-                moter(1,ccw,0.5);
-                moter(2,ccw,0.5);
+                moter(1,cw,0.5);
+                moter(2,cw,0.5);
                 moter(3,cw,1);
                 moter(4,STOP,0);
              break;
             
             case LEFT:
-                moter(1,cw,0.5);
-                moter(2,cw,0.5);
+                moter(1,ccw,0.5);
+                moter(2,ccw,0.5);
                 moter(3,ccw,1);
                 moter(4,STOP,0);
              break;
@@ -286,28 +374,28 @@
                 moter(1,STOP,0);
                 moter(2,STOP,0);
                 moter(3,STOP,0);
-                moter(4,cw,0.1);
+                moter(4,cw,LIFTPOWER_UP);
              break;
             
             case DOWN:
                 moter(1,STOP,0);
                 moter(2,STOP,0);
                 moter(3,STOP,0);
-                moter(4,ccw,0.1);
+                moter(4,ccw,LIFTPOWER_DOWN);
              break;
             
         }
 
-        pc.printf("posX=%d,posY=%d\n\r", posX, posY);
-        pc.printf("PHO = %d%d%d\tLIM = %d%d%d%d\n\r", pho1, pho2, pho3, lim1, lim2, lim3, lim4);
-        pc.printf("seq = %d", seq);
+        pc.printf("posX=%d,posY=%d  ", posX, posY);
+        pc.printf("PHO = %d%d%d\tLIM = %d%d%d%d\t BTN=%d  ", pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1);
+        pc.printf("seq = %d\n\r", seq);
          }    
 }
 
 
 
 void moter(int num, char dir, float power){
-    
+    int pho1 = !photo1;
     int output;
     if(num == 1){
         if(dir == cw){
@@ -322,7 +410,7 @@
             md1_cw1 = 0;
             md1_ccw1 = 0;
             }
-        output = PERIOD * power * hosei1;
+        output = PERIOD * power * (float)hosei1;
         md1_pwm1.pulsewidth_us(output);
         } 
         if(num == 2){
@@ -338,7 +426,7 @@
             md1_cw2 = 0;
             md1_ccw2 = 0;
             }
-        output = PERIOD * power * hosei2;
+        output = PERIOD * power * (float)hosei2;
         md1_pwm2.pulsewidth_us(output);
         } 
         if(num == 3){
@@ -354,7 +442,7 @@
             md2_cw1 = 0;
             md2_ccw1 = 0;
             }
-        output = PERIOD * power * hosei3;
+        output = PERIOD * power * (float)hosei3;
         md2_pwm1.pulsewidth_us(output);
         }
         if(num == 4){
@@ -364,18 +452,20 @@
             }
             else if(dir == ccw){
             md2_cw2 = 0;
-            md2_ccw2 = 1;
+            
+                if(pho1 == 1){
+                md2_ccw2 = 0;
+                }
+                else{
+                md2_ccw2 = 1;
+                }
             }
             else if(dir == STOP){
             md2_cw2 = 0;
             md2_ccw2 = 0;
             }
-        output = PERIOD * power ;
+        output = 100 * power ;
         md2_pwm2.pulsewidth_us(output);
-        }
-     
-    
-    
-    
+        }    
     }
  
\ No newline at end of file