Diff: main.cpp

Revision:

2:f6f12530dbd9

Parent:

1:e18e367432bd

Child:

4:a95f90a0410d

--- a/main.cpp	Sat Apr 02 09:25:30 2016 +0000
+++ b/main.cpp	Sat Apr 02 14:21:53 2016 +0000
@@ -15,6 +15,7 @@
 #include "l6474_class.h"
 #include "DevI2C.h"
 #include "vl6180x_class.h"
+#include "VL6180x.h"
 
 /* Definitions ---------------------------------------------------------------*/
 
@@ -23,20 +24,27 @@
 /* Variables -----------------------------------------------------------------*/
 
 /* Start and  Stop Component */
+
 InterruptIn startup(PC_1);
 Ticker game_length;
 volatile bool start = 1;
 volatile bool end = 1;
-
+char data = 0x08 | (char)64;
+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;
 /* Motor Control Component */
 L6474 *motor1;
 L6474 *motor2;
 
 /* Distance Sensors Component */
-DevI2C *i2c =new DevI2C(D14, D15); 
+//DevI2C *i2c =new DevI2C(D14, D15);
 /*VL6180X sensor1(i2c);
 VL6180X sensor2(i2c);
 VL6180X sensor3(i2c);*/
+I2C i2c(D14, D15);
+VL6180x sensor(D14, D15, VL6180X_ADDRESS<<1);
 
 /* Functions -----------------------------------------------------------------*/
 
@@ -50,14 +58,32 @@
     end = 0;
 }
 
-void init_sensor(){
-    
+void init_sensor()
+{
+
 }
-    
-void switch_sensor(int number){
-    
+
+void switch_sensor(int number)
+{
+
 }
 
+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);
+    motor2->Move(StepperMotor::FWD,nbPas);
+    motor1->WaitWhileActive();
+    motor2->WaitWhileActive();
+}
 
 /* Main ----------------------------------------------------------------------*/
 
@@ -75,51 +101,101 @@
         exit(EXIT_FAILURE);
     if (motor2->Init() != COMPONENT_OK)
         exit(EXIT_FAILURE);
+    int result = i2c.write(i2c8BitAddres, &data, 1 );
+    sensor.VL6180xDefautSettings();
 
     /* 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 */
-        
+
     }
-    
+
     while(end) {
         /* In-game code */
-        
-        motor1->Move(StepperMotor::FWD,4096);
-        motor2->Move(StepperMotor::BWD,4096);
-        
-        /* Requesting to run backward. */
-        motor1->Run(StepperMotor::BWD);
-        motor2->Run(StepperMotor::FWD);
+
+
+        switch (step) {
+
+            case 0:
+                step = 1;
+                break;
+
+            case 1 :
+                /* Avancer du tapis judqu'au mur */
+                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);
+                }
+                motor1->Move(StepperMotor::FWD,1100);
+                motor2->Move(StepperMotor::BWD,1100);
+                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);
+                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->WaitWhileActive();
+                motor2->WaitWhileActive();
+                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);
+                }
+                motor1->HardStop();
+                motor2->HardStop();
+                step = 4;
+                break;
+
+            case 4 :
+                // STOP : robot arrêté
+                break;
+
+        }
+
 
         /* Waiting until delay has expired. */
-        wait_ms(3000);
-        
+        /*wait_ms(3000);
+
         motor1->HardStop();
         motor2->HardStop();
 
         motor1->WaitWhileActive();
         motor2->WaitWhileActive();
-        
+
         /* Requesting to run backward. */
-        motor1->Run(StepperMotor::FWD);
+        /*motor1->Run(StepperMotor::FWD);
         motor2->Run(StepperMotor::BWD);
 
         /* Waiting until delay has expired. */
-        wait_ms(3000);
-        
+        /*wait_ms(3000);
+
         motor1->HardStop();
         motor2->HardStop();
 
         motor1->WaitWhileActive();
-        motor2->WaitWhileActive();
+        motor2->WaitWhileActive();*/
     }
-    
+
     motor1->HardStop();
     motor2->HardStop();