diff --git a/Fahrzeug/.vscode/arduino.json b/Fahrzeug/.vscode/arduino.json
deleted file mode 100644
index 518e1b1e95e17db29418369a67d7dee7ecb8936e..0000000000000000000000000000000000000000
--- a/Fahrzeug/.vscode/arduino.json
+++ /dev/null
@@ -1,5 +0,0 @@
-{
- "port": "COM6",
- "configuration": "cpu=atmega328old",
- "board": "arduino:avr:nano"
-}
\ No newline at end of file
diff --git a/Fahrzeug/bits3-bare-minimum/basic_functions.h b/Fahrzeug/bits3-bare-minimum/basic_functions.h
index 58ebcd43d20cbba8296230fce341aafe56839642..19790ab4a63edd7972cbd7e6a13382da1f23962b 100644
--- a/Fahrzeug/bits3-bare-minimum/basic_functions.h
+++ b/Fahrzeug/bits3-bare-minimum/basic_functions.h
@@ -10,6 +10,145 @@ void buzzer_signal(int pitch, int count, int delay_time){
}
}
+//Fahrlicht: bestehend aus mittleren 4 LEDs vorne (weiss) und zwei hinteren LEDs (rot)
+void drive_light_on() {
+ front_strip.set_pixel_color(1, thk_LedStripController::Color(30, 30, 30));
+ front_strip.set_pixel_color(2, thk_LedStripController::Color(30, 30, 30));
+ front_strip.set_pixel_color(3, thk_LedStripController::Color(30, 30, 30));
+ front_strip.set_pixel_color(4, thk_LedStripController::Color(30, 30, 30));
+ front_strip.show();
+ back_strip.set_pixel_color(0, thk_LedStripController::Color(50, 0, 0));
+ back_strip.set_pixel_color(5, thk_LedStripController::Color(50, 0, 0));
+ back_strip.show();
+}
+
+//schaltet Fahrlicht wieder aus
+void drive_light_off() {
+ front_strip.set_pixel_color(1, thk_LedStripController::Color(0, 0, 0));
+ front_strip.set_pixel_color(2, thk_LedStripController::Color(0, 0, 0));
+ front_strip.set_pixel_color(3, thk_LedStripController::Color(0, 0, 0));
+ front_strip.set_pixel_color(4, thk_LedStripController::Color(0, 0, 0));
+ front_strip.show();
+ back_strip.set_pixel_color(0, thk_LedStripController::Color(0, 0, 0));
+ back_strip.set_pixel_color(5, thk_LedStripController::Color(0, 0, 0));
+ back_strip.show();
+}
+
+void communication() {
+ Serial.println("Start communication process! Wait for IR...");
+ while(command==0){
+ command = ir_sensor.receive_command();
+ }
+ Serial.println();
+
+ if(command != 0 || command == 64){
+ //command = ir_sensor.receive_command();
+ Serial.print("IR-Daten empfangen: ");
+ Serial.println(command);
+
+ // Aendern + Ausgabe der Addresse zum Schreiben
+ address_write[4]=command;
+ Serial.print("Fahrzeug schreibt auf Kanal: ");
+ for (int i = 0; i < 5; i++) {
+ Serial.print(address_write[i]);
+ Serial.print(" ");
+ }
+ Serial.println();
+
+ // Fahrzeuginformation via NRF senden
+ radio.stopListening();
+ radio.openWritingPipe(address_write);
+ char vehicle_info[] = "Gruppe1,KTH123";
+
+ //Schleife zum Senden
+ while(transmitted==false){
+ transmitted=radio.write(&vehicle_info, sizeof(vehicle_info));
+ Serial.print("Gesendete Daten: ");
+ Serial.println(vehicle_info);
+ delay(100);
+ Serial.print("Empfangen (ACK)?: ");
+ if(transmitted == false){
+ Serial.println("No");
+ }else{
+ Serial.println("Yes");
+ }
+ }
+ transmitted = false;
+
+
+ // Aendern + Ausgabe der Addresse zum Lesen
+ address_read[4]=command+1;
+ Serial.print("Fahrzeug liest auf Kanal: ");
+ for (int i = 0; i < 5; i++) {
+ Serial.print(address_read[i]);
+ Serial.print(" ");
+ }
+ Serial.println();
+
+ // Koordinaten via NRF empfangen
+ radio.openReadingPipe(1, address_read);
+ radio.startListening();
+ int text[20];
+ for(int k=0;k<20;k++){
+ text[k]=0;
+ }
+
+ // Schleife zum Empfangen
+ while(text[1]==0 && text[2]==0){
+ if(radio.available()){
+ Serial.println("Empfangene Nachricht: ");
+ radio.read(&text, sizeof(text));
+ for(int i = 0; i<20;i++){
+ Serial.println(text[i]);
+ }
+ Serial.println("Ende der Nachricht");
+ Serial.print("Groesse der Nachricht: ");
+ int size = sizeof(text);
+ int lastIndex;
+ for(int l=14; l>0;l--){ //Letzten Index des empfangenen Arrays bestimmen
+ if(text[l]!=0){
+ lastIndex = l;
+ break;
+ }
+ }
+ Serial.println(size);
+ int m = 0;
+ for(int l=0; l<ABSCHNITTE;l=l+1){
+ state_table[l].x_kor=float(text[m])/100.0;
+ state_table[l].y_kor=float(text[m+1])/100.0;
+
+ if(m==lastIndex-3){
+ state_table[l].parking_side=text[m+2]; // Seite des Parkplatzes uebergeben
+ state_table[l].parking_type=text[m+3]; // Art des Parkplatzes uebergeben
+ break;
+ }
+ m=m+2;
+ }
+ }
+ }
+ text[0]=0;
+ }
+}
+
+void printCourseTable()
+{
+ Serial.println(F("-------------- Fahrkurs -----------"));
+ for (int i = 0; i < ABSCHNITTE; i++)
+ {
+ Serial.print(state_table[i].part);
+ Serial.print(F(" "));
+ Serial.print(state_table[i].x_kor);
+ Serial.print(F(" "));
+ Serial.print(state_table[i].y_kor);
+ Serial.print(F(" "));
+ Serial.print(state_table[i].wall_side);
+ Serial.print(F(" "));
+ Serial.print(state_table[i].parking_side);
+ Serial.print(F(" "));
+ Serial.println(state_table[i].parking_type);
+ }
+}
+
void all_led_blink(int count, int delay_time){
for (int k=0; k<count; k++){
front_strip.set_pixel_color(thk_LedStripController::Color(red, green, blue));
@@ -364,4 +503,4 @@ void drive_course() { //Main-Funktion zum Abfahren des Kurses und anschliesse
end_of_programm(); //Programmende sobald alle Abschnitte durchfahren sind
}
-#endif
\ No newline at end of file
+#endif
diff --git a/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino b/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
index eb8e1b2a26f350d336e6a776d196acb4b501614b..2d427ded0499f4eb1d27816ca0b537bdd61e3f5d 100644
--- a/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
+++ b/Fahrzeug/bits3-bare-minimum/bits3-bare-minimum.ino
@@ -97,12 +97,23 @@ void setup() {
// buzzer_signal(350, 2, 100); //tone() und IR-Empfangen läuft nicht gleichzeitig!!!!
// Heranfahren an Schranke
-
- // Kommunikation-Funktion (IR+NRF)
-
- // Warten bis Schranke offen ist
+ drive_light_on();
+ ServoSteer.turn(STEER_START_POS);
+ readUS();
+ while (US_distance_f >= 100) { //Fahre bis die Schranke 100 mm entfernt
+ Motor.drive_forward(50);
+ readUS();
+ }
+ Motor.stop();
+
+ communication(); // Kommunikation-Funktion (IR+NRF)
+ printCourseTable(); // Ausgabe der Empfangenen Zustandstabelle
+ set_led_color(0, 100, 0);
+ all_led_blink(2, 200);
+ delay(3000); // Warten bis Schranke offen ist
}
+
/* ************* *
Hauptprogramm
* ************* */
diff --git a/Fahrzeug/bits3-bare-minimum/config.h b/Fahrzeug/bits3-bare-minimum/config.h
index e53a9d1ef4a9f54678ba8a22852b8b5c31d66f25..1c23931c51dd57825927c022a835da09694e70ee 100644
--- a/Fahrzeug/bits3-bare-minimum/config.h
+++ b/Fahrzeug/bits3-bare-minimum/config.h
@@ -1,7 +1,7 @@
/*
-##########################################################
-######### DIESES SKRIPT NICHT BEARBEITEN!!! ##############
-##########################################################
+####################################################################################
+######### DIESES SKRIPT BIS AUF DIE STATE MACHINE NICHT BEARBEITEN!!! ##############
+####################################################################################
*/
/* BITS-i sensor shematic
@@ -159,6 +159,18 @@ int tof_distance_front;
* ************* */
const double Fahrbahnbreite = 500.00;
+/* ************ *
+ * IR Empfänger *
+ * ************ */
+uint8_t command;
+
+/* ******** *
+ * NRF24L01 *
+ * ******** */
+byte address_read[] = {0,0,0,0,0};
+byte address_write[] = {0,0,0,0,0};
+boolean transmitted = false;
+
/* ******* *
Encoder
* ******* */
@@ -181,7 +193,7 @@ const int MIN_DIST_TURN = 300; //Mindestanbstand vorne für anschließende
/* ********** *
PID Regler
* ********** */
-const double prop_gain = 0.4;
+const double prop_gain = 0.0;
const double int_gain = 0.0;
const double diff_gain = 0.0;
uint8_t setpoint = 0; //Zielwert Regeldifferenz
@@ -194,31 +206,31 @@ double front_rear_difference_right; //Differenz TOF rechts
double front_rear_difference_left; //Differenz TOF links
const double wall_distance = (Fahrbahnbreite / 2.00) - 60.00; //Soll-Abstand zur Wand auf einer Seite, damit fzg mittig auf Spur
-/* ************* *
- State machine
- * ************* */
+/* ******************************************** *
+ State machine (diese darf bearbeitet werden)
+ * ******************************************** */
//Definiton struct für Tabelle:
typedef struct course_state_t
{
- const byte part; //Name des Streckenabschnitts
- float x_kor; //x-Koordinate (Meter)
- float y_kor; //y-Koordinate (Meter)
- int wall_side; //Seite der Wand für Kurskorrektur
- int parking_side; //Parkplatz Seite r oder l
- int parking_type; //vorwaerts / rueckwaerts / seitlich
+ const byte part; //Name des Streckenabschnitts (1. Spalte)
+ float x_kor; //x-Koordinate (Meter) (2. Spalte)
+ float y_kor; //y-Koordinate (Meter) (3. Spalte)
+ int wall_side; //Seite der Wand für Kurskorrektur (4. Spalte; 0: keine Seite verfuegbar; 1: rechts; 2: links)
+ int parking_side; //Parkplatz Seite r oder l (5. Spalte; 1: rechts; 2: links)
+ int parking_type; //Parkplatzart; 1: vorwaerts; 2: rueckwaerts; 3: seitlich
};
// Hier wird der Fahrkurs in einer Tabelle für das Fahrzeug definiert:
// Leere Zustandstabelle, Daten kommen mit Hilfe von Funk/IR von Parkplatzinfrastruktur
course_state_t state_table[] = {
- {1, 0.00, 0.00, 0, 0, 0}, // i = 0 (erster Abschnitt)
- {2, 0.00, 0.00, 2, 0, 0}, // 0 = keine Seite (für Korrektur verfuegbar)
- {3, 0.00, 0.00, 1, 0, 0}, // 1 = rechte Seite
- {4, 0.00, 0.00, 1, 0, 0}, // 2 = linke Seite
- {5, 0.00, 0.00, 1, 0, 0} //Parken 1: vorwaerts , 2: rueckwaerts , 3: seitlich
+ {1, 0.00, 0.00, 0, 0, 0},
+ {2, 0.00, 0.00, 2, 0, 0},
+ {3, 0.00, 0.00, 1, 0, 0},
+ {4, 0.00, 0.00, 1, 0, 0},
+ {5, 0.00, 0.00, 1, 0, 0}
};
const int ABSCHNITTE = sizeof(state_table) / sizeof(course_state_t); //Größe der Tabelle (Anzahl der Abschnitte siehe Parklayout)
int i; //Laufvariable für drive_course und parking
-#endif
\ No newline at end of file
+#endif