#define MIT_PID 1
//ohne Regelung - > die Zeile die beiden // entfernen
//#undef MI_TPID
#include "LiquidCrystal_I2C.h"
#include "TEST__.h"
#include "PID.h"
// PID Klasse initialisieren
PID pid = PID();
// Rotary Encoder Anschlüsse
// pin CLK muss ein interruptfähiger Pin sein
// pin SW muss ein interruptfähiger Pin sein
#define DT_Pot 19
#define CLK_Pot 18
#define SW_Pot 2
// Rotary - Voreinstellungen default Werte
volatile uint8_t lastvalueCLK;
// Motor Pin's ENA ENB ENA
#define P1A 10
#define P2A 5
#define P3A 6
int p1a = P1A;
int p2a = P2A;
int p3a = P3A;
// L298N Motorsteuerung - IN1 IN3 IN1
#define P11 7
#define P21 9
#define P31 3
int p11 = P11;
int p21 = P21;
int p31 = P31;
// Hallsensoren Blau, Weiß und Grün
#define MHB 50
#define MHW 51
#define MHG 52
int mhb = MHB;
int mhw = MHW;
int mhg = MHG;
#define K_SPEED 55
volatile int istSpeed = K_SPEED;
volatile int sollSpeed = K_SPEED;
bool motorStoped = false; // läuft der Motor ?
int lastSpeed = K_SPEED - 3;
int lastSollSpeed = K_SPEED - 3;
bool motorVor = true; // Drehrichtung true - vor false - zurück
bool takt1 = false, takt2 = false; // Berechnung der Umdrehungen
unsigned int takt = 0;
unsigned long t1Drehzahl = 0;
unsigned long umdrehung = 0;
int sollUmin = 0;
int istUmin = 0;
int lastistUmin = -1;
volatile bool sollSpeedChanged = false;
LiquidCrystal_I2C lcd(0x27, 16, 2); // lcd initialisieren - Adressezuweisen
/*
setup für das LCD Display
*/
void setupDisplay() {
lcd.init(); // initialize the lcd
lcd.clear();
lcd.backlight();
lcd.setCursor(0, 0);
lcd.print(" IST-U/min: ");
lcd.setCursor(0, 1);
lcd.print("SOLL-U/min: ");
}
/*
auf dem LCD Display die Drehzahl ausdrucken
*/
void showDrehzahl() {
char s1[8];
if (lastSpeed != istSpeed || lastistUmin != istUmin) {
if (motorStoped == true)
sprintf(s1, "%5d", 0);
else {
lastSpeed = istSpeed;
lastistUmin = istUmin;
sprintf(s1, "%5d", istUmin);
}
lcd.setCursor(11, 0);
lcd.print(s1);
}
if (sollSpeedChanged) {
sollSpeedChanged = false;
lcd.setCursor(11, 1);
sprintf(s1, "%5d", sollUmin);
lcd.print(s1);
}
}
/*
Setup der 2 L298N Motorsteuerungsbausteine
*/
void setupBridges() {
pinMode(mhb, INPUT_PULLUP);
pinMode(mhw, INPUT_PULLUP);
pinMode(mhg, INPUT_PULLUP);
Serial.begin(115200);
pinMode(p11, OUTPUT);
pinMode(p21, OUTPUT);
pinMode(p31, OUTPUT);
pinMode(p1a, OUTPUT);
pinMode(p2a, OUTPUT);
pinMode(p3a, OUTPUT);
digitalWrite(p1a, HIGH);
digitalWrite(p2a, LOW);
digitalWrite(p3a, LOW);
digitalWrite(p11, LOW);
digitalWrite(p21, LOW);
digitalWrite(p31, LOW);
TCCR1A = 0b10100010; // Configure TCCR1A for PWM on Pin 9
TCCR1B = 0b00011001; // Configure TCCR1B for PWM on Pin 9
OCR1A = 32768; // Set the compare value for Channel A
OCR1B = 32768; // Set the compare value for Channel A
OCR1C = 32768; // Set the compare value for Channel A
// Drehrichtung des Motors
motorVor = true;
}
/* Setup für den Rotary Encoder
Pinmode
Interrupt Funktion zuweisen
*/
void setupRotary() {
pinMode(CLK_Pot, INPUT);
pinMode(DT_Pot, INPUT);
pinMode(SW_Pot, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CLK_Pot), interruptDT, CHANGE);
attachInterrupt(digitalPinToInterrupt(SW_Pot), interruptSW, CHANGE);
lastvalueCLK = digitalRead(CLK_Pot);
}
/*
Motor ausschalten
*/
void motorAus() {
motorStoped = true;
lastSpeed = 0;
t1Drehzahl = 0;
digitalWrite(p1a, LOW);
digitalWrite(p2a, LOW);
digitalWrite(p3a, LOW);
digitalWrite(p11, LOW);
digitalWrite(p21, LOW);
digitalWrite(p31, LOW);
}
/*
Motor einschalten
*/
void motorEin() {
// Motor starten
motorStoped = false;
takt1 = false;
takt2 = false;
t1Drehzahl = 0;
umdrehung = 0;
#ifdef MIT_PID
pid.resetN();
#endif
}
/* Interrupt Funktion, die bei einem Interrupt ausgeführt wird
auslösen des Interrupt -> drehen am KY-040 Rotary Encoder - Änderung des Wertes am Pin DT_Pot
-> dient zur Steuerung
*/
void interruptDT() {
noInterrupts();
uint8_t valueCLK = digitalRead(CLK_Pot);
if (valueCLK != lastvalueCLK) {
long sp = abs(sollSpeed);
if (!motorVor)
sp = -sp;
if (digitalRead(DT_Pot) != valueCLK)
sp--;
else
sp++;
istSpeed = constrain(abs(sp), 0, 255);
if (motorStoped && istSpeed >= 50) {
motorStoped = false;
istSpeed = K_SPEED;
}
else if (istSpeed < 50 && istSpeed > 35)
{
if (!motorStoped) {
motorAus();
istSpeed = 35;
sollUmin = 0;
}
}
else if (istSpeed < 20)
{
motorVor = !motorVor;
motorEin();
istSpeed = K_SPEED;
sollUmin = 0;
}
sollSpeed = istSpeed;
}
lastvalueCLK = valueCLK;
interrupts();
}
/* Interupt Funktion, die bei einem Interrupt ausgeführt wird
auslösen des Interrupt -> Clicken am KY-040 Rotary Encoder - Änderung des Wertes am Pin SW_Pot
schaltet den Motor aus oder ein
*/
void interruptSW() {
int valueSW = digitalRead(SW_Pot);
if (valueSW) {
if (motorStoped == true) {
// Motor starten
motorEin();
} else {
// Motor stopen
motorAus();
}
}
}
/*
setup routinedes Arduinos
hier werden die verschiedenen Baustein initialisiert
*/
void setup() {
Serial.begin(115200);
setupRotary();
setupBridges();
setupDisplay();
#ifdef MIT_PID
pid.setDefaultK(1.0, 0.4, 0.5);
#endif
}
/*
wartet bis der nächste HAll Schritt erreicht ist
*/
void waitNextStep(int h1, int h2, int h3) {
while (h1 == digitalRead(mhb) && h2 == digitalRead(mhw) && h3 == digitalRead(mhg))
;
}
#ifdef __TEST
/*
ist nur zu testzwecken aktiv
- kann in der Datei Test__.h aktiviert/deaktiviert werden
*/
void printSchritt(int step, int h1, int h2, int h3) {
int ta = 8;
// Hallsensoren lesen
if (h1 == 1) // H1
ta += 4;
if (h2 == 1) // H2
ta += 2;
if (h3 == 1) // H3
ta += 1;
Serial.print("Schritt ");
Serial.print(step);
Serial.print(" takt = ");
Serial.print(ta);
Serial.print(" Hall 1 2 3: ");
Serial.print(h1);
Serial.print(h2);
Serial.println(h3);
}
#endif
/*
ermittelt die aktuelle Drehzahl
*/
void countDrehzahl() {
unsigned long t2;
if (t1Drehzahl == 0)
{
t1Drehzahl = millis();
t2 = t1Drehzahl + 1000;
umdrehung = 0;
}
// takt: 2, 3, 1, 5, 4, 6 - Vorwärts
// takt: 12, 13, 9, 11, 10, 14 - Rückwärts
switch (takt) {
case 1:
case 9:
takt1 = true;
break;
case 2:
case 12:
takt2 = true;
break;
default: return;
}
if (takt1 == false || takt2 == false)
return;
takt1 = false;
takt2 = false;
umdrehung++;
#define UMDREH 100
if (umdrehung == UMDREH || millis() > t2) {
float t;
// 1 Umdrehung hat 5 * 6 Phasen
umdrehung /= 5;
t = ((millis() - t1Drehzahl) / 1000.0);
t = round((float(umdrehung) / t) * 6) * 10.0;
istUmin = int(t);
takt1 = false;
takt2 = false;
t1Drehzahl = 0;
umdrehung = 0;
if (lastSollSpeed != sollSpeed) {
#ifdef MIT_PID
pid.resetN();
#endif
if (istUmin < 350)
return;
sollSpeedChanged = true;
lastSollSpeed = sollSpeed;
sollUmin = istUmin;
}
#ifdef MIT_PID
istSpeed = pid.getNewSpeed(istSpeed, sollUmin, istUmin);
#endif
}
}
/*
die Hauptfunktion des Programmes
hier wird das Drehen des Motors gesteuert
*/
void motorDreht(void) {
// Ermittlung des Taktes der ausgeführt wird
// Anfangswert - takt = 0 -> im Uhrzeigersinn rechts Lauf
// takt = 8 -> im Uhrzeigersinn links Lauf
if (motorVor)
takt = 0;
else
takt = 8;
// Hallsensoren lesen
int h1 = digitalRead(mhb);
int h2 = digitalRead(mhw);
int h3 = digitalRead(mhg);
// Werte in die Variable takt schreiben
if (h1 == 1) // H1
takt += 4;
if (h2 == 1) // H2
takt += 2;
if (h3 == 1) // H3
takt += 1;
// takt: < 8 - Vorwärts
// takt: > 8 - Rückwärts
switch (takt) {
case 5: // Schritt 4 Sensor (CBA) 010 vor
case 12: // Schritt 1 Sensor (CBA) 101 zurück
#ifdef __TEST
if (takt == 5)
printSchritt(4, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
else
printSchritt(1, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
#endif
analogWrite(p21, istSpeed);
digitalWrite(p11, LOW);
digitalWrite(p31, LOW);
digitalWrite(p1a, HIGH);
digitalWrite(p2a, HIGH);
digitalWrite(p3a, LOW);
waitNextStep(h1, h2, h3);
digitalWrite(p21, LOW);
digitalWrite(p1a, LOW);
digitalWrite(p2a, LOW);
break;
case 1: // Schritt 3 Sensor (CBA) 110 vor
case 13: // Schritt 6 Sensor (CBA) 001 zurück
#ifdef __TEST
if (takt == 1)
printSchritt(3, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
else
printSchritt(6, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
#endif
analogWrite(p31, istSpeed);
digitalWrite(p11, LOW);
digitalWrite(p21, LOW);
digitalWrite(p3a, HIGH);
digitalWrite(p1a, HIGH);
digitalWrite(p2a, LOW);
waitNextStep(h1, h2, h3);
digitalWrite(p31, LOW);
digitalWrite(p3a, LOW);
digitalWrite(p1a, LOW);
break;
case 3: // Schritt 2 Sensor (CBA) 100 vor
case 9: // Schritt 5 Sensor (CBA) 011 zurück
#ifdef __TEST
if (takt == 3)
printSchritt(2, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
else
printSchritt(5, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
#endif
analogWrite(p31, istSpeed);
digitalWrite(p11, LOW);
digitalWrite(p21, LOW);
digitalWrite(p3a, HIGH);
digitalWrite(p2a, HIGH);
digitalWrite(p1a, LOW);
waitNextStep(h1, h2, h3);
digitalWrite(p31, LOW);
digitalWrite(p3a, LOW);
digitalWrite(p2a, LOW);
break;
case 2: // Schritt 1 Sensor (CBA) 101 vor
case 11: // Schritt 4 Sensor (CBA) 010 zurück
#ifdef __TEST
if (takt == 2)
printSchritt(1, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
else
printSchritt(4, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
#endif
analogWrite(p11, istSpeed);
digitalWrite(p21, LOW);
digitalWrite(p31, LOW);
digitalWrite(p1a, HIGH);
digitalWrite(p2a, HIGH);
digitalWrite(p3a, LOW);
waitNextStep(h1, h2, h3);
digitalWrite(p11, LOW);
digitalWrite(p1a, LOW);
digitalWrite(p2a, LOW);
break;
case 6: // Schritt 6 Sensor (CBA) 001 vor
case 10: // Schritt 3 Sensor (CBA) 110 zurück
#ifdef __TEST
if (takt == 6)
printSchritt(6, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
else
printSchritt(3, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
#endif
analogWrite(p11, istSpeed);
digitalWrite(p21, LOW);
digitalWrite(p31, LOW);
digitalWrite(p1a, HIGH);
digitalWrite(p3a, HIGH);
digitalWrite(p2a, LOW);
waitNextStep(h1, h2, h3);
digitalWrite(p11, LOW);
digitalWrite(p1a, LOW);
digitalWrite(p3a, LOW);
break;
case 4: // Schritt 5 Sensor (CBA) 011 vor
case 14: // Schritt 2 Sensor (CBA) 100 zurück
#ifdef __TEST
if (takt == 4)
printSchritt(5, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
else
printSchritt(2, digitalRead(mhb), digitalRead(mhw), digitalRead(mhg));
#endif
analogWrite(p21, istSpeed);
digitalWrite(p11, LOW);
digitalWrite(p31, LOW);
digitalWrite(p2a, HIGH);
digitalWrite(p3a, HIGH);
digitalWrite(p1a, LOW);
waitNextStep(h1, h2, h3);
digitalWrite(p21, LOW);
digitalWrite(p2a, LOW);
digitalWrite(p3a, LOW);
break;
}
}
/*
Hauptloop - hier wird die Arbeit gemacht
- Drehzahl anzeigen
- Moto steuern
- Drehzahl ermitteln
*/
void loop() {
// Serial.print(" sollSpeed = ");
// Serial.print(sollSpeed);
// Serial.print(" motorStoped = ");
// Serial.print(motorStoped);
// Serial.print(" motorVor = ");
// Serial.println(motorVor);
showDrehzahl();
if (!motorStoped) {
motorDreht();
countDrehzahl();
}
}