23-11-2020, 14:43
Hallo,
Ich habe die Drehzahlregelung für die Autos nochmal upgedated, V 1.5. Die Schaltung wird jetzt mit 15V betrieben, dazu habe ich die Zenerdioden D5,D6,D7,D8 durch 8,2 Volt Typen ersetzt. Außerdem wurden die Widerstände R2, R4, R5 und R8 durch 150 Ohm ersetzt.
Ich habe eine Funktionsbeschreibung beigefügt und einen Sketch geschrieben, bei dem ein PID Regler die PWM regelt.
Have fun!
Ich habe die Drehzahlregelung für die Autos nochmal upgedated, V 1.5. Die Schaltung wird jetzt mit 15V betrieben, dazu habe ich die Zenerdioden D5,D6,D7,D8 durch 8,2 Volt Typen ersetzt. Außerdem wurden die Widerstände R2, R4, R5 und R8 durch 150 Ohm ersetzt.
Ich habe eine Funktionsbeschreibung beigefügt und einen Sketch geschrieben, bei dem ein PID Regler die PWM regelt.
Have fun!
Code:
/* AMS controller myPID Control V0.8.7.ino
This sketch controls rpm of 2 x 2 Faller AMS cars by measuring motor rpm via back EMF using circuit diagram AMS control with feedback V 1.5.
Before starting:
- double check direction of zenerdiodes and measurement circuit to avoid supply voltage on Arduino input!
- use stabilized power supply!
- add heatsink to L293!
- check each car is using diode
- checked with and optimized for Faller zinc, LKW-Flachanker and PKW-Flachanker
*** Version 0.8.7 preliminary ***
(c) Rainer Woerthmann
-> No warranty
-> Have fun
================================= [include libraries ] ==========================================*/
#include <PID_v1.h>
/*
================================= [global variables ] ===========================================*/
// input pin definition
const byte bemf1pin = A0; // Back-EMF input car 1 track A
const byte bemf2pin = A1; // Back-EMF input car 2 track A
const byte bemf3pin = A2; // Back-EMF input car 3 track B
const byte bemf4pin = A3; // Back-EMF input car 4 track B
const byte mx0 = 2; // to multiplexer 4051 pin 11, LSB
const byte mx1 = 3; // to multiplexer 4051 pin 10, HSB
const byte analogPin =A5; // setpoint readings via multiplexer 4051, cars 1...4
// output pin definition H-bridge 1, track A (L293 V1 und V4)
const byte L293V1pin = 11; // track A, car 1, L293 pin 2, PWM
const byte L293V4pin = 6; // track A, car 2, L293 Pin 15, PWM
// output pin definition H-bridge 2, track B, (L293 V2 and V3)
const byte L293V2pin = 10; // track B, car 3, L293 pin 7, PWM
const byte L293V3pin = 9; // track B, car 4, L293 pin 10, PWM
// output pin definition H-bridge, high state
const byte L293V12ena = 7; // car 1+2 enable, L293 Pin 1
const byte L293V34ena = 8; // car 3+4 enable, L293 Pin 9
// variables
double setpoint[4]; // setpoints cars 1...4
double rpm[4]; // rpm cars 1...4
double pwm[4]; // output voltage cars 1...4
double data1,data2; // bemf readings
const double carspeed[4] = {1,1,1,1};
int ref[4]; // reference values cars 1...4, standstill
byte carenable[4] = {0,0,0,0};
word i; // counter
const word samples = 20; // number of bemf-readings: manipulates overall car speeds and system frequency, original set to 20
const int pulsetime = 6; // frequency (around (2*(6ms+measurement))
// examples: zinc=1, truck Flachanker = 3.5, PKW Flachanker = 4...8
// myPID control parameters
PID myPID1(&rpm[0], &pwm[0], &setpoint[0],0.2,2,0.0001, DIRECT); // values of PID-controller to play with; first P-value, second I-value, third D-value
PID myPID2(&rpm[1], &pwm[1], &setpoint[1],0.1,1,0.0008, DIRECT); // f.e. zinc motor: P=0.2,I=1,D=0.0001 by 15V supply, pulsetime 6ms
PID myPID3(&rpm[2], &pwm[2], &setpoint[2],0.18,3,0.0008, DIRECT); // f.e. Truck Flachanker P=0.2,I=2,D=0.0006
PID myPID4(&rpm[3], &pwm[3], &setpoint[3],0.15,2,0.0008, DIRECT); // f.e. PKW Flachanker P=0.35,I=2,D=0.0008
// =========================== [ set up ]=========================================================================
void setup() {
Serial.begin(9600); // monitor everything
Serial.println(" ");
Serial.println("AMS RPM controller V.0.8.7");
// pin definition
pinMode(2,OUTPUT); // multiplexer 4051, LSB
pinMode(3,OUTPUT); // multiplexer 4051, HSB
pinMode(analogPin,INPUT); // setpoints cars 1...4
pinMode(bemf1pin,INPUT); // back-emf car 1
pinMode(bemf2pin,INPUT); // back-emf car 2
pinMode(bemf3pin,INPUT); // back-emf car 3
pinMode(bemf4pin,INPUT); // back-emf car 4
pinMode(L293V12ena,OUTPUT); // H-bridge V 1,2 high state
pinMode(L293V34ena,OUTPUT); // H-bridge V 3,4 high state
digitalWrite(L293V12ena,LOW);
digitalWrite(L293V34ena,LOW);
pinMode(L293V1pin,OUTPUT); // H bridge input, cars 1..4
pinMode(L293V2pin,OUTPUT);
pinMode(L293V3pin,OUTPUT);
pinMode(L293V4pin,OUTPUT);
// read and show reference values
digitalWrite(L293V34ena,LOW); // cars 1 + 3 high state
digitalWrite(L293V1pin,HIGH); // car 1 to plus
digitalWrite(L293V2pin,HIGH); // car 3 to plus
digitalWrite(L293V12ena,HIGH); // cars 1 + 3 enable
delay(1000);
ref[0] = analogRead(bemf1pin); // read reference (standstill) value car 1
ref[2] = analogRead(bemf3pin); // read reference (standstill) value car 3
digitalWrite(L293V12ena,LOW); // cars 2 + 4 high state
digitalWrite(L293V3pin,HIGH); // car2 to plus
digitalWrite(L293V4pin,HIGH); // car 4 to plus
digitalWrite(L293V34ena,HIGH); // cars 2 + 4 enable
delay(500);
ref[1] = analogRead(bemf2pin); // read reference (standstill) value car 2
ref[3] = analogRead(bemf4pin); // read reference (standstill) value car 4
for (i = 0;i<4;i++) { // show reference value information
Serial.print ("Car ");
Serial.print (i+1);
if (ref[i] < 800 || ref[i] > 950) {
Serial.print(" error: ");
}
else {
Serial.print (" ok: ");
carenable[i] =1;
}
Serial.println(ref[i],DEC);
}
delay(3000);
Serial.println();
Serial.println("running...");
// initialize myPID controller
myPID1.SetMode(AUTOMATIC);
myPID1.SetSampleTime(10);
myPID2.SetMode(AUTOMATIC);
myPID2.SetSampleTime(10);
myPID3.SetMode(AUTOMATIC);
myPID3.SetSampleTime(10);
myPID4.SetMode(AUTOMATIC);
myPID4.SetSampleTime(10);
}
/* =========================== end of setup ======================================================================
=========================== [ main ] ========================================================================== */
void loop() {
// 1. halfwave
// setup L293 for bemf measurement car 1 + 3
digitalWrite(L293V34ena,LOW); // L293 cars 1 + 3 high state
digitalWrite(L293V1pin,HIGH); // car 1 to plus
digitalWrite(L293V2pin,HIGH); // car 3 to plus
digitalWrite(L293V12ena,HIGH);
delayMicroseconds(500); // allow settings (0,5ms)
// measure rpm car 1 + 3
rpm[0] = 0;
rpm[2] = 0;
for (i = 0; i < samples; i++) { // do 20 or so readings
data1 = analogRead(bemf1pin); // read supply voltage minus bemf car 1
if (data1 < ref[0]/4) { // poor contact?
data1 = ref[0];
}
rpm[0] += ref[0]-min(data1,ref[0]); // calculate rpm car1 and limit
data2 = analogRead(bemf3pin); // read supply voltage minus bemf car 3
if (data2 < ref[2]/4) { // poor contact??
data2 = ref[2];
}
rpm[2] += ref[2]-min(data2,ref[2]); // calculate rpm car3 and limit
}
rpm[0] = constrain(rpm[0]*carspeed[0],0,1023); // include motor mapping and limit
rpm[2] = constrain(rpm[2]*carspeed[2],0,1023); // include motor mapping and limit
// setup L293 for pwm
analogWrite(L293V1pin,pwm[0]);
digitalWrite(L293V4pin,LOW);
analogWrite(L293V2pin,pwm[2]);
digitalWrite(L293V3pin,LOW);
digitalWrite(L293V34ena,HIGH); // end of measurement
// calculate new PWM value car 1
if (setpoint[0] <1 || carenable[0]==0) { // standstill
analogWrite(L293V1pin,0);
}
else {
myPID1.Compute(); // calculate new PWM value
analogWrite(L293V1pin,pwm[0]); // write new PWM value car 1
}
// calculate new PWM value car 3
if (setpoint[2] <1 || carenable[2]==0) { // standstill
analogWrite(L293V2pin,0);
}
else {
myPID3.Compute(); // calculate new PWM value
analogWrite(L293V2pin,pwm[2]); // write new PWM value car 3
}
// read setpoints car 1 and 3
digitalWrite(mx0,LOW); // initialize multiplexer 4051
digitalWrite(mx1,LOW);
setpoint[0] = analogRead(analogPin); // read Setpoint 1
digitalWrite(mx1,HIGH);
setpoint[2] = analogRead(analogPin); // read Setpoint 3
// show parameters
Serial.print(setpoint[0],0); // useful for diagnosis and adjustment but affects timing
Serial.print(" "); // shows car 1 and car 2 parameters
Serial.print(rpm[0],0);
Serial.print(" ");
Serial.print(pwm[0],0);
Serial.print(" ");
Serial.print(setpoint[1],0);
Serial.print(" ");
Serial.print(rpm[1],0);
Serial.print(" ");
Serial.print(pwm[1],0);
Serial.print(" ");
delay(pulsetime); // hold signal until end of halfwave
// end 1. halfwave
// 2. halfwave
// setup L293 for bemf measurement car 2 + 4
digitalWrite(L293V12ena,LOW); // L293 cars 2 + 4 high state
digitalWrite(L293V3pin,HIGH); // car 2 to plus
digitalWrite(L293V4pin,HIGH); // car 4 to plus
digitalWrite(L293V34ena,HIGH);
delayMicroseconds(500); // allow settings (0,5ms)
// measure bemf car 2 + 4
rpm[1] = 0;
rpm[3] = 0;
for ( i = 0; i < samples; i++) { // do 20 or so readings
data1 = analogRead(bemf2pin); // read supply voltage minus bemf car 2
if (data1 < ref[1]/5){ // poor contact?
data1 = ref[1];
}
rpm[1] += ref[1]-min(data1,ref[1]); // calculate rpm car 2
data2 = analogRead(bemf4pin); // read supply voltage minus bemf car4
if (data2 < ref[3]/5) { // poor contact?
data2 = ref[3];
}
rpm[3] +=ref[3]-min(data2,ref[3]); // calculate rpm car 4
}
rpm[1] = constrain(rpm[1]*carspeed[1],0,1023);
rpm[3] = constrain(rpm[3]*carspeed[3],0,1023);
// setup L293 for PWM
analogWrite(L293V4pin,pwm[1]);
digitalWrite(L293V1pin,LOW);
analogWrite(L293V3pin,pwm[3]);
digitalWrite(L293V2pin,LOW);
digitalWrite(L293V12ena,HIGH); // end of measurement
// calculate new PWM value car 2
if (setpoint[1] <1 || carenable[1] ==0) { // standstill
analogWrite(L293V4pin,0);
}
else {
myPID2.Compute(); // calculate new PWM value
analogWrite(L293V4pin,pwm[1]); // write new PWM value car 2
}
// calculate new PWM value car 4
if (setpoint[3] <1 || carenable[3] ==0) { // standstill
analogWrite(L293V3pin,0);
}
else {
myPID4.Compute(); // calculate new PWM value
analogWrite(L293V3pin,pwm[3]); // write new PWM value car 4
}
// check setpoints car 2 and 4
digitalWrite(mx0,HIGH); // initialize multiplexer 4051
digitalWrite(mx1,LOW);
setpoint[1] = analogRead(analogPin); // read Setpoint 2
digitalWrite(mx1,HIGH);
setpoint[3] = analogRead(analogPin); // read Setpoint 4
Serial.print(setpoint[2],0); // useful for diagnosis and adjustment but affects timing
Serial.print(" "); // shows car 3 and 4 parameters
Serial.print(rpm[2],0);
Serial.print(" ");
Serial.print(pwm[2],0);
Serial.print(" ");
Serial.print(setpoint[3],0);
Serial.print(" ");
Serial.print(rpm[3],0);
Serial.print(" ");
Serial.print(pwm[3],0);
Serial.println(" ");
delay(pulsetime); // hold signal until end of halfwave
// end 2. halfwave
} // repeat forever
// ==========================[ end main ] ========================================================================