Nachfolgendes Testprogramm lässt den AU.ROB eine gewisse Zeit geradeaus fahren. Mein AU.ROB meistert diese Übung wirklich ausgezeichnet. D.h. die Auflösung meiner Rasterscheiben von 30 ist für diesen Zweck ausreichen. Natürlich, je höher die Auflösung umso genauer das Ergebnis.
// Motor
int M11=8; // Motor 1-1
int M12=7; // Motor 1-2
int PWM1=6; // PWM Motor 1
int M21=5; // Motor 2-1
int M22=4; // Motor 2-2
int PWM2=3; // PWM Motor 2
int LSMotorR = 11;
int LSMotorL = 12;
void motorStop() {
digitalWrite(M11, LOW);
digitalWrite(M12, LOW);
digitalWrite(M21, LOW);
digitalWrite(M22, LOW);
delay(500);
}
void setup() {
pinMode(M11, OUTPUT);
pinMode(M12, OUTPUT);
pinMode(M21, OUTPUT);
pinMode(M22, OUTPUT);
pinMode(PWM1, OUTPUT);
pinMode(PWM2, OUTPUT);
motorStop();
Serial.begin(115200);
}
int inByte = 0;
int MyTime = 0;
int LSMotorL_val = 0;
int LSMotorR_val = 0;
int LSMotorL_old = 1;
int LSMotorR_old = 1;
int LSMotorL_count = 0;
int LSMotorR_count = 0;
void loop() {
if (Serial.available() > 0) {
inByte = Serial.read();
switch (inByte) {
case 48: // Zahl 0
MyTime = 300;
break;
case 49:
MyTime = 600;
break;
case 50:
MyTime = 900;
break;
case 51:
MyTime = 2000;
break;
case 52:
MyTime = 4000;
break;
case 53:
MyTime = 6000;
break;
}
Serial.println(MyTime);
digitalWrite(M11, HIGH);
digitalWrite(M12, LOW);
digitalWrite(M21, LOW);
digitalWrite(M22, HIGH);
for (int i=0 ; i<MyTime ; i++) {
LSMotorL_val = digitalRead(LSMotorL);
LSMotorR_val = digitalRead(LSMotorR);
if (LSMotorL_val != LSMotorL_old) {
LSMotorL_old = LSMotorL_val;
LSMotorL_count++;
}
if (LSMotorR_val != LSMotorR_old) {
LSMotorR_old = LSMotorR_val;
LSMotorR_count++;
}
if (LSMotorR_count == LSMotorL_count) {
analogWrite(PWM1, 160);
analogWrite(PWM2, 160);
}
else if (LSMotorR_count > LSMotorL_count) {
analogWrite(PWM1, 160);
analogWrite(PWM2, 0);
}
else if (LSMotorR_count < LSMotorL_count) {
analogWrite(PWM1, 0);
analogWrite(PWM2, 160);
}
Serial.print(“LSMotorL_count => “);
Serial.println(LSMotorL_count);
Serial.print(“LSMotorR_count => “);
Serial.println(LSMotorR_count);
}
LSMotorL_val = 0;
LSMotorR_val = 0;
LSMotorL_old = 1;
LSMotorR_old = 1;
LSMotorL_count = 0;
LSMotorR_count = 0;
motorStop();
}
}