Select Git revision
Robotcar_functions.h
Sven Marian Göbel authored
Robotcar_functions.h 21.24 KiB
/***************************************************
________ ___ _________ ________ _____
|\ __ \|\ \|\___ ___\\ ____\ / __ \
\ \ \|\ /\ \ \|___ \ \_\ \ \___|_|\/_|\ \
\ \ __ \ \ \ \ \ \ \ \_____ \|/ \ \ \
\ \ \|\ \ \ \ \ \ \ \|____|\ \ \ \ \
\ \_______\ \__\ \ \__\ ____\_\ \ \ \__\
\|_______|\|__| \|__| |\_________\ \|__|
\|_________|
Name:
BitS1 v2
Author:
Prof. Kamau
Jannis Kampmann
Timo Altan
IFK TH Köln
Last Modified:
22.12.2023
backward_factor and search_factor needs revisit
Fall 8.1 & 8.2 if and else conditions adapted
MOTOR_IN1 & 2 changed to PIN7
MOTOR_IN3 & 4 changed to PIN8
check_voltage() changed from quantity sensitive to time sensitive (2000ms)
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 = 7;
const int MOTOR_IN2 = 7;
const int MOTOR_PWM_B = 6;
const int MOTOR_IN3 = 8;
const int MOTOR_IN4 = 8;
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 modecounter;
int obstacle_velocity = 0;
int no_reflection = 950;
int divisor = 1000;
int rest = divisor / 2;
int voltagetimer = 0;
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 = 3.0; //Neuer backward_factor: Muss getestet werden
float search_factor = 120000; //Neuer search_facotr: Muss getestet werden
float search_time = 0;
unsigned long time_drive = 0;
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;
}
search_time = search_factor*1/((float)total_velocity);
}
//*************************************************************************************************************
//*****************************************/* 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() //gibt 0 wieder wenn Spannug zu niedrig, 1 wenn Spannung niedrig, 2 wenn Spannung gut
{
float voltage = analogRead(Voltage_pin) * 0.0375;
voltage = voltage + (voltage * 0.08);
if (voltage > voltage_min) //Wenn die gemessene Spannung größer ist, als die mindest Spannung...
{
voltagetimer = millis(); //Speichere die Systemzeit auf die Variable voltagetimer
return 2;
} else if((millis()-voltagetimer)>=2000) //Überprüfe, ob 2 Sekunden lang die Spannung zu niedrig ist
{
if(voltage > voltage_low){
return 1; } else
{
return 0;
}
} 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(Left, turn_velocity);
}
else if(millis() - time_drive < 2*search_time)
{
drive(Right, turn_velocity);
}
else
{
drive(Break, 0);
current_mode = standby_mode;
}
}
else if ((last_direction == 'R')) // Fall 8.2 (Fahrzeug sucht die Linie mit zeitlicher Begrenzung)
{
if (millis() - time_drive < search_time)
{
drive(Right, turn_velocity);
}
else if(millis() - time_drive < 2*search_time)
{
drive(Left, turn_velocity);
}
else
{
drive(Break, 0);
current_mode = standby_mode;
}
}
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;
}
}
}
}
}
};