Commit 9fce3463 authored by Jens Noack's avatar Jens Noack

Added Coderacer MKII Arduino files

parent 0281b951
// Übungen mit Lösungen
// Stand 20.11.2019
// Ergänze den Code für die Übungen an den Stellen wo die Übung formuliert ist, bzw. wo der Hinweis steht das der Code hier einesetzt werden soll.
// !!! Denke daran dir stets eine Sicherungskopie der letzten lauffähigen Programmversion abzulegen, also auch hiervon, bevor du weitermachst!!!
// Ich schlage vor jeweils das Datum in umgekehrter Reihenfolge anzuhängen. Für den 2. November 2019 also 191102.
// Dann werden die Programmversionen im File Exporer sortiert angezeigt. Wenn nötig kannst du zusätzlich die Uhrzeit anhängen.
//
// Einige Übungen sind einfach mit Copy und Paste zu erledigen. Aber auch das will gelent sein. Oft geht dabei was schief und es fehlt z.B.
// die schließende Klammer für einen Anweisungsblock. Wenn dir so etwas passiert oder sich aus anderen Gründen dein Code nicht mehr übersetzen lässt
// und du den Fehler nicht sofort findest Kommentier den betreffenden Teil, oder auch z.b. den ganzen Inhalt der switch Anweisung, mit /* und */ aus,
// so dass es sich wieder übersetzen lässt. Kommentier dann Bloch für Block oder auch zeile für zeile wieder ein indem du /* oder */ an eine andere Position setzt.
//
// Bedenke das zum Programmieren alle Microschalter in der Stellung 0 oder off stehen müssen. Wenn das nicht der Fall ist kann es sein das sich der ESP32 nicht programmieren lässt.
// Um dir die Arbeit zu erleichten kannst du den case 0 auskommentieren und den case in dem du die Übung bearbeitest vorübergehend zu case 0 machen. Das erspart das einstellen der Zahl.
// Schalterstellung Aktion
// 0000 Lauflicht
// 0001 measure battery voltage and show as bar
// 0010 move Servo
// 0011 measure distance and show as binary number
// 0100 measure distance and show as bar
// 0101 Motoren vorwärts
// 0110 Motoren rückwärts
// 0111 Motor Links vorwärts/rückwärts
// 1000 Motor Rechts vorwärts/rückwärts
// 1001 Fahren, 2 Sekunden nach loslassen der Taste_Links fährt der CodeRacer 5 Sekunden vorwärts
// 1010 Variable Geschwindigkeit für beide Motoren und anzeigen des Zählers mit den LEDs
// 1011 x
// 1100 x
// 1101 x
// 1110 x
// 1111 x
// Übung 0, case 0, CProgrammiere die Funktion Lauflicht. Den Rumpf des Unterprogramms findest du im Abschnitt Subroutines.
// Übung 1, case 4, case 3 als Balken Anzeige für die Werteintervalle bis 4, 8, 12, 16, 20, 24
// Übung 2, case 5, Ergänzen für den rechten Motor
// Übung 3, case 6, case 5 ändern für Motoren rückwärts
// Übung 4, case 7, linker Motor vorwärts wenn Taste_Links gedrückt und rückwärts wenn Taste_Rechts gedrückt
// Übung 5, case 8, rechter Motor vorwärts wenn Taste_Links gedrückt und rückwärts wenn Taste_Rechts gedrückt
// Übung 6, case 9, 2 Sekunden nach loslassen der Taste_Links soll der CodeRacer 5 Sekunden vorwärts fahren und dann wieder halten
//Schreibe Funktionen MotorLinks(byte Richtung, byte Speed) und MotorRechts(byte Richtung, byte Speed)
//Ersetze die Befehle um die Motoren anzusteuern im gesamten Programm durch diese Funktionen.
//CodeRacer um ein Rad drehen
//Ein Rad steht still. Das andere Rad läuft vorwärts. Wie lange muss man den Motor eingeschaltet lassen , bzw. wie viele Impulse abwarten, bis das Fahrzeug wieder an der ursprünglichen Position steht?
//CodeRacer auf der Stelle drehen
//Beide Räder drehen sich in entgegengesetzte Richtung. Das Fahrzeug dreht sich auf der Stelle. Wie lange muss man den Motor eingeschaltet lassen , bzw. wie viele Impulse abwarten, bis das Fahrzeug wieder an der ursprünglichen Position steht?
//Quadrat abfahren
//Der CodeRacer soll 50cm geradeaus fahren, dann um 90\[Degree] drehen, wieder 50cm geradeaus fahren und so weiter bis es wieder auf der Ausgangsposition steht.
// Arduino Code für CodeRacer MK II des MakerLab Murnau
// Testen der Hardware des CodeRacers
//------------------------------------------------------------------------------------------------------------------------
#include <ESP32Servo.h>
#include "esp32-hal-ledc.h"
//-- Pin Zuordnung -------------------------------------------------------------------------------------------------------
#define LED_1 32 // Pins an dem die LEDs angeschlossen sind
#define LED_2 33
#define LED_3 25
#define LED_4 27
#define LED_5 14
#define LED_6 12
#define Taster_Links 13 // Pins an denen die Schalter angeschlossen sind
#define Taster_Rechts 16
#define Schalter_1 4
#define Schalter_2 0
#define Schalter_3 2
#define Schalter_4 15
#define SERVO_Pin 18 // Pin an dem der Servo angeschlossen ist
#define Trigger 21 // Pin an denen der Ultraschallabstandssensor angeschlossen ist
#define Echo 17
#define Motor_Links_Ein 23 // Pins an denen die Motore angeschlossen sind
#define Motor_Links_VorwaertsRueckwaerts 22
#define Motor_Rechts_Ein 5
#define Motor_Rechts_VorwaertsRueckwaerts 19
#define Geschwindigkeit_Links 34 // Pins an denen die Geschwindigkeitssensoren angeschlossen sind
#define Geschwindigkeit_Rechts 35
#define Abstand_Hinten_Links x // Pins an denen die Abstandssensoren hinten angeschlossen sind
#define Abstand_Hinten_Rechts x
#define Batteriespannung 26 // Pin an dem der Spannungsteiler für die Batterie Spannung angeschlossen ist
#define PWM_Links 5 // Zuordnung der PWM Kanäle
#define PWM_Rechts 6
//-- Defines --------------------------------------------------------------------------------------------------------------------
#define LEDon 100 // 100ms eingeschaltet
#define LEDoff 50 // 50ms ausgeschaltet
#define Vorwaerts HIGH
#define Rueckwaerts LOW
//-- Globale Variables ----------------------------------------------------------------------------------------------------------
int i, Number, Schalter;
volatile unsigned int StL, StR;
volatile unsigned int StepsL, StepsR;
volatile unsigned long timeL, timeR, timeLPrev, timeRPrev;
volatile unsigned long countL, countR;
volatile unsigned int diffL, diffR;
volatile boolean isrRFlag, isrLFlag;
Servo servo;
//-- Subroutines ----------------------------------------------------------------------------------------------------------------
long measureDistance()
{ long dist, duration;
pinMode(Echo, INPUT);
digitalWrite(Trigger, LOW);
delayMicroseconds(2);
digitalWrite(Trigger, HIGH);
delayMicroseconds(10);
digitalWrite(Trigger, LOW);
duration=pulseIn(Echo,HIGH);
dist=duration/2*0.0344;
return(dist);
}
void Lauflicht()
{ // Ergänze hier den Code für Übung 0
digitalWrite(LED_1,HIGH);
delay(LEDon);
digitalWrite(LED_1,LOW);
delay(LEDoff);
digitalWrite(LED_2,HIGH);
delay(LEDon);
digitalWrite(LED_2,LOW);
delay(LEDoff);
digitalWrite(LED_3,HIGH);
delay(LEDon);
digitalWrite(LED_3,LOW);
delay(LEDoff);
digitalWrite(LED_4,HIGH);
delay(LEDon);
digitalWrite(LED_4,LOW);
delay(LEDoff);
digitalWrite(LED_5,HIGH);
delay(LEDon);
digitalWrite(LED_5,LOW);
delay(LEDoff);
digitalWrite(LED_6,HIGH);
delay(LEDon);
digitalWrite(LED_6,LOW);
delay(LEDoff);
}
void LEDsoff()
{ digitalWrite(LED_1,LOW);
digitalWrite(LED_2,LOW);
digitalWrite(LED_3,LOW);
digitalWrite(LED_4,LOW);
digitalWrite(LED_5,LOW);
digitalWrite(LED_6,LOW);
}
void ShowDistance(int distance)
{ digitalWrite(LED_1, bitRead(distance,6));
digitalWrite(LED_2, bitRead(distance,5));
digitalWrite(LED_3, bitRead(distance,4));
digitalWrite(LED_4, bitRead(distance,3));
digitalWrite(LED_5, bitRead(distance,2));
digitalWrite(LED_6, bitRead(distance,1));
};
void MotorLinks(byte Richtung, byte Speed)
{ digitalWrite(Motor_Links_VorwaertsRueckwaerts, Richtung);
ledcWrite(PWM_Links, Speed);
}
void MotorRechts(byte Richtung, byte Speed)
{ digitalWrite(Motor_Rechts_VorwaertsRueckwaerts, Richtung);
ledcWrite(PWM_Rechts, Speed);
}
void DreheLinks(int steps)
{ int StartR;
StartR = countR;
digitalWrite(Motor_Links_VorwaertsRueckwaerts, HIGH);
digitalWrite(Motor_Rechts_VorwaertsRueckwaerts,HIGH);
ledcWrite(PWM_Links, 0);
ledcWrite(PWM_Rechts, 80);
while ( (countR-StartR)<steps ) {};
ledcWrite(PWM_Links, 0);
ledcWrite(PWM_Rechts, 0);
}
void DreheRechts(int steps)
{ int StartL;
StartL = countL;
digitalWrite(Motor_Links_VorwaertsRueckwaerts, HIGH);
digitalWrite(Motor_Rechts_VorwaertsRueckwaerts,HIGH);
ledcWrite(PWM_Links, 85);
ledcWrite(PWM_Rechts, 0);
while ( (countL-StartL)<steps ) {};
ledcWrite(PWM_Links, 0);
ledcWrite(PWM_Rechts, 0);
}
void IRAM_ATTR detectStepL()
{ timeL = micros();
diffL = timeL - timeLPrev;
timeLPrev = timeL;
countL++;
isrLFlag = true;
}
void IRAM_ATTR detectStepR()
{ timeR = micros();
diffR = timeR - timeRPrev;
timeRPrev = timeR;
countR++;
isrRFlag = true;
}
//-- Setup ----------------------------------------------------------------------------------------------------------------
void setup()
{ Serial.begin(115200); // Serielle Schnittstelle aktivieren, Erstellungsdatum senden
Serial.println("Build "+String(__DATE__)+" "+String(__TIME__)+"\n");
pinMode(LED_1, OUTPUT); // LED Pins als Ausgang setzen und LEDs ausschalten
digitalWrite(LED_1,LOW);
pinMode(LED_2, OUTPUT);
digitalWrite(LED_2,LOW);
pinMode(LED_3, OUTPUT);
digitalWrite(LED_3,LOW);
pinMode(LED_4, OUTPUT);
digitalWrite(LED_4,LOW);
pinMode(LED_5, OUTPUT);
digitalWrite(LED_5,LOW);
pinMode(LED_6, OUTPUT);
digitalWrite(LED_6,LOW);
pinMode(Taster_Links, INPUT_PULLUP); // Taster Pins als Eingang setzen
pinMode(Taster_Rechts, INPUT_PULLUP);
pinMode(Schalter_1, INPUT_PULLUP); // Schalter Pins als Eingang setzen
pinMode(Schalter_2, INPUT_PULLUP);
pinMode(Schalter_3, INPUT_PULLUP);
pinMode(Schalter_4, INPUT_PULLUP);
servo.attach(SERVO_Pin); // Pin für Servo setzen
pinMode(Trigger, OUTPUT); // Pins für Ultraschallsensor setzen
digitalWrite(Trigger,LOW);
pinMode(Echo, INPUT);
pinMode(Motor_Links_Ein, OUTPUT); digitalWrite(Motor_Links_Ein,LOW); // Pins für Motore setzen
pinMode(Motor_Links_VorwaertsRueckwaerts, OUTPUT); digitalWrite(Motor_Links_VorwaertsRueckwaerts,LOW);
pinMode(Motor_Rechts_Ein, OUTPUT); digitalWrite(Motor_Rechts_Ein,LOW);
pinMode(Motor_Rechts_VorwaertsRueckwaerts, OUTPUT); digitalWrite(Motor_Rechts_VorwaertsRueckwaerts,LOW);
ledcSetup(PWM_Links, 100, 8);
ledcAttachPin(Motor_Links_Ein, PWM_Links);
ledcWrite(PWM_Links, 0);
ledcSetup(PWM_Rechts, 100, 8);
ledcAttachPin(Motor_Rechts_Ein, PWM_Rechts);
ledcWrite(PWM_Rechts, 0);
// ledcSetup(7, 50, 8);
// ledcAttachPin(LED_6, 7);
// ledcWrite(7, 127);
pinMode(Geschwindigkeit_Links , INPUT); // Pins für Geschwindigkeitssensoren setzen und mit Interrupt Service Routine verknüpfen
pinMode(Geschwindigkeit_Rechts, INPUT);
attachInterrupt(digitalPinToInterrupt(Geschwindigkeit_Links ), detectStepL, RISING);
attachInterrupt(digitalPinToInterrupt(Geschwindigkeit_Rechts), detectStepR, RISING);
}
//-- Main ----------------------------------------------------------------------------------------------------------------
void loop(void)
{ byte TL, TR, S1, S2, S3, S4, b, brightness;
int speed, speedSet, speedSetL, speedSetR, speedL, speedR, g, minspeed, maxspeed, distance;
int DistanceL, DistanceR;
unsigned int StartL, StartR;
while (true)
{ servo.write(90);
TL = !digitalRead(Taster_Links );
TR = !digitalRead(Taster_Rechts);
S1 = !digitalRead(Schalter_1 );
S2 = !digitalRead(Schalter_2 );
S3 = !digitalRead(Schalter_3 );
S4 = !digitalRead(Schalter_4 );
Schalter = (S1<<3) | (S2<<2) | (S3<<1) | S4;
//Serial.println(Schalter);
switch(Schalter)
{ case 0: // Hindernissparcour 0
{ while( (distance=measureDistance()) > 15 ) // 7 ~ 10cm
{ ShowDistance(distance);
MotorLinks(Vorwaerts, 100);
MotorRechts(Vorwaerts, 100);
};
MotorLinks(Vorwaerts, 0);
MotorRechts(Vorwaerts, 0);
ShowDistance(distance);
servo.write(180);
delay(400);
DistanceL=measureDistance();
servo.write(0);
delay(400);
DistanceR=measureDistance();
servo.write(90);
delay(400);
if( DistanceL<DistanceR )
{ // drehe nach rechts
DreheRechts(16);
}
else
{ // drehe nach linhs
DreheLinks(16);
};
break;
};
/*case 15: //----------------------------------------------- Lauflicht
}
{ delay(100);
// Übung 0, Programmiere die Funktion Lauflicht. Den Rumpf des Unterprogramms findest du im Abschnitt Subroutines.
Lauflicht();
break;
};*/
/* case 1: //----------------------------------------------- measure battery voltage and show as bar
{ //delay(1000);
//LEDsoff();
ledcSetup(7, 50, 8);
ledcAttachPin(LED_6, 7);
for( brightness = 255; brightness > 0; brightness = brightness - 1)
{ delay(10);
ledcWrite(7, brightness);
};
for( brightness = 0; brightness < 254; brightness = brightness + 1)
{ delay(10);
ledcWrite(7, brightness);
};
ledcDetachPin(LED_6);
pinMode(LED_6, OUTPUT);
digitalWrite(LED_6,LOW);
break;
};*/
case 2: //----------------------------------------------- move Servo
{ LEDsoff(); delay(100);
if( TL && TR )
{ servo.write(90);
break;
}
else if( TL )
{ servo.write(180);
break;
}
else if( TR )
{ servo.write(0);
break;
}
else
{ break;
};
};
case 3: //----------------------------------------------- measure distance and show as binary number
{ delay(200);
distance = measureDistance();
Serial.println(distance);
digitalWrite(LED_1, bitRead(distance,6));
digitalWrite(LED_2, bitRead(distance,5));
digitalWrite(LED_3, bitRead(distance,4));
digitalWrite(LED_4, bitRead(distance,3));
digitalWrite(LED_5, bitRead(distance,2));
digitalWrite(LED_6, bitRead(distance,1));
break;
};
case 4: //----------------------------------------------- measure distance and show as bar
{ delay(200);
distance = measureDistance();
Serial.println(distance);
// Übung 1, case 3 als Balken Anzeige für die Werteintervalle bis 4, 8, 12, 16, 20, 24
digitalWrite(LED_6, distance > 4 );
digitalWrite(LED_5, distance > 8 );
digitalWrite(LED_4, distance > 12 );
digitalWrite(LED_3, distance > 16 );
digitalWrite(LED_2, distance > 20 );
digitalWrite(LED_1, distance > 24 );
break;
};
case 5: //----------------------------------------------- Motoren vorwärts
{ LEDsoff(); delay(100);
if( TL )
{ digitalWrite(Motor_Links_VorwaertsRueckwaerts, HIGH);
ledcWrite(PWM_Links, 255);
}
else
{ ledcWrite(PWM_Links, 0);
};
// Übung 2, Ergänzen für den rechten Motor
if( TR )
{ digitalWrite(Motor_Rechts_VorwaertsRueckwaerts, HIGH);
ledcWrite(PWM_Rechts, 255);
}
else
{ ledcWrite(PWM_Rechts, 0);
};
break;
};
case 6: //----------------------------------------------- Motoren rückwärts
{ LEDsoff(); delay(100);
// Übung 3, case 5 ändern für Motoren rückwärts
if( TL )
{ digitalWrite(Motor_Links_VorwaertsRueckwaerts, LOW);
ledcWrite(PWM_Links, 255);
}
else
{ ledcWrite(PWM_Links, 0);
};
if( TR )
{ digitalWrite(Motor_Rechts_VorwaertsRueckwaerts, LOW);
ledcWrite(PWM_Rechts, 255);
}
else
{ ledcWrite(PWM_Rechts, 0);
};
break;
};
case 7: //----------------------------------------------- Motor Links vorwärts/rückwärts
{ LEDsoff(); delay(100);
// Übung 4, linker Motor vorwärts wenn Taste_Links gedrückt und rückwärts wenn Taste_Rechts gedrückt
if( TL )
{ digitalWrite(Motor_Links_VorwaertsRueckwaerts,HIGH);
ledcWrite(PWM_Links, 255);
break;
}
else
{ ledcWrite(PWM_Links, 0);
};
if( TR )
{ digitalWrite(Motor_Links_VorwaertsRueckwaerts,LOW);
ledcWrite(PWM_Links, 255);
break;
}
else
{ ledcWrite(PWM_Links, 0);
};
break;
};
case 8: //----------------------------------------------- Motor Rechts vorwärts/rückwärts
{ LEDsoff(); delay(100);
// Übung 5, rechter Motor vorwärts wenn Taste_Links gedrückt und rückwärts wenn Taste_Rechts gedrückt
if( TL )
{ digitalWrite(Motor_Rechts_VorwaertsRueckwaerts,HIGH);
ledcWrite(PWM_Rechts, 255);
break;
}
else
{ ledcWrite(PWM_Rechts, 0);
};
if( TR )
{ digitalWrite(Motor_Rechts_VorwaertsRueckwaerts,LOW);
ledcWrite(PWM_Rechts, 255);
break;
}
else
{ ledcWrite(PWM_Rechts, 0);
};
break;
};
case 9: //----------------------------------------------- Fahren, 2 Sekunden nach loslassen der Taste_Links fährt der CodeRacer 5 Sekunden vorwärts
{ LEDsoff(); delay(100);
if( TL )
{ while(!digitalRead(Taster_Links )) {}; // wait until button is released
// Übung 6, 2 Sekunden nach loslassen der Taste_Links soll der CodeRacer 5 Sekunden vorwärts fahren und dann wieder halten
delay(2000);
digitalWrite(Motor_Links_VorwaertsRueckwaerts, HIGH);
digitalWrite(Motor_Rechts_VorwaertsRueckwaerts,HIGH);
ledcWrite(PWM_Links, 100);
ledcWrite(PWM_Rechts, 100);
delay(3000);
ledcWrite(PWM_Links, 0);
ledcWrite(PWM_Rechts, 0);
}
break;
};
case 10: //----------------------------------------------- Variable Geschwindigkeit für beide Motoren und anzeigen des Zählers mit den LEDs
{ LEDsoff(); delay(100); // Start 2 Sekunden nach loslassen der Taste_Links
if( TL )
{ while(!digitalRead(Taster_Links )) {}; // wait until button is released
delay(2000);
digitalWrite(Motor_Links_VorwaertsRueckwaerts, HIGH);
digitalWrite(Motor_Rechts_VorwaertsRueckwaerts,HIGH);
for( speed = 80; speed <= 255; speed = speed + 4 )
{ delay(20);
ledcWrite(PWM_Links, speed);
ledcWrite(PWM_Rechts, speed);
Serial.println(speed);
digitalWrite(LED_1, bitRead(speed,8));
digitalWrite(LED_2, bitRead(speed,7));
digitalWrite(LED_3, bitRead(speed,6));
digitalWrite(LED_4, bitRead(speed,5));
digitalWrite(LED_5, bitRead(speed,4));
digitalWrite(LED_6, bitRead(speed,3));
};
delay(2000);
for( speed = 255; speed >= 80; speed = speed - 4 )
{ delay(20);
ledcWrite(PWM_Links, speed);
ledcWrite(PWM_Rechts, speed);
Serial.println(speed);
digitalWrite(LED_1, bitRead(speed,6));
digitalWrite(LED_2, bitRead(speed,5));
digitalWrite(LED_3, bitRead(speed,4));
digitalWrite(LED_4, bitRead(speed,3));
digitalWrite(LED_5, bitRead(speed,2));
digitalWrite(LED_6, bitRead(speed,1));
};
ledcWrite(PWM_Links, 0);
ledcWrite(PWM_Rechts, 0);
break;
};
};
case 11: //----------------------------------------------- circle left and right
{ LEDsoff(); delay(100); // Start 2 Sekunden nach loslassen der linken oder rechten Taste
if( TL )
{ while(!digitalRead(Taster_Links )) {}; // wait until button is released
delay(2000);
DreheLinks(16); // 65 ganze Drehung
};
if( TR )
{ while(!digitalRead(Taster_Rechts )) {}; // wait until button is released
delay(2000);
DreheRechts(16); // 65 ganze Drehung
};
break;
};
/*case 0: //----------------------------------------------- Fahre Qudrat
{ //delay(100);
if( TL )
{ while(!digitalRead(Taster_Links )) {}; // wait until button is released
delay(2000);
for( i = 0; i < 4; i = i + 1 )
{//1. Strecke
StartL = StL;
StartR = StR;
MotorLinks(Vorwaerts, 70);