/*Filamentaufwickler_V0.1.9.2_w/CodeAufteilung*/
//Einbindung einer benötigten Bibliothek
//Programmcode ausgelegt auf Messinstrument ohne Tastrolle
#include <LiquidCrystal_I2C.h>
#include <TimerOne.h>
//Deklaration aller Variablen, welche bereits im Setup benötigt werden:
#define PIN_TRIG 9 // Schallwellenerzeuger des Abstandssensors
#define PIN_ECHO 8 // Schallwellenempfänger des Abstandssensors
//Deklaration aller Pins für die Ansteuerung des ersten Schrittmotors (Schlitten)
#define stepperDriver1_MS1 A0
#define stepperDriver1_MS2 12
#define stepperDriver1_MS3 11
#define stepperDriver1_Dir 4
#define stepperDriver1_Step 5
//Deklaration aller Pins für die Ansteuerung des zweiten Schrittmotors (Spule)
#define stepperDriver2_MS1 A3
#define stepperDriver2_MS2 A2
#define stepperDriver2_MS3 A1
#define stepperDriver2_Dir 7
#define stepperDriver2_Step 6
//Pinbelegung des Drehencoders
#define ENCODER_CLK 2 // Drehencoder Puls(A)
#define ENCODER_DT 3 // Drehencoder Puls(B)
#define ENCODER_BTN 13 // Drehencoder Taster
#define FilamentSensor 10 // Filamentsensor
// Konfiguration des eingebundenen LCD-Displays
LiquidCrystal_I2C lcd(0x27, 20, 4);
//Darstellung von Symbolen
byte wheel1_1[8] = {
0b00011,
0b00100,
0b01010,
0b01001,
0b01001,
0b01010,
0b00100,
0b00011
};
byte wheel1_2[8] = {
0b11000,
0b00100,
0b01010,
0b10010,
0b10010,
0b01010,
0b00100,
0b11000
};
byte wheel2_1[8] = {
0b00011,
0b00100,
0b01000,
0b01111,
0b01001,
0b01001,
0b00101,
0b00011
};
byte wheel2_2[8] = {
0b11000,
0b10100,
0b10010,
0b10010,
0b11110,
0b00010,
0b00100,
0b11000
};
byte arrow[8] = {
0b01000,
0b01100,
0b01110,
0b01111,
0b01110,
0b01100,
0b01000,
0b00000
};
/*
///////////////////////////////////////////////////////////////////////////////////////////
###### ######## ######## ## ## ########
## ## ## ## ## ## ## ##
## ## ## ## ## ## ##
###### ###### ## ## ## ########
## ## ## ## ## ##
## ## ## ## ## ## ##
###### ######## ## ####### ##
///////////////////////////////////////////////////////////////////////////////////////////
*/
void setup() {
// Initialisierung aller:
// -Inputs
pinMode(ENCODER_CLK, INPUT);
pinMode(ENCODER_DT, INPUT);
pinMode(ENCODER_BTN, INPUT_PULLUP);
pinMode(PIN_ECHO, INPUT);
Timer1.initialize(5000000);
Timer1.attachInterrupt(calculations);
// -Outputs
pinMode(stepperDriver1_MS1, OUTPUT);
pinMode(stepperDriver1_MS2, OUTPUT);
pinMode(stepperDriver1_MS3, OUTPUT);
pinMode(stepperDriver1_Dir, OUTPUT);
pinMode(stepperDriver1_Step, OUTPUT);
pinMode(stepperDriver2_MS1, OUTPUT);
pinMode(stepperDriver2_MS2, OUTPUT);
pinMode(stepperDriver2_MS3, OUTPUT);
pinMode(stepperDriver2_Dir, OUTPUT);
pinMode(stepperDriver2_Step, OUTPUT);
pinMode(PIN_TRIG, OUTPUT);
pinMode(FilamentSensor, INPUT_PULLUP);
// Grundzustand aller Outputs
digitalWrite(stepperDriver1_MS1, HIGH);
digitalWrite(stepperDriver1_MS2, HIGH);
digitalWrite(stepperDriver1_MS3, LOW);
digitalWrite(stepperDriver1_Dir, LOW);
digitalWrite(stepperDriver1_Step, LOW);
digitalWrite(stepperDriver2_MS1, HIGH);
digitalWrite(stepperDriver2_MS2, HIGH);
digitalWrite(stepperDriver2_MS3, LOW);
digitalWrite(stepperDriver2_Dir, LOW);
digitalWrite(stepperDriver2_Step, LOW);
// Initialisierung des Displays
lcd.begin(20, 4);
lcd.backlight();
lcd.print("Volkswagen - EIS21");
lcd.setCursor(0, 1);
lcd.print("APT2-Projekt");
lcd.setCursor(0, 2);
lcd.print("Filament-Aufwickler");
delay(1500);
lcd.clear();
//Erstellung der Zeichen für die Verwendung im Display
lcd.createChar(0, wheel1_1);
lcd.createChar(1, wheel1_2);
lcd.createChar(2, wheel2_1);
lcd.createChar(3, wheel2_2);
lcd.createChar(4, arrow);
drawMenu(); //Aufrufen des Kalibrierungs-Setup
//Serial.begin(9600);
}
/*
///////////////////////////////////////////////////////////////////////////////////////////
## ####### ####### ########
## ## ## ## ## ## ##
## ## ## ## ## ## ##
## ## ## ## ## ########
## ## ## ## ## ##
## ## ## ## ## ##
######## ####### ####### ##
///////////////////////////////////////////////////////////////////////////////////////////
*/
//----------------------------------------------------------------------------
//Variablen für das Festlegen der Mess-Wiederholrate
unsigned long m_currentMillis;
unsigned long m_previousMillis;
unsigned long m_lastCalculationTime = 0;
const unsigned long calculationInterval = 5000;
byte completesetup = 0;
//----------------------------------------------------------------------------
void loop() {
m_currentMillis = millis();
if(completesetup == 1)//Wenn das Setup abgeschlossen ist, dann soll die Anlage startbereit sein.
{
//Im 5-Sekundentakt soll der Füllstand der Spule überprüft und angezeigt werden.
if (m_currentMillis - m_previousMillis >= 5000) {
measuring();//Methode für das Ausmessen der Entfernung zur Spule //Methode zum Umrechnen der Entfernung zum Durchmesser
drawMenu(); //Methode zum Aufrufen des Menüinhalts
}
windingcontrol();//Methode zum Darstellen der Animation auf dem Display
}
readRotaryEncoder(); //Methode zum Auslesen der Drehrichtung
lcd.display(); //Bespielen des LCDs
}
/*
///////////////////////////////////////////////////////////////////////////////////////////
## ## ######## ###### ###### ## ## ## ## ######
### ### ## ## ## ## ## ## ## ### ## ## ##
#### #### ## ## ## ## ## #### ## ##
## ### ## ###### ###### ###### ## ## ## ## ## ## ####
## ## ## ## ## ## ## ## #### ## ##
## ## ## ## ## ## ## ## ## ## ### ## ##
## ## ######## ###### ###### ####### ## ## ######
///////////////////////////////////////////////////////////////////////////////////////////
*/
//----------------------------------------------------------------------------
float curdistance = 0; // aktuell gemessener Abstand (in mm)
//----------------------------------------------------------------------------
void measuring() {
// Messvorgang starten
digitalWrite(PIN_TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(PIN_TRIG, LOW);
//Da die Dauer mit "pulseIn" in us angegeben wird, muss der Wert noch umgewandelt werden.
float duration = pulseIn(PIN_ECHO, HIGH);
//Durch das Teilen durch 1000 wandeln wir den Wert von us in ms um
duration = duration/1000;
/*0.001 in der ersten Klammer, weil die duration-Variable einen Wert in ms angibt,
aber die Schallgeschwindigkeit einen Wert in m/s (laut Tabellenbuch 340m/s). So wird aus 340m/s gleich 0,34m/ms.
Das zweite Mal wird die Dezimalzahl dafür genutzt, um von der Einheit m auf mm zu kommen.*/
//Der Kehrwert von (340m/s*0.001)/(2*0.001) als Faktor = 5.88*10^-3s = 0.00588
//Der Faktor in der Berechnung weicht leicht vom Faktor aus dem drüber stehenden Kommentar
//ab. Durch das Vergleichen mit einem physisch gemessenen Abstand wurde der Faktor durch Herantasten
//angepasst, sodass der Wert programmtechnisch mit dem realen Wert übereinstimmt.
curdistance = duration/0.0052 ; // Aktuelle Distanz in mm
//Serial.println(curdistance);
m_previousMillis = m_currentMillis;
}
/*
///////////////////////////////////////////////////////////////////////////////////////////
######## ######## ######## ######## ###### ## ## ## ## ## ##
## ## ## ## ## ## ## ## ## ## ### ## ## ##
## ## ## ## ## ## ## ## ## #### ## ## ##
######## ###### ######## ###### ## ######### ## ## ## ## ##
## ## ## ## ## ## ## ## ## ## #### ## ##
## ## ## ## ## ## ## ## ## ## ## ### ## ##
######## ######## ## ## ######## ###### ## ## ## ## #######
## ## ######
### ## ## ##
#### ## ##
## ## ## ## ####
## #### ## ##
## ### ## ##
## ## ######
///////////////////////////////////////////////////////////////////////////////////////////
*/
//----------------------------------------------------------------------------
//Variablen zur Berechnung der Pulsdauer
/*maxdistance: Gemessene Distanz vom Ultraschallsensor bis zur Gewindestange,
die als Referenz zur Ermittlung des Spulendurchmessers wichtig ist (in mm)VF*
*/
float arrFilamentdiameter[3] = {1.75, 2.85, 3.00};
int currentIndex = 0;
float maxdistance = 198.4;
float curdiameter_reel = 0; // Aktueller Spulendurchmesser (in mm)
float diameter_reel = 0; // Durchmesser der Spule ohne Filament (in mm) VF*
float diameter_island = 9.5; // Durchmesser des Zahnriemens (in mm) VF*
float distance = 0; // Gemessener Abstand zur Spule ohne Filament (in mm)
float distancefullreel = 0; // Dient zum Abspeichern der Entfernung, bei welcher die Spule voll sein soll (in mm)
float process = 0; // Fortschritt der Füllmenge in Prozent
float timeperrep_reel = 0; // Zeitdauer pro Umdrehung der Spule (in s)
float timeperrep_island = 0; // Zeitdauer pro Umdrehung des Zahnriemens (in s)
float rotspeed_reel = 20; // Drehgeschwindigkeit der Spule (in mm/s) VF*
float rotspeed_island = 0; // Drehgeschwindigkeit der Insel (in mm/s) // Durchmesser des Filaments (in mm) VF*
float pulsetime_reel = 0; // Abstand der steigenden Flanken für die Spule(in ms)
float pulsetime_island = 0; // Abstand der steigenden Flanken für die Insel (in ms)
float stepsperrep = 200; // Anzahl an benötigten Schritten, um mit den Schrittmotoren eine Umdrehung zu absolvieren VF*
//*VF = wird vorher festgelegt
//----------------------------------------------------------------------------
void calculations() {
if (curdistance <= distance + 1) {
curdiameter_reel = diameter_reel + (distance - curdistance) * 2; // Bestimmung des Spulendurchmessers nach Sensor
timeperrep_reel = (curdiameter_reel * 3.1415) / (rotspeed_reel*0.001); // Bestimmung der Zeitdauer für eine Umdrehung der Spule; 0,001 zum Wandeln von mm/s in mm/ms
rotspeed_island = arrFilamentdiameter[currentIndex] / timeperrep_reel; // Bestimmung der Geschwindigkeit für die Insel
timeperrep_island = (diameter_island * 3.1415) / rotspeed_island; // Bestimmung der Zeitdauer für eine Umdrehung des Zahnriemens
/*Zeitlicher Abstand der jeweiligen Flanken. "/8" weil durch das Setzen der Microsteps der Schrittmotor nun 200*8 Schritte
pro Umdrehung macht (Feinste mögliche Einstellung mit dem A4988*/
pulsetime_reel = timeperrep_reel / stepsperrep/ 16;
pulsetime_island = timeperrep_island / stepsperrep/ 16;
process = (1 - ((curdistance - distancefullreel) / distance)) * 100;
//Anzeige des Fortschritts beim Aufwickeln
}
}
/*
///////////////////////////////////////////////////////////////////////////////////////////
###### ###### ## ## ######## #### ######## ########
## ## ## ## ## ## ## ## ## ## ##
## ## ## ## ## ## ## ## ##
###### ## ######### ######## ## ## ##
## ## ## ## ## ## ## ## ##
## ## ## ## ## ## ## ## ## ## ##
###### ###### ## ## ## ## #### ## ##
## ## ####### ######## ####### ########
### ### ## ## ## ## ## ## ##
#### #### ## ## ## ## ## ## ##
## ### ## ## ## ## ## ## ########
## ## ## ## ## ## ## ## ##
## ## ## ## ## ## ## ## ##
## ## ####### ## ####### ## ##
///////////////////////////////////////////////////////////////////////////////////////////
*/
//----------------------------------------------------------------------------
//Variablen zur gezielteren Steuerung des Schrittmotors für die Insel
bool direction = false; //Drehrichtung
int steps; //Anzahl der Schritte zum Merken der Grenzen
//Grundzustand der Augänge am Nano für die STEP-Pins am Treiber
bool stepper1state = false;
bool stepper2state = false;
byte breakval = 0; //Freigabewert für die Schrittmotoren zum Drehen
//Variablen für die spätere Taktfrequenz der Schrittmotoren
unsigned long previousSec1 = 0;
unsigned long previousSec2 = 0;
int stepsperfullwind = 0; // Schritte die benötigt werden, um die ganze Spule einmal abzufahren
//----------------------------------------------------------------------------
unsigned long currentSec1;
unsigned long currentSec2;
void windingcontrol()
{
currentSec1 = millis();
currentSec2 = currentSec1;/*Übergabe der aktuellen Betriebsdauer in Millisekunden durch "millis()".*/
//Serial.println(currentSec2);
if (process >= 99) // || digitalRead(filamentSensor) == HIGH //Sobald mehr als 99% erreicht wurden, soll der Betrieb eingestellt werden.
{
breakval = 2;
}
//Solange die Variable "breakval" dem Wert 1 besitzt, sollen die Schrittmotoren angesteuert werden.
if (breakval == 1) {//Festlegung der Taktzeit für den Schrittmotor der Insel
if (currentSec1 - previousSec1 >= pulsetime_island) {
previousSec1 = currentSec1;
stepper1state = HIGH;
/*Durch das Hochzählen der Schritte und vergleichen mit der maximalen Anzahl an
Schritten weiß der Arduino, wann die Insel, ihre Grenze erreicht hat, sodass
dann ein Richtungswechsel erfolgen soll.
*/
if (direction == true && steps < stepsperfullwind) {
steps+=1;
}
else {
direction = false;
}
if (direction == false && steps > 0)
{
steps-=1;
}
else {
direction = true;
}
digitalWrite(stepperDriver1_Dir, direction);
}
//Festlegung der Taktzeit für den Schrittmotor der Spule
if (currentSec2 - previousSec2 >= pulsetime_reel) {
previousSec2 = currentSec2;
stepper2state = HIGH;
//Serial.print("pulsetime_reel: ");
//Serial.println(pulsetime_reel);
}
//Schrittmotorenansteuerung
//Insel
//Serial.println(stepper1state);
digitalWrite(stepperDriver1_Step, stepper1state);
//Spule
digitalWrite(stepperDriver2_Step, stepper2state);
stepper1state = LOW;
stepper2state = LOW;
digitalWrite(stepperDriver1_Step, stepper1state);
digitalWrite(stepperDriver2_Step, stepper2state);
}
}
/*
///////////////////////////////////////////////////////////////////////////////////////////
### ## ## #### ## ## ### ######## #### ####### ## ##
## ## ### ## ## ### ### ## ## ## ## ## ## ### ##
## ## #### ## ## #### #### ## ## ## ## ## ## #### ##
## ## ## ## ## ## ## ### ## ## ## ## ## ## ## ## ## ##
######### ## #### ## ## ## ######### ## ## ## ## ## ####
## ## ## ### ## ## ## ## ## ## ## ## ## ## ###
## ## ## ## #### ## ## ## ## ## #### ####### ## ##
///////////////////////////////////////////////////////////////////////////////////////////
*/
//----------------------------------------------------------------------------
//Variablen für das Abspeichern der Zeitvariable
unsigned long curMillisA;
unsigned long prevMillisA;
//----------------------------------------------------------------------------
void animation()
{
//ANIMATION
/*Während das Hauptmenü offen ist, soll eine Animation abgespielt werden*/
curMillisA = millis();
int timeperframe = 500; //Dauer, wie lange ein Frame am Stück angezeigt werden soll
//Nach einer Sekunde (500msx2) soll die Animation von vorne beginnen.
if (curMillisA - prevMillisA > timeperframe * 2)
{
prevMillisA = curMillisA;
}
//Solange die ersten 500ms nicht vergangen sind, soll Frame 1 der Animation angezeigt werden
if (curMillisA - prevMillisA < timeperframe)
{
if (process >= 99)
{
lcd.setCursor(6, 1);
lcd.print("FERTIG");
}
if (process <= 99 && breakval == 1)
{
lcd.setCursor(18, 0);
lcd.write(byte(0));
lcd.write(byte(1));
}
}
//Wenn die ersten 500ms vergangen sind, soll Frame 2 der Animation angezeigt werden
else if (curMillisA - prevMillisA > timeperframe)
{
lcd.setCursor(6, 1);
lcd.print(" ");
lcd.setCursor(18, 0);
lcd.write(byte(2));
lcd.write(byte(3));
}
}
/*
///////////////////////////////////////////////////////////////////////////////////////////
## ## ######## ## ## ## ##
### ### ## ### ## ## ##
#### #### ## #### ## ## ##
## ### ## ###### ## ## ## ## ##
## ## ## ## #### ## ##
## ## ## ## ### ## ##
## ## ######## ## ## #######
///////////////////////////////////////////////////////////////////////////////////////////
*/
//----------------------------------------------------------------------------
//Variablen für Kalibrierungs-Setup auf dem Display und intern
int setuppage = 0; //Ebene des Setups
//----------------------------------------------------------------------------
//Funktion für die Darstellung des Menüs auf dem Display
void drawMenu()
{
if(completesetup == 0)
{
//SETUP SEITE 1
if (setuppage == 0) {
lcd.backlight();
lcd.setCursor(0, 0);
lcd.print("SETUP (1/5)");
lcd.setCursor(0, 1);
lcd.print("Abstandssensor bitte");
lcd.setCursor(0, 2);
lcd.print("auf Spule ablegen.");
lcd.setCursor(0, 3);
lcd.write(byte(4));
lcd.print("Weiter");
}
//SETUP SEITE 2
if (setuppage == 1) {
lcd.setCursor(0, 0);
lcd.print("SETUP (2/5)");
lcd.setCursor(0, 1);
lcd.print("Sensor bitte auf");
lcd.setCursor(0, 2);
lcd.print("max. Hoehe halten.");
lcd.setCursor(0, 3);
lcd.write(byte(4));
lcd.print("Weiter");
}
//SETUP SEITE 2
if (setuppage == 2) {
lcd.setCursor(0, 0);
lcd.print("SETUP (3/5)");
lcd.setCursor(0, 1);
lcd.print("SPULENBREITE - Insel");
lcd.setCursor(0, 2);
lcd.print("an 1. Grenze fahren.");
lcd.setCursor(0, 3);
lcd.write(byte(4));
lcd.print("Weiter");
}
//SETUP SEITE 2
if (setuppage == 3) {
lcd.setCursor(0, 0);
lcd.print("SETUP (4/5)");
lcd.setCursor(0, 1);
lcd.print("SPULENBREITE - Insel");
lcd.setCursor(0, 2);
lcd.print("an 2. Grenze fahren.");
lcd.setCursor(0, 3);
lcd.write(byte(4));
lcd.print("Weiter");
}
//SETUP SEITE 5
if (setuppage == 4) {
lcd.setCursor(0, 0);
lcd.print("SETUP (5/5)");
lcd.setCursor(0, 1);
lcd.print("Filamentdurchmesser:");
lcd.setCursor(0, 2);
lcd.print(arrFilamentdiameter[currentIndex]);
lcd.print(" mm");
lcd.setCursor(0, 3);
lcd.write(byte(4)); // Pfeilsymbol oder Weiter-Button
lcd.print("Weiter");
}
///FEHLERMELDUNG 1
if (setuppage == 10)
{
lcd.setCursor(0, 0);
lcd.print("Maximale Fuellhoehe");
lcd.setCursor(0, 1);
lcd.print("<= Spulendurchmesser");
lcd.setCursor(0, 2);
lcd.print("(Fehlercode: 1)");
lcd.setCursor(0, 3);
lcd.write(byte(4));
lcd.print("Weiter");
}
}
//STATUSANZEIGE
if (completesetup == 1)
{
//lcd.backlight();
lcd.setCursor(3, 0);
lcd.print("STATUSANZEIGE");
lcd.setCursor(0, 2);
lcd.print("Drehgeschw:");
lcd.print(rotspeed_reel);
lcd.print("mm/s");
lcd.setCursor(0, 3);
lcd.print("Fuellstand:");
lcd.print(process);
lcd.print("% ");
}
}
/*
///////////////////////////////////////////////////////////////////////////////////////////
######## ## ## ###### ####### ######## ######## ########
## ### ## ## ## ## ## ## ## ## ## ##
## #### ## ## ## ## ## ## ## ## ##
###### ## ## ## ## ## ## ## ## ###### ########
## ## #### ## ## ## ## ## ## ## ##
## ## ### ## ## ## ## ## ## ## ## ##
######## ## ## ###### ####### ######## ######## ## ##
///////////////////////////////////////////////////////////////////////////////////////////
*/
//----------------------------------------------------------------------------
//Variablen zum Abspeichern der Inselgrenzen
int limit;
int firstlimit;
int secondlimit;
//Variablen für die Verarbeitung der Eingaben durch den Drehencoder
boolean up = false;
boolean down = false;
boolean middle = false;
bool switchState_encoder; // Variable für den aktuellen Status des Encoder-Tasters
bool prevSwitchState_encoder; //Variablen für den vorherigen Status des Encoder-Tasters
//Funktion zum Auslesen der Drehrichtung vom Encoder
//Definition der Drehrichtung grafisch dargestellt unter folgenden Link:
//https://docs.wokwi.com/parts/wokwi-ky-040
bool lastClk = HIGH;
//----------------------------------------------------------------------------
void readRotaryEncoder()
{
bool newClk = digitalRead(ENCODER_CLK);
if (newClk != lastClk) {
//Eine Veränderung am CLK-Pin wurde wahrgenommen
lastClk = newClk;
bool dtValue = digitalRead(ENCODER_DT);
if (newClk == LOW && dtValue == HIGH) {
//Serial.println("Im Uhrzeigersinn ⏩");
down = true;
}
if (newClk == LOW && dtValue == LOW) {
//Serial.println("Gegen Uhrzeigersinn ⏪");
up = true;
}
//Die Veränderung soll in der Methode "encoderChanged" überprüft werden.
encoderChanged();
}
switchState_encoder = digitalRead(ENCODER_BTN); //Auslesen des Encoder-Buttons
/*Ist eine Veränderung des Encoder-Buttons wahrzunehmen, dann soll beim Betätigen (PullUp, deswegen LOW)
die dazugehörige Variable auf "true" (1) gesetzt und der Displayinhalt beim Öffnen eines neuen Fensters
gelöscht werden. Damit diese Funktion wiederholt funktioniert, wird die Variable für die aktuelle Seite
auf die Variable für die vorherige Seite gesetzt. */
if (switchState_encoder != prevSwitchState_encoder) {
if (switchState_encoder==LOW) {
middle = true;
if (completesetup == 0) {
lcd.clear();
}
}
}
prevSwitchState_encoder = switchState_encoder; //Zwischenspeichern des aktuellen Zustandes
/*
///////////////////////////////////////////////////////////////////////////////////////////
#### ## ## #### ########
## ### ## ## ##
## #### ## ## ##
## ## ## ## ## ##
## ## #### ## ##
## ## ### ## ##
#### ## ## #### ##
///////////////////////////////////////////////////////////////////////////////////////////
*/
if (middle) //Wenn der Encoder-Button gedrückt wurde:
{
middle = false; //aktueller Wert, nachdem der Taster gedrückt wurde (true) wird auf Ausgangswert (false) zurückgesetzt
if (completesetup == 0)
{
switch(setuppage)
{
case 0: //Festlegen der maximalen Messdistanz (Tastrolle liegt auf Auflagefläche der Spule)
measuring();
distance = curdistance;
diameter_reel = (maxdistance - distance)*2;
//Serial.println(diameter_reel);
setuppage++;
break;
case 1: //Festlegen der minimalen Messdistanz (Tastrolle wird auf die Höhe gehalten, auf der die Spule als befüllt erkannt werden soll)
measuring();
distancefullreel = curdistance;
setuppage++;
/*Nur wenn die gemessene Distanz der vollen Spule kleiner als die gemessene
Distanz der leeren Spule ist, dann soll das Setup beendet werden können.
Andererseite soll eine Fehlermeldung erscheinen, und das Setup zum Ermitteln
der Abstände neugestartet werden*/
if (distancefullreel >= distance)
{
setuppage = 10;
}
break;
case 2: //Erstes Limit für den Fahrbereich des Schlittens wird festgelegt
setuppage++;
firstlimit = limit;
//Serial.print("firstlimit: ");
//Serial.println(firstlimit);
break;
case 3: //Zweites Limit für den Fahrbereich des Schlittens wird festgelegt
secondlimit = limit;
//Serial.print("secondlimit: ");
//Serial.println(secondlimit);
stepsperfullwind = abs(secondlimit - firstlimit);
//Serial.print("stepsperfullwind: ");
//Serial.println(stepsperfullwind);
//Zurückfahren in die Startposition
if(secondlimit > firstlimit)
{
for(int i1 = 0; i1 < stepsperfullwind; i1++)
{
//Serial.println(i1);
digitalWrite(stepperDriver1_Dir, 0);
stepper1state = !stepper1state;
digitalWrite(stepperDriver1_Step, stepper1state);
delay(1); //Wird benötigt, damit der Schrittmotor nicht zu schnell in seine Ursprungsposition zurückfährt
stepper1state = !stepper1state;
digitalWrite(stepperDriver1_Step, stepper1state);
//Serial.println("Steps");
}
}
setuppage++;
break;
case 4:
completesetup=1;
setuppage = 0;
case 10: //Fehlercode
setuppage = 0;
break;
}
}
else if(breakval != 1 && completesetup == 1)
{
breakval = 1;
}
else
{
breakval = 2;
}
//Serial.println(breakval);
drawMenu(); //Ausgabe der Verarbeitung auf dem Display
}
}
//Wurde eine Drehung wahrgenommen, dann soll über die Funktion
//eine Veränderung vorgenommen werden.
void encoderChanged()
{
//Gegen Uhrzeigersinn
if(up)
{
up = false;
if(setuppage == 4){
if(currentIndex > 0){
currentIndex--;
}
}
if(setuppage == 2 || setuppage == 3){
//Schrittmotorenansteuerung
digitalWrite(stepperDriver1_Dir, 0);
for(int i2 = 0; i2 < 8; i2++)
{
limit--;
//Schrittmotor wird einen vollen Schritt gegen den Uhrzeigersinn bewegt
stepper1state = !stepper1state;
digitalWrite(stepperDriver1_Step, stepper1state);
stepper1state = !stepper1state;
digitalWrite(stepperDriver1_Step, stepper1state);
delay(1);
}
}
//Dekrementiert die Drehgeschwindigkeit
if(completesetup == 1 && rotspeed_reel > 0){
rotspeed_reel -= 5;
}
}
//Im Uhrzeigersinn
if(down)
{
down = false;
if(setuppage == 4) {
if(currentIndex < 2){
currentIndex++;
}
}
if(setuppage == 2 || setuppage == 3){
//Schrittmotorenansteuerung
digitalWrite(stepperDriver1_Dir, 1);
for(int i3 = 0; i3 < 8; i3++)
{
limit++;
//Schrittmotor wird einen vollen Schritt gegen den Uhrzeigersinn bewegt
stepper1state = !stepper1state;
digitalWrite(stepperDriver1_Step, stepper1state);
stepper1state = !stepper1state;
digitalWrite(stepperDriver1_Step, stepper1state);
delay(1);
}
}
//Inkrementiert die Drehgeschwindigkeit
if(completesetup == 1 && rotspeed_reel < 50){
rotspeed_reel += 5;
}
}
drawMenu();
}