Julien Tiron
hh
Dependencies: VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed
Fork of
1-DoorCloser_jor
by 
Jordan Ml
Revision 4:a95f90a0410d, committed 2016-05-05
- Comitter:
- julientiron
- Date:
- Thu May 05 20:05:29 2016 +0000
- Parent:
- 3:cbd7b3a6c722
- Commit message:
- kk;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r cbd7b3a6c722 -r a95f90a0410d main.cpp
--- 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();