1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
| /*Aansturing van het programma
De H-Brug zit aangesloten op D11+D12 deze is voor de richting links/rechts, De PWM is aan gesloten op D10 voor de gewenste snelheid
De groene led voor max en gele led voor "gas staat open" zitten respectieveleijk op d7 en d6. Ledjes voor links en rechtsom
zitten op 8 en 9.
Verder is er een drukknop aangesloten op D5 voor het omschakelen van links naar rechts en andersom.
A1 wordt gebruikt door de potmeter en verder zit de lcd scherm op A4 en A5 aangesloten.
Het inlezen van de encoder gebeurt via de kanalen aangesloten op D2 en D3
*/
//Haal alle benodigde biliotheken op
#include <Wire.h>
#include <Arduino.h>
#include <SPI.h>
#include <LiquidCrystal_I2C.h>
// LCD scherm declaratie
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27,16, 2);
//Declareer alle gebruikte pins
//Inputs
const int KNOP = 5; //ingang voor de knop om de richting te veranderen van links naar rechts of andersom
//Interupt inputs
const int pEncChAIn = 2; //hardware interupt puls signaal encoder
const int pEncChBIn = 3; //hardware interupt richting signaal encoder
//Outputs
const int MotorLinks = 12; //uitgang voor aansturing motor linksom
const int MotorRechts = 11; //uitgang voor aansturing motor rechtsom
const int MotorSnelheid = 10; //uitgang voor snelheid (PWM)van de motor
const int ledgeelstart = 6; //uitgang voor de gele led, deze geeft gas open aan
const int ledgroenmax = 7; //uitgang voor de groene led, geeft aan dat maximale snelheid bereikt is
const int ledroodlinks = 8; //uitgang voor de rode led links draaiend
const int ledroodrechts = 9; //uitgang voor van de rode led rechts draaiend
const int potmetervoeding =13;
//Declareer alle gebruikte variabelen
//Longs
long intTime = 0; //main timer van het programma
long intLastTimeCalc = 0;
long intLastTimePi = 0; //tijd van de laatste pi berekening
long intLastTimeKnop = 0;
long intPWMOutputValue = 0; //pwm output waarde (gecorigeerd nav pi regelaart)
//Floats
float intPulsesPerSecond = 0; //berekend aantal pulsen per seconde
float intPfactor = 0.01; //factor van het proportionele deel van de regelaar
float intIfactor = 0.08; //factor voor de integrator van de regelaar
float intPWMPControl = 0; //variabele om gain in op te slaan
float intPWMIControl = 0; //variabele om integrator in op te slaan
float intError = 0; //totale afwijking
float intIError = 0; //ierror
//Integers
int intTeller_motor = 50; //teller voor aansturen motor
int intTeller_knop = 2000; //minimale tussentijd voor reactiveren knop voor richting
int potmeter = 0; //inlezen van de analoge waarde van de potmeter
int Error = 0; //error
int Mrichting=0; //richtingg voor de motor
int knop = 0; //knop voor het aanzetten
int setknop = 0; //voor het inlezen van de knop voor de richting
int intPulseCounter = 0; //teller voor het aantal pulsen tussen 2 samples
int intPulseTimeToSpeedFactor = 10; //factor om op rpm uit te komen
int intIntervalTimeSpeed = 100; //snelheid sample tijd
int intActueleSnelheid = 0; //actuele binnengelezen snelheid
int intIntervalTimePi = 200; //pi regelaar sample tijd
int intWensSnelheid = 0; //gewenste snelheid
//Booleans
bool mMotorDirection = false; //bitje voor motor richting
void setup()
{
//Alle pins als input, output of interupt declareren
pinMode(MotorLinks, OUTPUT);
pinMode(MotorRechts,OUTPUT);
pinMode(MotorSnelheid,OUTPUT);
attachInterrupt(digitalPinToInterrupt(pEncChAIn), EncChanADetect, RISING);
attachInterrupt(digitalPinToInterrupt(pEncChBIn), EncChanBDetect, RISING);
pinMode(KNOP,INPUT_PULLUP);
pinMode(ledgeelstart,OUTPUT);
pinMode(ledgroenmax,OUTPUT);
pinMode(ledroodlinks, OUTPUT);
pinMode(ledroodrechts, OUTPUT);
pinMode(potmetervoeding, OUTPUT);
//Schrijf pins naar de gewenste startwaardes
Mrichting = 0;
digitalWrite(MotorRechts, HIGH);
digitalWrite(MotorLinks, LOW);
digitalWrite(ledgeelstart,HIGH);
digitalWrite(ledgroenmax,LOW);
// Open seriele communicatie met het scherm met een snelheid van 9600 bits per second:
Serial.begin(9600);
lcd.init();
lcd.backlight();
lcd.begin(16,2);
}
void loop() {
potmeter = analogRead(A1);
if (potmeter<=100){potmeter=0;
}
// int pwmOutput = map(potmeter, 0, 1023, 0 , 255); // Map the potentiometer value from 0 to 255
// analogWrite(MotorSnelheid, pwmOutput); // Send PWM signal to L298N Enable pin
intTime=millis(); //main timer van het programma
knop = digitalRead(KNOP); //status van de richtingknop in de gaten houden
digitalWrite(potmetervoeding,HIGH);
//Print de gewenste tekst op het HMI
lcd.setCursor(0,0); //begin op eerste digit eerste regel
///lcd.print("speedwish = " ); //print de tekst speed wens = op scherm
///lcd.print(intWensSnelheid); //schrijf de wenswaarde naar het scherm
//lcd.print("intEr = ");
//lcd.print(intError);
lcd.print("LastTPi = ");
lcd.print(intLastTimePi);
lcd.setCursor(0,1);
//lcd.print("intIEr = ");
//lcd.print(intIError);
lcd.print("PWMIC = ");
lcd.print(intPWMIControl);
//tweede regel eerste digit, komt de volgende info te staan
///lcd.print("speedact = "); //print de tekst speed actual = op scherm
// /lcd.print(intActueleSnelheid);
//Bereken de actuele snelheid van de motor aan de hand van het encoder ingangssignaal (dit gebeurt elke 100ms)
if ((intTime - intLastTimeCalc) >= intIntervalTimeSpeed) {
intPulsesPerSecond = (intPulseTimeToSpeedFactor * intPulseCounter); //vermenigvuldig het aantal getelde pulsen met 10 om het aantal pulsen per seconde te berekenen
intActueleSnelheid = ((intPulsesPerSecond / 440) * 60); //deel dit door het aantal pulsen per omwenteling en doe keer 60 om op de snelheid in rpm te komen
intPulseCounter = 0; //reset de pulscounter voor de volgende 100miliseconden
intLastTimeCalc = intTime; //sla de huidige tijd op zodat na 100ms weer opnieuw kan worden berekend hoeveel pulsen er verstreken zijn
}
//Bereken de gewenste motorsnelheid aan de hand van de potmeter
potmeter = analogRead(A1); //opslaan van de analoge waarde in de variabele "potmeter"
intWensSnelheid = (potmeter*0.1953125); //Bereken gewenste snelheid (200rpm / 1024 (analoge input)
if (intWensSnelheid<=10) { //kleiner dan 10?
digitalWrite(ledgeelstart,LOW); //gele led uit
}
if (intWensSnelheid > 10){ //groter dan 10?
digitalWrite(ledgeelstart,HIGH); //gele led aan
}
//Bereken de PI control waardes elke 200 miliseconden
if ((intTime - intLastTimePi) >= intIntervalTimePi) {
intError = intWensSnelheid - intActueleSnelheid; //Bereken de afwijking door de actuele snelheid van de gewenste snelheid af te trekken
intPWMPControl = intPfactor * intError; //laat een factor op deze error los om de gain van de regelaar te berekenen
intIError = intIError + intError; //bereken de integraal aan de hand van de fout
intPWMIControl = intIfactor * intIError; //bereken de benodigde I waarde voor correctie van de snelheid
intPWMOutputValue = intPWMPControl + intPWMIControl; //bereken de benodigde PWM waarde om de motor op de juiste snelheid te krijgen
if (intPWMOutputValue > 255) intPWMOutputValue = 255; //set PWM op 255 als hoger dan 255
if (intPWMOutputValue < 0) intPWMOutputValue = 0; //set PWM op 0 wanneer lager dan 0
intLastTimePi = intTime; //sla de huidige tijd op zodat over 200ms weer kan worden berekend wat de PWM moet zijn
}
analogWrite(MotorSnelheid,intWensSnelheid); //nieuwe snelheid voor de motor wegschrijven
//analogWrite(MotorSnelheid,intWensSnelheid); //nieuwe snelheid voor de motor wegschrijven (gebruikt om te testen!)
if (knop==0 && setknop==0) { //als op de knop gedrukt set memory bit en set timer
setknop=1; //memory bit = 1
intLastTimeKnop = intTime; //actuele tijd opslaan
}
if (((intTime-intLastTimeKnop)>=intTeller_knop)&&(setknop==1)&&(Mrichting == 1)) { //als er 2 seconden verstreken is wissel van richting // kijk wat de richting was en verander deze
Mrichting = 0;
setknop=0; //memory bit = 0
}
if (((intTime-intLastTimeKnop)>=intTeller_knop)&&(setknop==1)&&(Mrichting == 0)) { //als er 2 seconden verstreken is wissel van richting // kijk wat de richting was en verander deze
Mrichting = 1;
setknop=0; //memory bit = 0
}
if (Mrichting==0 && setknop==0){
digitalWrite(MotorRechts, HIGH); //rechtsom = hoog
digitalWrite(MotorLinks, LOW); //linksom = laag
}
if (Mrichting==1 && setknop==0){
digitalWrite(MotorRechts, LOW); //rechtsom = laag
digitalWrite(MotorLinks, HIGH); //linksom = hoog
}
if (setknop==1){
digitalWrite(MotorRechts, LOW); //rechtsom = laag
digitalWrite(MotorLinks, LOW); //linksom = laag
}
//led voor rechts draaien aan
if (MotorRechts >= HIGH){
digitalWrite(ledroodrechts,HIGH);
}
//led voor links draaien aan
if (MotorLinks >= HIGH){
digitalWrite(ledroodlinks,HIGH);
}
//Led voor snel draaien aan
if (intActueleSnelheid>250) {
digitalWrite(ledgroenmax,HIGH); //led indicatie dat de motor maximaal draait
}
//Led voor snel draaien uit
if (intActueleSnelheid<=250){
digitalWrite(ledgroenmax,LOW); //led indicatie dat de motor niet maximaal draait
}
}
void EncChanADetect() {
intPulseCounter++; //Hardware interupt omdat encoder kanaal A hoog is
}
void EncChanBDetect() {
mMotorDirection = digitalRead(pEncChAIn); //Hardware interupt omdat encoder kanaal B hoog is
} |