Diff: main.cpp

Revision:

4:a95f90a0410d

Parent:

2:f6f12530dbd9

--- a/main.cpp	Sat Apr 02 14:48:08 2016 +0000
+++ b/main.cpp	Thu May 05 20:05:29 2016 +0000
@@ -33,7 +33,7 @@
 bool tag = true;
 int i2cAddres=0x70;                // Address of DS1307 is 0x68 (7 bit address)
 int i2c8BitAddres= i2cAddres <<1;  // Convert to 8bit addressing used by mbed
-volatile unsigned int step = 1;
+volatile unsigned int step = 0;
 /* Motor Control Component */
 L6474 *motor1;
 L6474 *motor2;
@@ -68,21 +68,54 @@
 
 }
 
-void deplacement(int distanceEnCm){
+void deplacement(int distanceEnCm)
+{
     int nbPas = distanceEnCm*160;
-    motor1->Move(StepperMotor::FWD,nbPas);
-    motor2->Move(StepperMotor::BWD,nbPas);
-    motor1->WaitWhileActive();
-    motor2->WaitWhileActive();
-}
-
-void rotation(int angleEnDegre, int diametre){
-    int deplacementEnCm = (angleEnDegre/360)*(diametre-2)*3.14;
-    int nbPas = 160*deplacementEnCm;
-    motor1->Move(StepperMotor::FWD,nbPas);
+    motor1->Move(StepperMotor::BWD,nbPas);
     motor2->Move(StepperMotor::FWD,nbPas);
     motor1->WaitWhileActive();
     motor2->WaitWhileActive();
+    motor1->HardStop();
+    motor2->HardStop();
+}
+
+void rotationD(int angleEnDegre, int diametre)
+{
+    int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
+    int nbPas = 160*deplacementEnCm;
+    nbPas /= 500;
+
+    /*
+    int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
+    int nbPas = 160*deplacementEnCm;
+    nbPas *= 2;*/
+
+    printf("%d \n\r", nbPas);
+    motor2->Move(StepperMotor::FWD,nbPas);
+    motor1->Move(StepperMotor::FWD,nbPas);
+    motor1->WaitWhileActive();
+    motor2->WaitWhileActive();
+    motor1->HardStop();
+    motor2->HardStop();
+}
+void rotationG(int angleEnDegre, int diametre)
+{
+    int deplacementEnCm = (1000*angleEnDegre/360)*(diametre)*3.14;
+    int nbPas = 160*deplacementEnCm;
+    nbPas /= 500;
+
+    /*
+    int deplacementEnCm = (int)((float) angleEnDegre/ (float) 360)*((float) diametre)* (float) 3.14;
+    int nbPas = 160*deplacementEnCm;
+    nbPas *= 2;*/
+
+    printf("%d \n\r", nbPas);
+    motor2->Move(StepperMotor::BWD,nbPas);
+    motor1->Move(StepperMotor::BWD,nbPas);
+    motor1->WaitWhileActive();
+    motor2->WaitWhileActive();
+    motor1->HardStop();
+    motor2->HardStop();
 }
 
 /* Main ----------------------------------------------------------------------*/
@@ -103,18 +136,37 @@
         exit(EXIT_FAILURE);
     int result = i2c.write(i2c8BitAddres, &data, 1 );
     sensor.VL6180xDefautSettings();
-
+    wait(1);
     /* Interrupt to start the robot */
     startup.fall(&go);
 
-    /* Interrupt to stop the robot */
-    game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
 
     while(start) {
         /* Waiting code */
-
+        //deplacement(10);
+        /*rotation(45, 9);
+        wait(2);
+        rotation(90,9);
+        wait(2);
+        rotation(45,9);
+        wait(2);
+        rotation(180,9);
+        wait(2);
+        rotation(45,9);
+        start = 0;
+        */
+        /*
+        motor1->Move(StepperMotor::FWD,nbPas);
+        motor2->Move(StepperMotor::BWD,nbPas);
+        motor1->WaitWhileActive();
+        motor2->WaitWhileActive();*/
     }
-
+    
+    
+    /* Interrupt to stop the robot */
+    game_length.attach(&stop, 90); //1 minutes 30 secondes pour la Coupe
+    
+    
     while(end) {
         /* In-game code */
 
@@ -122,6 +174,8 @@
         switch (step) {
 
             case 0:
+
+                wait(1);
                 step = 1;
                 break;
 
@@ -129,34 +183,45 @@
                 /* Avancer du tapis judqu'au mur */
                 motor1->Run(StepperMotor::FWD);
                 motor2->Run(StepperMotor::BWD);
-                while(sensor.getDistance()>30) {
+                int dis;
+                while(((dis = sensor.getDistance())>30)) {
                     //Tant que la distance est superieure on continue d'avancer
                     wait_ms(20);
                 }
-                motor1->Move(StepperMotor::FWD,1100);
-                motor2->Move(StepperMotor::BWD,1100);
-                motor1->WaitWhileActive();
-                motor2->WaitWhileActive();
+                printf("%d \n\r", dis);
+//                motor1->Move(StepperMotor::FWD,1300);
+//                motor2->Move(StepperMotor::BWD,1300);
+//                motor1->WaitWhileActive();
+//                motor2->WaitWhileActive();
                 motor1->HardStop();
                 motor2->HardStop();
                 step = 2 ;
                 break;
 
             case 2 : /* Angle 90° */
-                motor1->Move(StepperMotor::FWD,2100);
-                motor2->Move(StepperMotor::FWD,2100);
+                rotationD(90,9); //rotation droite
                 motor1->WaitWhileActive();
                 motor2->WaitWhileActive();
                 step = 3 ;
                 break;
 
             case 3 : /* Fermeture de 2 portes : 60cm avant + arrêt à 3 cm du mur */
-                motor1->Move(StepperMotor::FWD,9000); //160pas = 1cm
-                motor2->Move(StepperMotor::BWD,9000);
+                motor1->Move(StepperMotor::FWD,7000); //160pas = 1cm
+                motor2->Move(StepperMotor::BWD,7000);
                 motor1->WaitWhileActive();
                 motor2->WaitWhileActive();
-                motor1->Run(StepperMotor::FWD);
-                motor2->Run(StepperMotor::BWD);
+                rotationG(180,9);
+                deplacement(20);
+                rotationD(90,9);
+                deplacement(20);
+                rotationD(90,9);
+                deplacement(40);
+                motor1->WaitWhileActive();
+                motor2->WaitWhileActive();
+                
+                //160 pas = 1 cm
+                //motor1->Run(StepperMotor::FWD);
+                //motor2->Run(StepperMotor::BWD);
                 while(sensor.getDistance()>30) {
                     //Tant que la distance est superieure on continue d'avancer
                     wait_ms(20);
@@ -172,28 +237,6 @@
 
         }
 
-
-        /* Waiting until delay has expired. */
-        /*wait_ms(3000);
-
-        motor1->HardStop();
-        motor2->HardStop();
-
-        motor1->WaitWhileActive();
-        motor2->WaitWhileActive();
-
-        /* Requesting to run backward. */
-        /*motor1->Run(StepperMotor::FWD);
-        motor2->Run(StepperMotor::BWD);
-
-        /* Waiting until delay has expired. */
-        /*wait_ms(3000);
-
-        motor1->HardStop();
-        motor2->HardStop();
-
-        motor1->WaitWhileActive();
-        motor2->WaitWhileActive();*/
     }
 
     motor1->HardStop();