Diff: main.cpp

Revision:

4:0e9f65e3e2a0

Parent:

3:387812a9386a

--- a/main.cpp	Fri May 06 07:42:51 2016 +0000
+++ b/main.cpp	Fri May 06 18:04:47 2016 +0000
@@ -78,13 +78,19 @@
         motor1->Move(StepperMotor::FWD,nbPas);
         motor2->Move(StepperMotor::BWD,nbPas);
     }
+    /*while((sensor.getDistance()>50) && end) {
+        //Tant que la distance est superieure on continue d'avancer
+        wait_ms(20);
+        printf("1");
+    }*/
     motor1->WaitWhileActive();
     motor2->WaitWhileActive();
     motor1->HardStop();
     motor2->HardStop();
 }
 
-void rotation(int angleEnDegre, int diametre)
+
+void rotation(int angleEnDegre, int diametre, int sens)
 {
     int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
     int nbPas = 160*deplacementEnCm;
@@ -96,8 +102,14 @@
     nbPas *= 2;*/
 
     printf("%d \n\r", nbPas);
-    motor2->Move(StepperMotor::FWD,nbPas);
-    motor1->Move(StepperMotor::FWD,nbPas);
+    if(sens) {
+        motor2->Move(StepperMotor::FWD,nbPas);
+        motor1->Move(StepperMotor::FWD,nbPas);
+    } else {
+        motor2->Move(StepperMotor::BWD,nbPas);
+        motor1->Move(StepperMotor::BWD,nbPas);
+
+    }
     motor1->WaitWhileActive();
     motor2->WaitWhileActive();
     motor1->HardStop();
@@ -179,114 +191,129 @@
 
             case 1:
                 //Déplacement 1ers cubes au milieu
+                printf("deplacement init \n");
                 wait_ms(100);
-                motor1->Move(StepperMotor::BWD,12000);
-                motor2->Move(StepperMotor::FWD,12000);
-                while((sensor.getDistance()>50) && end) {
-                    //Tant que la distance est superieure on continue d'avancer
-                    wait_ms(20);
-                    printf("1");
-                }
+                deplacement(90,1);
+
                 step = 2;
                 break;
 
             case 2:
-                motor2->Move(StepperMotor::FWD,2100); //première rotation 90°
-                motor1->Move(StepperMotor::FWD,2100);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                
-                motor2->Move(StepperMotor::BWD,6000); //Avance jusqu'au mur
-                motor1->Move(StepperMotor::FWD,6000);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                 motor2->Move(StepperMotor::FWD,10); //recul pour rotation
-                motor1->Move(StepperMotor::BWD,10);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                
-                motor2->Move(StepperMotor::FWD,2100); //deuxième rotation 90° (même sens que première)
-                motor1->Move(StepperMotor::FWD,2100);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                
-                motor2->Move(StepperMotor::BWD,6000); //mise en position
-                motor1->Move(StepperMotor::FWD,6000);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                motor2->Move(StepperMotor::BWD,2100); //troisième rotation 90° 
-                motor1->Move(StepperMotor::BWD,2100);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                
-                motor2->Move(StepperMotor::BWD,20); //Collage mur 
-                motor1->Move(StepperMotor::FWD,20);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                
-                motor2->Move(StepperMotor::FWD,10); //recul pour rotation
-                motor1->Move(StepperMotor::BWD,10);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                motor2->Move(StepperMotor::BWD,2100); //quatrième rotation 90° 
-                motor1->Move(StepperMotor::BWD,2100);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
-                motor1->HardStop();
-                motor2->HardStop();
-                
-                
-        
-        //Mise en position du robot pour pêche
+                //Mise en place pour pêche
+                //motor1->Move(StepperMotor::FWD,4000); //première rotation 90°
+//                motor2->Move(StepperMotor::FWD,4000);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//                printf("Rotation 1 \n");
+//
+//
+//                motor1->Move(StepperMotor::BWD,6000); //Avance jusqu'au mur
+//                motor2->Move(StepperMotor::FWD,6000);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//
+//                motor1->Move(StepperMotor::FWD,600); //recul pour rotation
+//                motor2->Move(StepperMotor::BWD,600);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//                printf("Recul Rotation 2\n");
+//
+//
+//
+//                motor1->Move(StepperMotor::FWD,4000); //deuxième rotation 90° (même sens que première)
+//                motor2->Move(StepperMotor::FWD,4000);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//                printf("Rotation 2 \n");
+//
+//
+//                motor1->Move(StepperMotor::BWD,6000); //mise en position
+//                motor2->Move(StepperMotor::FWD,6000);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//                printf("Mise en position\n");
+//
+//                motor2->Move(StepperMotor::BWD,4000); //troisième rotation 90°
+//                motor1->Move(StepperMotor::BWD,4000);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//                printf("Rotation 3 \n");
+//
+//
+//                motor2->Move(StepperMotor::BWD,500); //Collage mur
+//                motor1->Move(StepperMotor::FWD,500);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//                printf("Collage\n");
+//
+//
+//                motor2->Move(StepperMotor::FWD,600); //recul pour rotation
+//                motor1->Move(StepperMotor::BWD,600);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//
+//                motor2->Move(StepperMotor::BWD,4000); //quatrième rotation 90°
+//                motor1->Move(StepperMotor::BWD,4000);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
+//                motor1->HardStop();
+//                motor2->HardStop();
+//                printf("Rotation 4 \n");
+                printf("deplacement arriere 40 \n");
+                deplacement(40,0);
+                printf("rotation 90 1 \n");
+                rotation(90, 30, 0);
+                printf("Rotation 1 \n");
+                printf("deplacement coller mur\n");
+                deplacement(112,1);
+                printf("deplacement arriere\n");
+                deplacement(3,0);
+                printf("rotation finale\n");
+                rotation(90, 30, 1);
+                printf("avancement 40\n");
+                deplacement(40,1);
 
-        step = 3;
-        break;
-    case 3:
-        //déploiement des bras, pêche
+                //Mise en position du robot pour pêche
+
+                step = 3;
+                break;
+            case 3:
+                //déploiement des bras, pêche
 
-        step = 4;
-        break;
-    case 4:
-        //Dépose des poissons dans le filet
+                step = 4;
+                break;
+            case 4:
+                //Dépose des poissons dans le filet
 
-        step = 5;
+                step = 5;
 
-    case 5:
-        end = 0;
-        break;
+            case 5:
+                break;
 
 
+        }
     }
-}
 
-parasol.write(0.5);
-printf("2");
-motor1->HardStop();
-motor2->HardStop();
+    parasol.write(0.5);
+    printf("2");
+    motor1->HardStop();
+    motor2->HardStop();
 }
 
 void pwm()