Catégories
Liens
Ceci est une ancienne révision du document !
// ----------------- VARIABLES A PARAMETRER ----------------- int nb_briques = 2; // a changer int positions [][2] = { {3,2}, {5,4}, {2,6}, {4,3}, {6,5} }; // positions gauche > droite // electro aimant int V_electroPin = 7; // moteur vertical int V_moteurPinD = 10; // descente int V_moteurPinM = 11; // montée int V_enablePin = 12; // moteur translation int H_moteurPinG = 6; // gauche int H_moteurPinD = 5; // droite int H_enablePin = 4; // capteurs fin de course int V_FCMPin = 8; // remontée int V_FCBPin = 9; // dépose des briques int H_FCTPin = 3; // position translation //int H_FCDPin = 7; // depassement translation // variable d'arrêt et de debug (print) boolean ACTIVE = true; boolean DEBUG = true; boolean DEBUT = true; //----------------- SETUP -------------------------- void setup(){ if (DEBUG) Serial.begin(9600); //électroaimant pinMode(V_electroPin, OUTPUT); //V_moteur pinMode(V_moteurPinD, OUTPUT); pinMode(V_moteurPinM, OUTPUT); pinMode(V_enablePin, OUTPUT); //H_moteur pinMode(H_moteurPinG, OUTPUT); pinMode(H_moteurPinD, OUTPUT); pinMode(H_enablePin, OUTPUT); //fin de course pinMode(V_FCMPin, INPUT); pinMode(V_FCBPin, INPUT); pinMode(H_FCTPin, INPUT); //dépassement attachInterrupt (0, stop_program, RISING); // pin2 } void V_moteur_on (int state) { digitalWrite(V_enablePin, state); } void H_moteur_on (int state) { digitalWrite(H_enablePin, state); } void contact (int state) { if (DEBUG) Serial.print("CONTACT : "); if (DEBUG) Serial.println(state); if (ACTIVE) digitalWrite(V_electroPin, state); } void stop_program() { if (DEBUG) Serial.print("STOP!!. ACTIVE = FALSE"); H_moteur_on(LOW); V_moteur_on(LOW); contact(LOW); analogWrite(V_moteurPinM, 0); analogWrite(V_moteurPinD, 0); analogWrite(H_moteurPinG, 0); analogWrite(H_moteurPinD, 0); ACTIVE = false; while(true) {delay(5000);}; // attente infinie!! } // montee : V_go (V_moteurPinM, V_FCMPin, 190); // descente : V_go (V_moteurPinD, V_FCBPin, 140); void V_go (int moteurPin, int FCPin, int vitesse) { boolean go = true; int FC_state = 0; int FC_lastState = 0; if (DEBUG) Serial.println("V_moteur ON"); V_moteur_on(HIGH); analogWrite(moteurPin, vitesse); while (go) { FC_state = digitalRead(FCPin); // test changement d'état if (FC_state != FC_lastState) { // stop if (FC_state == HIGH) { if (DEBUG) Serial.println("STOP V_moteur"); analogWrite(moteurPin, 0); V_moteur_on(LOW); go = false; } } FC_lastState = FC_state; delay(15); } } // droite : H_go (H_moteurPinD, pos_droite[brique]); // gauche : H_go (H_moteurPinG, pos_gauche[brique]); void H_go (int moteurPin, int X) { boolean go = true; int acc_pwm = 0; int pwm; int FC_state; int FC_lastState = 0; int compteur = 0; int H_distance = X; int vitesse = 255; if (DEBUG) Serial.println("H_moteur ON"); if (DEBUG) {Serial.print("H_distance :");Serial.println(H_distance);} H_moteur_on(HIGH); while (go) { FC_state = digitalRead(H_FCTPin); // acceleration if (acc_pwm <= vitesse) { analogWrite(moteurPin, acc_pwm); acc_pwm += 5; } // fin de course if (FC_state != FC_lastState) { if (FC_state == HIGH) { compteur++; H_distance = X - compteur; if (DEBUG) {Serial.print("H_distance :");Serial.println(H_distance);} if (H_distance <= 3) { analogWrite(moteurPin, 120); // réduction de la vitesse s'il reste 3 crans à parcourir } // stop if (H_distance <= 0) { analogWrite(moteurPin, 0); if (DEBUG) Serial.println("STOP H_moteur"); H_moteur_on(LOW); go = false; } } } FC_lastState = FC_state; delay(50); } } void vertical (int colle) { if (DEBUG) Serial.println("DESCENTE"); V_go (V_moteurPinD, V_FCBPin, 150); // descend delay(1000); contact(colle); // contact delay(2000); if (DEBUG) Serial.println("MONTEE"); V_go (V_moteurPinM, V_FCMPin, 230); // montée delay(3000); } // -------------------------- void loop() { delay(3000); int X; if (ACTIVE) { // Début : placement au dessus de la brique 1 if (DEBUT) H_go (H_moteurPinG, positions[0][0]); // gauche DEBUT = false; // #1 boucle : amener briques de gauche à droite for ( int brique = 0; brique < nb_briques; brique++ ){ // récupère vertical(HIGH); // droite X = (positions[brique][0] - 1) + positions[brique][1]; H_go(H_moteurPinD, X); delay(3000); // dépose vertical(LOW); // gauche sauf pour la dernière brique if (brique == (nb_briques - 1) ) { X = (positions[brique][1] - 1) + positions[brique + 1][0]; H_go(H_moteurPinG, X); delay(3000); } } // #2 boucle : amener briques de droite à gauche for ( int brique = nb_briques - 1; brique >= 0; brique-- ){ // récupère vertical(HIGH); // gauche X = (positions[brique][1] - 1) + positions[brique][0]; H_go(H_moteurPinG, X); delay(3000); // dépose vertical(LOW); // droite sauf pour la dernière brique if (brique == 0 ) { X = (positions[brique][0] - 1) + positions[brique - 1][1]; H_go(H_moteurPinD, X); delay(3000); } } } //ACTIVE = false; // pour ne faire qu'une boucle }