/************************* * * Strandbeest-Steuerung * www.yeat.net * *************************/ /* Pin-Zuordnung des Motortreibers * PIN_ML... = linker Motor * PIN_MR... = rechter Motor */ #define PIN_ML_ENABLE_FORWARD 3 #define PIN_ML_ENABLE_REVERSE 4 #define PIN_ML_SPEED 5 #define PIN_MR_ENABLE_FORWARD 7 #define PIN_MR_ENABLE_REVERSE 8 #define PIN_MR_SPEED 6 /* Pin-Zuordnung des RC-Empfängers * RH = Kanal rechter Hebel, horizontal * RV = Kanal rechter Hebel, vertikal * LH = Kanal linker Hebel, horizontal */ #define PIN_INPUT_RH A0 #define PIN_INPUT_RV A1 #define PIN_INPUT_LH A3 /* Grenzwerte der Hebel-Auslenkungen * durch Ausprobieren herausfinden * MIN_INPUT... = ganz links oder unten * MAX_INPUT... = ganz rechts oder oben */ #define MIN_INPUT_RV 1200 #define MAX_INPUT_RV 1785 #define MIN_INPUT_RH 1180 #define MAX_INPUT_RH 1815 #define MIN_INPUT_LH 1150 #define MAX_INPUT_LH 1815 /* Grenzwert, ab dem eine Bewegung erkannt wird: * 10% um die Mittelstellung passiert nichts ("Nullbereich") */ #define THRESHOLD_RV 0.1 #define THRESHOLD_RH 0.1 #define THRESHOLD_LH 0.1 /* Vorabberechnung der Bewegungsspannbreite und * der Mittelstellung */ int inputRVRange = MAX_INPUT_RV - MIN_INPUT_RV; int inputRVMid = (double)inputRVRange / 2.0; int inputRHRange = MAX_INPUT_RH - MIN_INPUT_RH; int inputRHMid = (double)inputRHRange / 2.0; int inputLHRange = MAX_INPUT_LH - MIN_INPUT_LH; int inputLHMid = (double)inputLHRange / 2.0; /* Setup-Routine * wird einmalig beim Einschalten aufgerufen */ void setup() { //Serielle Schnittstelle starten Serial.begin(9600); //Pins des Motor-Treibers einrichten pinMode(PIN_MR_ENABLE_FORWARD, OUTPUT); pinMode(PIN_MR_ENABLE_REVERSE, OUTPUT); pinMode(PIN_MR_SPEED, OUTPUT); pinMode(PIN_ML_ENABLE_FORWARD, OUTPUT); pinMode(PIN_ML_ENABLE_REVERSE, OUTPUT); pinMode(PIN_ML_SPEED, OUTPUT); //Pins des RC-Empfängers einrichten pinMode(PIN_INPUT_RV, INPUT); pinMode(PIN_INPUT_RH, INPUT); } //Initialisierung einiger globaler Variablen boolean isOff = false; int curMotorLSpeed = 0; int curMotorRSpeed = 0; /* Arbeits-Routine * wird so lange ausgeführt, wie der Arduino läuft */ void loop() { //Auslesen der Hebelstellungen durch Messung der Pulsweiten int inputRVVal = pulseIn(PIN_INPUT_RV, HIGH, 30000); int inputRHVal = pulseIn(PIN_INPUT_RH, HIGH, 30000); int inputLHVal = pulseIn(PIN_INPUT_LH, HIGH, 30000); //nur Auswerten, wenn der Sender eingeschaltet ist (und damit Pulsweiten >0 zurückgeliefert werden) if ((inputRVVal != 0) && (inputRHVal != 0)) { //Gemessene Pulsweiten auf Maximal- und Minimalwerte begrenzen (um Werte kleiner 0% und größer 100% zu vermeiden) inputRVVal = max(min(inputRVVal, MAX_INPUT_RV), MIN_INPUT_RV); inputRHVal = max(min(inputRHVal, MAX_INPUT_RH), MIN_INPUT_RH); inputLHVal = max(min(inputLHVal, MAX_INPUT_LH), MIN_INPUT_LH); //Absolute Position der Hebel int inputRVPos = MAX_INPUT_RV - inputRVVal; int inputRHPos = MAX_INPUT_RH - inputRHVal; int inputLHPos = MAX_INPUT_LH - inputLHVal; //Relative Position der Hebel (zwischen -1 und 1) double inputRVPosRel = (double)(inputRVMid - inputRVPos) / (double)inputRVMid; double inputRHPosRel = (double)(inputRHMid - inputRHPos) / (double)inputRHMid; double inputLHPosRel = (double)(inputLHMid - inputLHPos) / (double)inputLHMid; //Werte innerhalb des Nullbereiches werden auf 0 gesetzt if (abs(inputRVPosRel) < THRESHOLD_RV) inputRVPosRel = 0.0; if (abs(inputRHPosRel) < THRESHOLD_RH) inputRHPosRel = 0.0; if (abs(inputLHPosRel) < THRESHOLD_LH) inputLHPosRel = 0.0; //nicht größer/kleiner als +/- 1.0 inputRVPosRel = max(min(inputRVPosRel, 1.0), -1.0); inputRHPosRel = max(min(inputRHPosRel, 1.0), -1.0); //Motorgeschwindigkeit initialisieren int motorLSpeed = 0; int motorRSpeed = 0; //Motorrichtungen und -geschwindigkeiten anhand Position des rechten Hebels festlegen if (inputRVPosRel != 0.0) { //Vorwärtskomponente motorLSpeed = inputRVPosRel * 255.0; motorRSpeed = inputRVPosRel * 255.0; //Seitwärtskomponente if (inputRHPosRel != 0.0) { motorLSpeed *= 2 * min(inputRHPosRel, 0.0) + 1.0; motorRSpeed *= -2 * max(inputRHPosRel, 0.0) + 1.0; } //Wenn rechter Hebel in Nullstellung, dann linken Hebel auswerten und ggf. auf der Stelle drehen } else if (inputLHPosRel != 0.0) { motorLSpeed = inputLHPosRel * 255.0; motorRSpeed = -inputLHPosRel * 255.0; } //Motor-Treiber nur behelligen, wenn sich die Geschwindigkeit eines Motors verändert hat if ((motorLSpeed != curMotorLSpeed) || (motorRSpeed != curMotorRSpeed)) { //linken Motor einstellen if (motorLSpeed != 0) { //Drehrichtung an die PIN_ML_ENABLE...-Pins ausgeben if (motorLSpeed > 0) { digitalWrite(PIN_ML_ENABLE_REVERSE, LOW); digitalWrite(PIN_ML_ENABLE_FORWARD, HIGH); } else { digitalWrite(PIN_ML_ENABLE_FORWARD, LOW); digitalWrite(PIN_ML_ENABLE_REVERSE, HIGH); } } else { digitalWrite(PIN_ML_ENABLE_FORWARD, LOW); digitalWrite(PIN_ML_ENABLE_REVERSE, LOW); } //Geschwindigkeit auf PIN_ML_SPEED schreiben analogWrite(PIN_ML_SPEED, abs(motorLSpeed)); //rechten Motor einstellen (wie linker Motor, siehe oben) if (motorRSpeed != 0) { if (motorRSpeed > 0) { digitalWrite(PIN_MR_ENABLE_REVERSE, LOW); digitalWrite(PIN_MR_ENABLE_FORWARD, HIGH); } else { digitalWrite(PIN_MR_ENABLE_FORWARD, LOW); digitalWrite(PIN_MR_ENABLE_REVERSE, HIGH); } } else { digitalWrite(PIN_MR_ENABLE_FORWARD, LOW); digitalWrite(PIN_MR_ENABLE_REVERSE, LOW); } analogWrite(PIN_MR_SPEED, abs(motorRSpeed)); //aktuelle Motorgeschwindigkeit speichern für späteren Vergleich mit neuen Geschwindigkeiten curMotorLSpeed = motorLSpeed; curMotorRSpeed = motorRSpeed; //Motor ist eingeschaltet. isOff=false; //aktuelle Geschwindigkeiten der Motoren über die serielle Schnittstelle ausgeben Serial.print("motor speed: left="); Serial.print(motorLSpeed); Serial.print(" right="); Serial.println(motorRSpeed); } /* kein Empfang - alles abschalten (wenn nicht bereits abgeschaltet)! * Das Strandbeest soll stehenbleiben, wenn die Verbindung zur Fernbedienung * verloren geht (und nicht ewig in die aktuelle Richtung weitermarschieren). */ } else if (!isOff) { digitalWrite(PIN_MR_ENABLE_REVERSE, LOW); digitalWrite(PIN_MR_ENABLE_FORWARD, LOW); analogWrite(PIN_MR_SPEED, 0); digitalWrite(PIN_ML_ENABLE_REVERSE, LOW); digitalWrite(PIN_ML_ENABLE_FORWARD, LOW); analogWrite(PIN_ML_SPEED, 0); //auch diesen Status auf der seriellen Konsole ausgeben Serial.println("no connection"); //Motor ist ausgeschaltet isOff=true; } }