Skip to content
Snippets Groups Projects
Commit 3db40d5f authored by Jannis Nicolas Kampmann's avatar Jannis Nicolas Kampmann
Browse files

BITS1

parents
Branches main
No related tags found
No related merge requests found
/***************************************************
________ ___ _________ ________ _____
|\ __ \|\ \|\___ ___\\ ____\ / __ \
\ \ \|\ /\ \ \|___ \ \_\ \ \___|_|\/_|\ \
\ \ __ \ \ \ \ \ \ \ \_____ \|/ \ \ \
\ \ \|\ \ \ \ \ \ \ \|____|\ \ \ \ \
\ \_______\ \__\ \ \__\ ____\_\ \ \ \__\
\|_______|\|__| \|__| |\_________\ \|__|
\|_________|
Name:
BitS1 v2
Author:
Prof. Kamau
Jannis Kampmann
IFK TH Köln
**************************************************/
#include <FastLED.h>
#include <thk_imu.h>
#include <thk_motor_driver.h>
#include <thk_motor_driver_tb6612fng.h>
#include "Robotcar_functions.h"
Robotcar_functions Robotcar_functions_object;
//Definieren von Variablen
float turn_factor = 1.0;
const float distance_one = 0;
const float distance_two = 0;
int acceleration_increment = 0;
int total_velocity = 0;
int white_surface_reflection = 150;
void setup()
{ //Innerhalb dieser Funktion wird alles einmal bei Start des Arduinos aufgerufen
Robotcar_functions_object.initialise();
search_time = 1.5 * total_velocity;
}
void loop()
{ //Alles innerhalb dieser Funktion wird so lange wiederholt, wie der Arduino eingeschaltet ist
Robotcar_functions_object.serial_sensor_data_output();
Robotcar_functions_object.mode_selection();
Robotcar_functions_object.check_standby();
Robotcar_functions_object.car_on_ground();
Robotcar_functions_object.check_voltage();
Robotcar_functions_object.obstacle_avoidance();
Robotcar_functions_object.line_follow();
}
# Base Packages
Code und Bibliotheken für das Bits1 Fahrzeug
# Beschreibung
"Robotcar-BitS1"-Code angepasst auf thk-Bibliotheken.
Bibliotheken "DeviceDriverSet_xxx0" und "MPU6050_getdata" nicht mehr notwendig.
# Vorraussetzung
- Bits1 Fahrzeug
- [thk-BITS1-Bibliotheken](https://git-ce.rwth-aachen.de/bits_libs/bits1-libraries) Bibliothek
# Installation
1. Kopiere die Bibliotheken aus base-package in den Arduino libraries Ordner.
2. Umbennen der .ino Datei analog zu dem des Verzeichnisses.
3. Hochladen der .ino Datei auf das Arduino Uno Board des Bits1 Fahrzeugs.
# Anwendung
1. Bits Fahrzeug einschalten
2. Fahrzeug auf der Teststrecke platzieren
3. Fahrmodi wählen: LED Fahrmodi: grün = Line Follow, blau = Obstacle Avoidance
# Anmerkungen
- Bibliotheks Ordnerstruktur: Arduino\libaries
- Programm Ordnerstruktur: Arduino\Robotcar-BitS1
**Achtet darauf, dass der Ordner in dem sich die .ino Datei befindet den selben Namen hat wie die .ino Datei**
- Das Fahrzeug stoppt wenn es angehoben wird.
/***************************************************
________ ___ _________ ________ _____
|\ __ \|\ \|\___ ___\\ ____\ / __ \
\ \ \|\ /\ \ \|___ \ \_\ \ \___|_|\/_|\ \
\ \ __ \ \ \ \ \ \ \ \_____ \|/ \ \ \
\ \ \|\ \ \ \ \ \ \ \|____|\ \ \ \ \
\ \_______\ \__\ \ \__\ ____\_\ \ \ \__\
\|_______|\|__| \|__| |\_________\ \|__|
\|_________|
Name:
BitS1 v2
Author:
Prof. Kamau
Jannis Kampmann
Timo Altan
IFK TH Köln
Last Modified:
21.08.2023
Switch from libraries "DeviceDriverSet_xxx0" to "thk-motor-driver-v2.0" & "MPU6050_getdata" to "thk_imu-v2.0"
08 September 2022
improvement outline / format
15 Juli 2022
Recoding all functions
**************************************************/
#define Key_pin 2
#define RGBLed_pin 4
#define Echo_pin 12
#define Trig_pin 13
#define OpticSensorR A0
#define OpticSensorM A1
#define OpticSensorL A2
#define Voltage_pin A3
#define NUM_LEDS 60
const int MOTOR_COUNT = 1;
const int MOTOR_STBY = 3;
const int MOTOR_PWM_A = 5;
const int MOTOR_IN1 = 8;
const int MOTOR_IN2 = 8;
const int MOTOR_PWM_B = 6;
const int MOTOR_IN3 = 7;
const int MOTOR_IN4 = 7;
thk_MotorDriverTB6612FNG motorR(MOTOR_COUNT, MOTOR_STBY, MOTOR_PWM_A, MOTOR_IN1, MOTOR_IN2, 0, 0, 0);
thk_MotorDriverTB6612FNG motorL(MOTOR_COUNT, MOTOR_STBY, MOTOR_PWM_B, MOTOR_IN3, MOTOR_IN4, 0, 0, 0);
const int INTERRUPT_PIN = 0; // Wenn kein Interrupt Pin angeschlossen ist, dann Interrupt Pin gleich 0 setzen.
thk_IMU imu(INTERRUPT_PIN);
extern unsigned long start_time = 0;
char commandline_input = 'E';
char last_direction = 'D';
int commandline_output = 3;
int not_enough_voltage_counter = 0;
int low_voltage_counter = 0;
int modecounter;
int obstacle_velocity = 0;
int no_reflection = 950;
int divisor = 1000;
int rest = divisor / 2;
boolean mode_changed = false;
float voltage_low = 7.2;
float voltage_min = 7.0;
float yaw = 0;
float pitch = 0;
float roll = 0;
float backward_factor = 5.0;
unsigned long time_drive = 0;
float search_time = 200;
float line_factor = 1.2;
CRGB leds[NUM_LEDS];
extern float turn_factor; //Übergabe: Geschwindigkeitsreduktion bei Kurvenfahrt
extern const float distance_one; //Übergabe: Muss größer als distance_two sein
extern const float distance_two; //Übergabe: Muss größer als 0 sein
extern int acceleration_increment; //Übergabe: Beschleunigung für Obstacle Modus
extern int total_velocity; //Übergabe: Geschwindigkeit; Werte von 0 bis 255 zulässig
extern int white_surface_reflection; //Übergabe: Wert muss für jeden Untergrund angepasst werden; Hoher Wert = Hohe reflection
enum direction //Definieren der möglichen Fahrtrichtungen
{
Forward,
Left,
Right,
Leftforward,
Rightforward,
Backward,
Leftbackward,
Rightbackward,
Coast,
Break
};
enum modi //Definieren des Datentyps "modi"
{
line_follow_mode,
obstacle_mode,
standby_mode
};
modi current_mode; //Variable "current_mode" vom Datentyp "modi" wird deklariert
class Robotcar_functions
{
public:
//*************************************************************************************************************
//*****************************************/* Car Initialization */********************************************
//*************************************************************************************************************
void initialise()
{
FastLED.addLeds<NEOPIXEL, RGBLed_pin>(leds, NUM_LEDS);
FastLED.setBrightness(25);
current_mode = standby_mode;
Serial.begin(19200);
pinMode(Echo_pin, INPUT);
pinMode(Trig_pin, OUTPUT);
pinMode(OpticSensorL, INPUT);
pinMode(OpticSensorM, INPUT);
pinMode(OpticSensorR, INPUT);
pinMode(Key_pin, INPUT);
pinMode(Voltage_pin, INPUT);
pinMode(4, OUTPUT);
imu.init();
motorR.init();
motorL.init();
if (total_velocity > 255)
{
total_velocity = 255;
}
if(turn_factor == 0)
{
turn_factor = 1;
}
if(turn_factor < 0)
{
turn_factor = turn_factor * -1.0;
}
}
//*************************************************************************************************************
//*****************************************/* Distance function */*********************************************
//*************************************************************************************************************
int get_distance()
{
unsigned long duration = 0;
int distance = 0;
digitalWrite(Trig_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig_pin, LOW);
duration = pulseIn(Echo_pin, HIGH);
distance = duration / 58;
if ( distance > 150 )
{
distance = 150;
}
return distance;
}
//*************************************************************************************************************
//*****************************************/* Brightness function */*******************************************
//*************************************************************************************************************
int get_brightness(char LMR)
{
switch (LMR)
{
case 'L':
return analogRead(OpticSensorL);
break;
case 'M':
return analogRead(OpticSensorM);
break;
case 'R':
return analogRead(OpticSensorR);
break;
default:
return 0;
break;
}
}
//*************************************************************************************************************
//*****************************************/* Serial Data Output */********************************************
//*************************************************************************************************************
void serial_sensor_data_output()
{
if (current_mode == standby_mode)
{
switch (commandline_input)
{
case 'D':
commandline_output = 0;
break;
case 'H':
commandline_output = 1;
break;
case 'W':
commandline_output = 2;
break;
case 'E':
commandline_output = 3;
Serial.println("");
Serial.println("Ausgabe der Sensorendaten!");
Serial.println("---------------------------------------------");
Serial.println("Eingabe 'D' für die Distanz!");
Serial.println("Eingabe 'H' für die Helligkeit!");
Serial.println("Eingabe 'W' für die Winkel!");
Serial.println("Eingabe 'E' um die Ausgabe zu Stoppen!");
Serial.println("");
Serial.flush();
break;
default:
break;
}
switch (commandline_output)
{
case 0:
Serial.print("Distanz: ");
Serial.print(get_distance());
Serial.println("");
break;
case 1:
Serial.print("Links: ");
Serial.print(get_brightness('L'));
Serial.print("\t");
Serial.print("Mitte: ");
Serial.print(get_brightness('M'));
Serial.print("\t");
Serial.print("Rechts: ");
Serial.print(get_brightness('R'));
Serial.println("");
break;
case 2:
Serial.print("Gieren: ");
Serial.print(imu.get_z_rot());
Serial.print("\t");
Serial.print("Nicken: ");
Serial.print(imu.get_y_rot());
Serial.print("\t");
Serial.print("Wanken: ");
Serial.print(imu.get_x_rot());
Serial.println("");
break;
default:
break;
}
commandline_input = Serial.read();
}
}
//*************************************************************************************************************
//*****************************************/* Car Drive Functions */*******************************************
//*************************************************************************************************************
void drive ( direction a , int velocity)
{
digitalWrite(3, HIGH);
switch (a)
{
case Forward:
motorR.drive(velocity, 1);
motorL.drive(velocity, 1);
break;
case Left:
motorR.drive(velocity, 1); // 1 = Vorwärts
motorL.drive(velocity, 0); // 0 = Rückwärts
break;
case Right:
motorR.drive(velocity, 0);
motorL.drive(velocity, 1);
break;
case Leftforward:
motorR.drive(velocity, 1);
motorL.drive(0,1);
break;
case Rightforward:
motorR.drive(0,1);
motorL.drive(velocity, 1);
break;
case Backward:
motorR.drive(velocity, 0);
motorL.drive(velocity, 0);
leds[0] = CRGB::Yellow;
FastLED.show();
break;
case Leftbackward:
motorR.drive(velocity, 0);
motorL.drive(0,0);
break;
case Rightbackward:
motorR.drive(0,0);
motorL.drive(velocity, 0);
break;
case Coast:
digitalWrite(MOTOR_STBY, LOW);
break;
case Break:
motorR.drive(0, 0);
motorL.drive(0, 0);
break;
default:
break;
}
return;
}
//*************************************************************************************************************
//****************************************/* Function Mode Select */*******************************************
//*************************************************************************************************************
void mode_selection()
{
if (digitalRead(Key_pin) == 0)
{
if (!mode_changed)
{
mode_changed = true;
modecounter++;
if (modecounter > 3)
{
modecounter = 0;
}
switch (modecounter)
{
case 0:
current_mode = standby_mode;
break;
case 1:
if (check_voltage() >= 1) {
current_mode = line_follow_mode;
}
start_time = millis();
break;
case 2:
current_mode = standby_mode;
break;
case 3:
if (check_voltage() >= 1) {
current_mode = obstacle_mode;
}
start_time = millis();
break;
default:
current_mode = standby_mode;
break;
}
}
}
else
{
mode_changed = false;
}
}
//*************************************************************************************************************
//*************************************/* Check Mode / Battery Level */****************************************
//*************************************************************************************************************
void check_standby()
{
if (current_mode == standby_mode)
{
drive(Break, 0);
if (check_voltage() == 2)
{
leds[0] = CRGB::White;
FastLED.show();
} else if (check_voltage() == 1) {
if ((millis() % divisor) < rest) {
leds[0] = CRGB::White;
FastLED.show();
} else {
leds[0] = CRGB::Red;
FastLED.show();
}
}
obstacle_velocity = 0;
start_time = millis();
last_direction = 'D';
}
}
int check_voltage()
{
float voltage = analogRead(Voltage_pin) * 0.0375;
voltage = voltage + (voltage * 0.08);
if (voltage < voltage_min)
{
not_enough_voltage_counter++;
if (not_enough_voltage_counter >= 500)
{
current_mode = standby_mode;
leds[0] = CRGB::Red;
FastLED.show();
}
}
if (voltage < voltage_low)
{
low_voltage_counter++;
}
if (not_enough_voltage_counter >= 500)
{
return 0;
} else if (not_enough_voltage_counter < 499 && low_voltage_counter >= 500) {
return 1;
} else {
return 2;
}
}
//*************************************************************************************************************
//***************************************/* Car on the ground? */**********************************************
//*************************************************************************************************************
boolean car_on_ground()
{
if ((get_brightness('L') > no_reflection) && (get_brightness('M') > no_reflection) && (get_brightness('R') > no_reflection)) {
current_mode = standby_mode;
return false;
} else {
return true;
}
}
//*************************************************************************************************************
//*************************************/* Obstacle Avoidance */************************************************
//*************************************************************************************************************
void obstacle_avoidance()
{
if (current_mode == obstacle_mode && check_voltage() >= 1)
{
if (check_voltage() == 2) {
leds[0] = CRGB::Blue;
FastLED.show();
}
else {
if ((millis() % divisor) < rest) {
leds[0] = CRGB::Blue;
FastLED.show();
} else {
leds[0] = CRGB::Red;
FastLED.show();
}
}
if ((millis() - start_time) > 5000)
{
float distance = 0;
distance = get_distance();
if ( distance > distance_one)
{
obstacle_velocity = obstacle_velocity + acceleration_increment;
if (obstacle_velocity > total_velocity)
{
obstacle_velocity = total_velocity;
}
drive(Forward, obstacle_velocity);
}
else if ((distance < distance_one) && (distance > distance_two))
{
obstacle_velocity = total_velocity * (distance / distance_one);
drive(Forward, obstacle_velocity);
}
else if (distance < distance_two)
{
drive(Break, 0);
current_mode = standby_mode;
}
}
}
}
//*************************************************************************************************************
//****************************************/* Car Line Follow */************************************************
//*************************************************************************************************************
void line_follow()
{
int distance;
if (current_mode == line_follow_mode && check_voltage() >= 1)
{
if (check_voltage() == 2) {
leds[0] = CRGB::Green;
FastLED.show();
}
else
{
if ((millis() % divisor) < rest)
{
leds[0] = CRGB::Green;
FastLED.show();
}
else
{
leds[0] = CRGB::Red;
FastLED.show();
}
}
if ((millis() - start_time) > 5000)
{
distance = get_distance();
if ((car_on_ground() == false) || (distance < 5))
{
current_mode = standby_mode;
return;
}
int optic_Robotcar_functions_object[3];
optic_Robotcar_functions_object[0] = get_brightness('L');
optic_Robotcar_functions_object[1] = get_brightness('M');
optic_Robotcar_functions_object[2] = get_brightness('R');
int turn_velocity = total_velocity / turn_factor; //lokale Variable zur berechnung der kurvengeschwindigkeit wird deklariert
if (turn_velocity > 255)
{
turn_velocity = 255;
}
if ((optic_Robotcar_functions_object[0] >= white_surface_reflection) && (optic_Robotcar_functions_object[1] >= white_surface_reflection) && (optic_Robotcar_functions_object[2] >= white_surface_reflection)) // Fall 1
{
drive(Forward, (total_velocity / line_factor));
last_direction = 'F';
time_drive = millis();
}
else if ((optic_Robotcar_functions_object[0] > white_surface_reflection) && (optic_Robotcar_functions_object[1] < white_surface_reflection) && (optic_Robotcar_functions_object[2] < white_surface_reflection)) // Fall 2
{
drive(Left, turn_velocity);
last_direction = 'L';
time_drive = millis();
}
else if ((optic_Robotcar_functions_object[0] < white_surface_reflection) && (optic_Robotcar_functions_object[1] < white_surface_reflection) && (optic_Robotcar_functions_object[2] > white_surface_reflection)) // Fall 3
{
drive(Right, turn_velocity);
last_direction = 'R';
time_drive = millis();
}
else if ((optic_Robotcar_functions_object[0] > white_surface_reflection) && (optic_Robotcar_functions_object[1] > white_surface_reflection) && (optic_Robotcar_functions_object[2] < white_surface_reflection)) // Fall 4
{
drive(Leftforward, turn_velocity);
last_direction = 'L';
time_drive = millis();
}
else if ((optic_Robotcar_functions_object[0] < white_surface_reflection) && (optic_Robotcar_functions_object[1] > white_surface_reflection) && (optic_Robotcar_functions_object[2] > white_surface_reflection)) // Fall 5
{
drive(Rightforward, turn_velocity);
last_direction = 'R';
time_drive = millis();
}
else if ((optic_Robotcar_functions_object[0] < white_surface_reflection) && (optic_Robotcar_functions_object[1] > white_surface_reflection) && (optic_Robotcar_functions_object[2] < white_surface_reflection)) // Fall 6
{
drive(Forward, turn_velocity);
last_direction = 'F';
time_drive = millis();
}
else if ((optic_Robotcar_functions_object[0] > white_surface_reflection) && (optic_Robotcar_functions_object[1] < white_surface_reflection) && (optic_Robotcar_functions_object[2] > white_surface_reflection)) // Fall 7
{
drive(Forward, turn_velocity);
last_direction = 'F';
time_drive = millis();
}
else if ((optic_Robotcar_functions_object[0] < white_surface_reflection) && (optic_Robotcar_functions_object[1] < white_surface_reflection) && (optic_Robotcar_functions_object[2] < white_surface_reflection)) // Fall 8
{
if ((last_direction == 'L')) // Fall 8.1 (Fahrzeug sucht die Linie mit zeitlicher Begrenzung)
{
if (millis() - time_drive >= search_time)
{
drive(Right, turn_velocity);
}
else
{
drive(Left, turn_velocity);
}
}
else if ((last_direction == 'R')) // Fall 8.2 (Fahrzeug sucht die Linie mit zeitlicher Begrenzung)
{
if (millis() - time_drive >= search_time)
{
drive(Left, turn_velocity);
}
else
{
drive(Right, turn_velocity);
}
}
else if (last_direction == 'F') // Fall 8.3 (Fahrzeug fährt Rückwärts bis wieder eine Linie erkannt wird)
{
drive(Break, 0);
delay(2000);
while ((get_brightness('L') < white_surface_reflection) && (get_brightness('M') < white_surface_reflection) && (get_brightness('R') < white_surface_reflection))
{
drive(Backward, (total_velocity / backward_factor));
}
time_drive = millis();
}
else if (last_direction == 'D') // Fall 8.4 (Fahrzeug wird zurück in den Stand-By versetzt, da es nicht auf die Linie gesetzt wurde)
{
current_mode = standby_mode;
return;
}
}
}
}
}
};
#!/bin/bash
RED='\033[0;31m'
GREEN='\033[0;32m'
NOCOLOR='\033[0m'
bold=$(tput bold)
normal=$(tput sgr0)
echo -e "$GREEN"
echo -e "Automatischer download der Bibliotheken wird vorbereitet..."
sleep 2
echo -e "$NOCOLOR"
mkdir BITS1_Bibliotheken
echo -e "$GREEN"
echo -e "Ordner BITS1_Bibliotheken wurde erfolgreich erstellt."
echo -e " "
sleep 1
cd BITS1_Bibliotheken
echo -e "Starte download..."
sleep 1
echo -e "$NOCOLOR"
# BITS-i Bibliotheken
git clone https://git-ce.rwth-aachen.de/bits_libs/bits1-libraries.git
echo -e "$GREEN"
echo -e "Alle Bibliotheken wurden erfolgreich "
echo -e "in den Ordner BITS-i_Bibliotheken gedownloadet."
echo -e "$RED"
echo -e "__________________________________________________________________"
echo -e "__________________________________________________________________"
echo -e " "
echo -e "\033[31;7m${bold} !!!WICHTIG!!! \033[0m"
echo -e " "
echo -e "\033[31;7m${bold}Bitte kopiert alle Ordner aus BITS1_Bibliotheken in den\033[0m"
echo -e "\033[31;7m${bold}Arduino Libraries Ordner!\033[0m"
echo -e "$RED"
echo -e "__________________________________________________________________"
echo -e "__________________________________________________________________"
echo -e " "
echo -e "$GREEN"
echo -e "Wenn die Bibliotheken erfolgreich in den Arduino Libraries Ordner"
echo -n "kopiert wurden,"
read -p " mit beliebiger Taste fortfahren" x
echo -e " "
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment