/* Dieses Programm steuert den Bagger ohne jegliche Sensoren.
* ********************Die Steppersoftware ist selber gemacht.*******************************
***** es sind die Bagger - Programme
***** STOP und
***** Justierung
***** Motor 2 fahren >>>> mit Schaufelbewegung
***** funktionsfähig
* *********** um die PullUp-Widerstände zu nutzen, LOW = Aktivierung der Funktion *****************
- Nr.1 (PIN 2) Stillstand
- Nr.2 (PIN 3) Justierung
- Nr.3 (PIN 4) eine Bewegung von M2 mit folgender Schaufelbewegung
- Nr.4 (PIN 5) eine Bewegung von M3 mit folgender Schaufelbewegung
- Nr.5 (PIN 6) eine Koordinate anfahren
- Nr.6 (PIN 7) einen "LKW" beladen
SUCH-ZIELE: #include void M1_drive void M2_drive void Zeitablauf1()
boolean Zeit_EIN void Justierung() void setup void loop()
*/
#include <SimpleTimer.h> // einbinden der Library
SimpleTimer timer1; // Benennung der Timer
#include <Bounce2.h> // einbinden der Library für die Unterdrückung des Kontaktentprellens
Bounce debouncer = Bounce();
float microsalt;
boolean MK1_1; // Variable für die Eingänge des Mäusklaviers 1; schalten gegen GND
boolean MK1_2; // sie sind aktiv bei LOW
boolean MK1_3;
boolean MK1_4;
boolean MK1_5;
boolean MK1_6;
boolean MK1_7;
boolean MK1_8;
boolean JustM3var = LOW; // wird erst HIGH, wenn JustPin wieder HIGH ist; Funktion Fangschaltung
boolean M1solldrehen = true; // für UP Justierung
boolean M2solldrehen = true; // für UP Justierung
boolean M3solldrehen = false; // für UP Justierung
boolean Zeitueberlauf = false; // Wird false, wenn Zeit von SimpleTimer 1 abgelaufen ist
boolean Zyklus_fahren = true;
int JustPin_roh = 53; // mit digital Eingang 53 wird beim Justieren die jeweilige Motorbewegung gestoppt
int JustPin; // diese Variable nimmt das Ergebnis nach der Prellunterdrückung an
int zeitId; // für den SimpleTimer
//++++++++++++++++++ für weitere UPs
boolean M1_fertig = false;
boolean M1_Start = true;
int Zyklus_M1 = 1;
float lzahn1_alt = 1.0; // Ausfahrlänge von Zahnstange 1 nach Justierung 1
float lzahn1; // Ausfahrlänge von Zahnstange 1
boolean M2_fertig = false;
boolean M2_Start = true;
int Zyklus_M2 = 1;
float lzahn2_alt = 98.0; // Ausfahrlänge von Zahnstange 2 nach Justierung 98
float lzahn2; // Ausfahrlänge von Zahnstange 2
boolean TimerAktiv = false;
//++++++++++++++++++++++++++++++++++
// ***************************** UP M1_drive ************************************************** ***************
void M1_drive (int p1, int p2, int p3, int p4, float n, int v, boolean dir) {
// n : Anzahl Umdrehungen; v in U/min; dir: 0 oder 1
static int M1_i;
static float M1_microsalt;
static float SPUsM1 = 1440.0; // entspricht etwa 1 U/min
static unsigned int Schrittdauer;
static unsigned int Schrittzahl = 1;
static unsigned int SchrittzahlEnd;
if ( M1_Start == true) { // wird nur beim 1. Aufruf des UPs erfüllt, wenn M1_Start = true
Schrittdauer = int(1000000.0 * 60.0 / (v * SPUsM1) ); // Ergebnis in us; 660 us =: 1515 Hz klappt noch als Minimum
//------------------------------------------- 10 U/min := 1464 us =: 683 Hz =: 0,16667 U/s =: 60 °/s
SchrittzahlEnd = n * SPUsM1;
M1_microsalt = micros();
M1_Start = false;
M1_i = 0;
} // ************ ENDE if ( M1_Start == true)
if (dir == 0 && (micros() - M1_microsalt) > Schrittdauer) { // Drehrichtung
switch (M1_i) { // es soll nur ein Schritt ausgeführt werden.
case 0:
M1_i = 1;
goto M1_0;
break;
case 1:
M1_i = 2;
goto M1_1;
break;
case 2:
M1_i = 3;
goto M1_2;
break;
case 3:
M1_i = 4;
goto M1_3;
break;
case 4:
M1_i = 5;
goto M1_4;
break;
case 5:
M1_i = 6;
goto M1_5;
break;
case 6:
M1_i = 7;
goto M1_6;
break;
case 7:
M1_i = 0;
goto M1_7;
break;
}
} // ************ ENDE if (dir == 0 && (micros() - M1_microsalt) > Schrittdauer)
else if (dir == 1 && (micros() - M1_microsalt) > Schrittdauer) { // Drehrichtung
switch (M1_i) { // es soll nur ein Schritt ausgeführt werden.
case 0:
M1_i = 1;
goto M1_7;
break;
case 1:
M1_i = 2;
goto M1_6;
break;
case 2:
M1_i = 3;
goto M1_5;
break;
case 3:
M1_i = 4;
goto M1_4;
break;
case 4:
M1_i = 5;
goto M1_3;
break;
case 5:
M1_i = 6;
goto M1_2;
break;
case 6:
M1_i = 7;
goto M1_1;
break;
case 7:
M1_i = 0;
goto M1_0;
break;
}
M1_0:
digitalWrite(p1, HIGH);
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
goto M1_Schritt;
M1_1:
digitalWrite(p1, HIGH);
digitalWrite(p2, HIGH);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
goto M1_Schritt;
M1_2:
digitalWrite(p1, LOW);
digitalWrite(p2, HIGH);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
goto M1_Schritt;
M1_3:
digitalWrite(p1, LOW);
digitalWrite(p2, HIGH);
digitalWrite(p3, HIGH);
digitalWrite(p4, LOW);
goto M1_Schritt;
M1_4:
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
digitalWrite(p3, HIGH);
digitalWrite(p4, LOW);
goto M1_Schritt;
M1_5:
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
digitalWrite(p3, HIGH);
digitalWrite(p4, HIGH);
goto M1_Schritt;
M1_6:
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, HIGH);
goto M1_Schritt;
M1_7:
digitalWrite(p1, HIGH);
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, HIGH);
goto M1_Schritt;
return;
M1_Schritt:
M1_microsalt = micros();
if ( Schrittzahl++ > SchrittzahlEnd) { // Der Auftrag vom aufrufenden Programm ist beendet
Schrittzahl = 0;
M1_fertig = true; // wenn Auftrag "M1_drive" erledigt ist
digitalWrite(p1, LOW); // alle LOW, damit der Motor im Stillstand stromlos ist.
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
} // ********** ENDE if ( Schrittzahl++ == SchrittzahlEnd)
} // ************ ENDE else if (dir == 1 && (micros() - M1_microsalt) > Schrittdauer)
} //******************* ENDE UP M1_drive ************************************************** *****************************
// ***************************** UP M2_drive ************************************************** **************************
void M2_drive (int p1, int p2, int p3, int p4, float n, int v, boolean dir) {
// n : Anzahl Umdrehungen; v in U/min; dir: 0 oder 1
static int M2_i;
static float M2_microsalt;
static float SPUsM2 = 4096.0; // entspricht etwa 1 U/min
static unsigned int Schrittdauer;
static unsigned int Schrittzahl = 1;
static unsigned int SchrittzahlEnd;
if ( M2_Start == true) { // wird nur beim 1. Aufruf des UPs erfüllt, wenn M2_Start = true
Schrittdauer = int(1000000.0 * 60.0 / (v * SPUsM2) ); // Ergebnis in us; 660 us =: 1515 Hz klappt noch als Minimum
//------------------------------------------- 10 U/min := 1464 us =: 683 Hz =: 0,16667 U/s =: 60 °/s
SchrittzahlEnd = n * SPUsM2;
M2_microsalt = micros();
M2_Start = false;
M2_i = 0;
} // ************ ENDE if ( M2_Start == true)
if (dir == 0 && (micros() - M2_microsalt) > Schrittdauer) { // Drehrichtung
switch (M2_i) { // es soll nur ein Schritt ausgeführt werden.
case 0:
M2_i = 1;
goto M2_0;
break;
case 1:
M2_i = 2;
goto M2_1;
break;
case 2:
M2_i = 3;
goto M2_2;
break;
case 3:
M2_i = 4;
goto M2_3;
break;
case 4:
M2_i = 5;
goto M2_4;
break;
case 5:
M2_i = 6;
goto M2_5;
break;
case 6:
M2_i = 7;
goto M2_6;
break;
case 7:
M2_i = 0;
goto M2_7;
break;
}
} // ************ ENDE if (dir == 0 && (micros() - M2_microsalt) > Schrittdauer)
else if (dir == 1 && (micros() - M2_microsalt) > Schrittdauer) { // Drehrichtung
switch (M2_i) { // es soll nur ein Schritt ausgeführt werden.
case 0:
M2_i = 1;
goto M2_7;
break;
case 1:
M2_i = 2;
goto M2_6;
break;
case 2:
M2_i = 3;
goto M2_5;
break;
case 3:
M2_i = 4;
goto M2_4;
break;
case 4:
M2_i = 5;
goto M2_3;
break;
case 5:
M2_i = 6;
goto M2_2;
break;
case 6:
M2_i = 7;
goto M2_1;
break;
case 7:
M2_i = 0;
goto M2_0;
break;
}
M2_0:
digitalWrite(p1, HIGH);
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
goto M2_Schritt;
M2_1:
digitalWrite(p1, HIGH);
digitalWrite(p2, HIGH);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
goto M2_Schritt;
M2_2:
digitalWrite(p1, LOW);
digitalWrite(p2, HIGH);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
goto M2_Schritt;
M2_3:
digitalWrite(p1, LOW);
digitalWrite(p2, HIGH);
digitalWrite(p3, HIGH);
digitalWrite(p4, LOW);
goto M2_Schritt;
M2_4:
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
digitalWrite(p3, HIGH);
digitalWrite(p4, LOW);
goto M2_Schritt;
M2_5:
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
digitalWrite(p3, HIGH);
digitalWrite(p4, HIGH);
goto M2_Schritt;
M2_6:
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, HIGH);
goto M2_Schritt;
M2_7:
digitalWrite(p1, HIGH);
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, HIGH);
goto M2_Schritt;
return;
M2_Schritt:
M2_microsalt = micros();
if ( Schrittzahl++ > SchrittzahlEnd) { // Der Auftrag vom aufrufenden Programm ist beendet
Schrittzahl = 0;
// M2_Start = true; // nötig für den nächsten Auftrag ???????
M2_fertig = true; // wenn Auftrag "M2_drive" erledigt ist
digitalWrite(p1, LOW); // alle LOW, damit der Motor im Stillstand stromlos ist.
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
digitalWrite(p4, LOW);
} // ********** ENDE if ( Schrittzahl++ == SchrittzahlEnd)
} // ************ ENDE else if (dir == 1 && (micros() - M2_microsalt) > Schrittdauer)
} //******************* ENDE UP M2_drive ************************************************** ****************
//**************************** UP Zeitablauf1 ************************************************** ********
void Zeitablauf1() { // wird ausgeführt, wenn SimpleTimer timer 1 abgelaufen ist
Zeitueberlauf = true; //
} // *********** ENDE UP Zeitablauf1 ************************************************** **************
// ************************************** UP Zeit_EIN ************************************************** *
boolean Zeit_EIN (boolean Mx_laeuft ) { // Zeitueberlauf ist global deklariert
// static boolean TimerAktiv = false; // true, wenn Timer angestoßen ist
if (Mx_laeuft == true) {
return false;
}
if (TimerAktiv == true) {
if ( Zeitueberlauf == true) {
return false;
}
// ************ENDE if ( Zeitueberlauf == true)
else if (Zeitueberlauf == false) {
return true; // true, damit dann auch UP Justierung verlassen wird.
} // ************ENDE else (Timer1aktiv == true)
} // ***********ENDE if (TimerAktiv == true)
zeitId = timer1.setTimeout(3000, Zeitablauf1); // dieser Aufruf löscht sich nach einer Benutzung selber
TimerAktiv = true;
return true;
} // *********** ENDE UP Zeit_EIN ************************************************** *******************************
// ************************************** UP Justierung ************************************************** ****************
void Justierung() {
static boolean M1_fertig = false;
static boolean M2_fertig = false;
static boolean M1_laeuft = false;
static boolean M2_laeuft = false;
static boolean Z_EIN; // true, solange Timer 1 läuft
if (M1_fertig == true) { // ab hier Motor 1 justieren
goto M2_Sprung; // M1 wird übersprungen
}
if (JustPin == HIGH) {
M1_laeuft = true;
M1_drive(41, 43, 45, 47, 30, 3, 1); // 41, 43, 45, 47, Mx_n, Mx_v, Mx_dir ; für den MEGA
// Mx_n : Zahl Umdrehungen, die der M1 machen soll
// muss hier so hoch sein, dass Zahnstange bis zum Ende fahren kann
// geht ohne Belastung noch bis ? ; tatsächliche Geschwindigkeit aber ungenau
// gewünschte Richtung von Mx; 1 einfahren, 0 ausfahren
return;
} // *********** ENDE if (JustPin == HIGH)
if (Zeit_EIN(M1_laeuft) == true) { // Aufruf des UPs Zeit_EIN(), das die Zeit startet, in der JustPin LOW sein muss, damit eine Motor übersprungen wird.
return; // Sprung aus UP Justierung
}
timer1.disable(zeitId); // Timer1 löschen
M1_fertig = true; // wenn Auftrag vom Hauptprogramm erledigt ist
M1_Start == true;
Zeitueberlauf = false; // eigenes Zeitsignal löschen
TimerAktiv = false;
lzahn1_alt = 1.0;
digitalWrite(41, LOW); // alle LOW, damit der Motor im Stillstand stromlos ist.
digitalWrite(43, LOW);
digitalWrite(45, LOW);
digitalWrite(47, LOW);
M2_Sprung: // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ab hier Start Motor 2 justieren
if (M2_fertig == true) {
goto M3_Sprung; // M2 wird übersprungen
}
if (JustPin == HIGH) {
M2_laeuft = true;
M2_drive(22, 24, 26, 28, 20, 3, 0); // 22, 24, 26, 28, Mx_n, Mx_v, Mx_dir
// Mx_n : Zahl Umdrehungen, die der M2 machen soll
// muss hier so hoch sein, dass Zahnstange bis zum Ende fahren kann
// geht ohne Belastung noch bis 18 ; tatsächliche Geschwindigkeit aber ungenau
// gewünschte Richtung von Mx; 1 gegen Uhrzeiger CCW, 0 mit Uhrzeiger CW
return;
} // *********** ENDE if (JustPin == HIGH)
if (Zeit_EIN(M2_laeuft) == true) { // Aufruf des UPs Zeit_EIN(), das die Zeit startet, in der JustPin LOW sein muss, damit eine Motor übersprungen wird.
return; // Sprung aus UP Justierung
}
timer1.disable(zeitId); // Timer1 löschen
M2_fertig = true; // wenn Auftrag vom Hauptprogramm erledigt ist
M2_Start == true;
Zeitueberlauf = false; // eigenes Zeitsignal löschen
TimerAktiv = false;
lzahn2_alt = 98.0;
digitalWrite(22, LOW); // alle LOW, damit der Motor im Stillstand stromlos ist.
digitalWrite(24, LOW);
digitalWrite(26, LOW);
digitalWrite(28, LOW);
M3_Sprung: // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ab hier Start Motor 3
return;
} //************* ENDE UP Justierung ************************************************** *****************************
void setup() {
Serial.begin (250000); // diese Buadrate muss auch in der Konsole (Serieller Monitor) eingestellt sein
while (!Serial);
pinMode (JustPin_roh, INPUT); // wird nur im UP Justierung benötigt
digitalWrite(JustPin_roh, HIGH); //schaltet den PullUp-Widerstand ein
debouncer.attach(JustPin_roh); // für Prellunterdrückung von Pin 53
debouncer.interval(100); // interval in ms
pinMode (2, INPUT); // Pins als Eingänge deklarieren
pinMode (3, INPUT);
pinMode (4, INPUT);
pinMode (5, INPUT);
pinMode (6, INPUT);
pinMode (7, INPUT);
pinMode (8, INPUT);
pinMode (13, OUTPUT);
digitalWrite (13, LOW);
digitalWrite (2, HIGH); // schaltet die 20 kOhm PullUp-widerstände ein
digitalWrite (3, HIGH);
digitalWrite (4, HIGH);
digitalWrite (5, HIGH);
digitalWrite (6, HIGH);
digitalWrite (7, HIGH);
digitalWrite (8, HIGH);
pinMode (41, OUTPUT); // M1
pinMode (43, OUTPUT);
pinMode (45, OUTPUT);
pinMode (47, OUTPUT);
pinMode (22, OUTPUT); // M2
pinMode (24, OUTPUT);
pinMode (26, OUTPUT);
pinMode (28, OUTPUT);
pinMode (32, OUTPUT); // M3
pinMode (34, OUTPUT);
pinMode (36, OUTPUT);
pinMode (38, OUTPUT);
pinMode (42, OUTPUT); // M4
pinMode (44, OUTPUT);
pinMode (46, OUTPUT);
pinMode (48, OUTPUT);
pinMode (31, OUTPUT); // M5
pinMode (33, OUTPUT);
pinMode (35, OUTPUT);
pinMode (37, OUTPUT);
}
void loop() {
// für die Prellunterdrückung des Justierstopeingangs
debouncer.update(); // aktualisiert den Bouncezustand
JustPin = debouncer.read(); // übergibt den aktualisierten Wert an die Variable JustPin. Diese wird im weiteren Programm verwendet
MK1_1 = HIGH; //digitalRead(2); // STOP > die Variablen MK.. erhalten die Zustände der Eingänge
MK1_2 = LOW; //digitalRead(3); // Justieren; sie sind aktiv bei LOW
MK1_3 = HIGH; //digitalRead(4); // M1 und M2 fahren
MK1_4 = HIGH; //digitalRead(5); // M1 und M3 fahren
MK1_5 = HIGH; //digitalRead(6); // xy anfahren float HP_X1
MK1_6 = HIGH; //digitalRead(7); // LKW beladen
MK1_7 = HIGH; //digitalRead(
;
MK1_8 = HIGH; //digitalRead(9);
if (MK1_1 == LOW ) { //STOP
// Zyklus_M2 = 1 ; // d.h. M2 soll den 1. Zyklus fahren
}
else if (MK1_2 == LOW && MK1_1 == HIGH ) { // Justierung
Justierung(); // Programm 2: Justierung
}
LOOP_END:
timer1.run();
} //************* ENDE loop
Zitieren
Zitat von Peter(TOO)
Lesezeichen