Yannick OMNES
azert
Dependencies: AX12 Servo VL6180x X_NUCLEO_COMMON X_NUCLEO_IHM01A1 mbed
Fork of
2-FisherMan
by 
Julien Tiron
Revision 4:0e9f65e3e2a0, committed 2016-05-06
- Comitter:
- Yannick292
- Date:
- Fri May 06 18:04:47 2016 +0000
- Parent:
- 3:387812a9386a
- Commit message:
- ramassage de poiscaille;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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()