From bb1a4df5fb7aebdbbf28e9d82abd64ac883d1032 Mon Sep 17 00:00:00 2001 From: UlmerMan Date: Wed, 15 Feb 2023 15:42:59 +0100 Subject: [PATCH] added controler --- src/Controler.cpp | 43 +++++++++++++++++ src/{movement.h => Controler.h} | 0 src/Elegoo.cpp | 76 +++++++++++++++--------------- src/Elegoo.h | 28 ++++++++--- src/Infrared.cpp | 2 +- src/{movement.cpp => Movement.cpp} | 44 +++++++++++++---- src/Movement.h | 1 + src/Sensors.cpp | 14 ++++-- 8 files changed, 150 insertions(+), 58 deletions(-) create mode 100644 src/Controler.cpp rename src/{movement.h => Controler.h} (100%) rename src/{movement.cpp => Movement.cpp} (65%) create mode 100644 src/Movement.h diff --git a/src/Controler.cpp b/src/Controler.cpp new file mode 100644 index 0000000..74abaeb --- /dev/null +++ b/src/Controler.cpp @@ -0,0 +1,43 @@ +#include "Controler.h" + +float i_val, lastDifference; + +controler::controler(int p_factor, float d_factor, float i_factor, int max){ + Kp = p_factor; + Kd = d_factor; + Ki = i_factor; + maxOut = max; +} + +float controler::compute(int is, int should){ + float difference = should - is; + + float d_val = difference - lastDifference; + + float result = (difference * Kp) + (d_val * Kd)+(i_val*Ki); + + lastDifference = difference; + i_val += lastDifference; + + if (result > maxOut){ + result = maxOut; + } + + return result; +} + +void controler::setPFactor(int factor){ + Kp = factor; +} + +void controler::setIFactor(int factor){ + Ki = factor; +} + +void controler::setDFactor(int factor){ + Kd = factor; +} + +void controler::setMax(int max){ + maxOut = max; +} \ No newline at end of file diff --git a/src/movement.h b/src/Controler.h similarity index 100% rename from src/movement.h rename to src/Controler.h diff --git a/src/Elegoo.cpp b/src/Elegoo.cpp index e8a6ab9..66c48f8 100644 --- a/src/Elegoo.cpp +++ b/src/Elegoo.cpp @@ -1,10 +1,8 @@ #include "Elegoo.h" #include -//www.elegoo.com -//2016.09.12 - - +// www.elegoo.com +// 2016.09.12 // The direction of the car's movement // ENA ENB IN1 IN2 IN3 IN4 Description @@ -16,7 +14,6 @@ // HIGH HIGH HIGH HIGH HIGH HIGH Car is stoped // LOW LOW N/A N/A N/A N/A Car is stoped - // Left motor truth table // ENA IN1 IN2 Description // LOW Not Applicable Not Applicable Motor is off @@ -41,39 +38,43 @@ // backward forward Car is turning left // backward backward elegoo is running backwards -//Constructor +// Constructor -elegoo::elegoo(int version) { - if(version == 1){ - in1=9; - in2=8; - in3=7; - in4=6; - ENA=10; - ENB=5; +elegoo::elegoo(int version) +{ + if (version == 1) + { + in1 = 9; + in2 = 8; + in3 = 7; + in4 = 6; + ENA = 10; + ENB = 5; } - if(version == 2){ - in1=6; - in2=7; - in3=8; - in4=9; - ENA=5; - ENB=11; + if (version == 2) + { + in1 = 6; + in2 = 7; + in3 = 8; + in4 = 9; + ENA = 5; + ENB = 11; } - if(version == 3){ - in1=7; - in2=8; - in3=9; - in4=11; - ENA=5; - ENB=6; - } - pinMode(in1,OUTPUT); - pinMode(in2,OUTPUT); - pinMode(in3,OUTPUT); - pinMode(in4,OUTPUT); - pinMode(ENA,OUTPUT); - pinMode(ENB,OUTPUT); + if (version == 3) + { + in1 = 7; + in2 = 8; + in3 = 9; + in4 = 11; + ENA = 5; + ENB = 6; + } + pinMode(in1, OUTPUT); + pinMode(in2, OUTPUT); + pinMode(in3, OUTPUT); + pinMode(in4, OUTPUT); + pinMode(ENA, OUTPUT); + pinMode(ENB, OUTPUT); pinMode(trig, OUTPUT); pinMode(echo, INPUT); pinMode(LED, OUTPUT); @@ -84,6 +85,7 @@ elegoo::elegoo(int version) { Servo libServo; libServo.attach(3); IRbegin(); - //IRrecv irrecv(RECV_PIN); - //irrecv->begin(RECV_PIN, LED); + + // IRrecv irrecv(RECV_PIN); + // irrecv->begin(RECV_PIN, LED); } diff --git a/src/Elegoo.h b/src/Elegoo.h index 73d9b2f..ec804dc 100644 --- a/src/Elegoo.h +++ b/src/Elegoo.h @@ -46,10 +46,6 @@ class elegoo { public: elegoo(int version); //Constructor - /** - This is the functiom to Initialze the Library. - Write the Version Number of your Robot into the brackets. - */ void forward(int speed); void back(int speed); void left(int speed); @@ -77,8 +73,6 @@ class elegoo unsigned long getBT(); char getBTdec(); void delay(unsigned long time); - void remote(); - float controler(float ist, float soll, int p_faktor, float d_faktor, float i_faktor); void IRbegin(); void keepDistance(); void enableKeepDistance(); @@ -89,6 +83,10 @@ class elegoo void enableBTremote(); void disableBTremote(); void BTremote(); + bool forwardDistance(int distance, int speed); + bool forwardDistance(int distance); + void backDistance(int distance, int speed); + void backDistance(int distance); private: Servo *libServo; int in1 = 0; @@ -97,6 +95,24 @@ class elegoo int in4 = 0; int ENA = 0; int ENB = 0; + int keepDistanceValue = 10; +}; + +class controler +{ + public: + controler(int p_factor, float d_factor, float i_factor, int max); + float compute(int is, int should); + void setPFactor(int factor); + void setIFactor(int factor); + void setDFactor(int factor); + void setMax(int max); + private: + int Kp; + int maxOut; + float Kd; + float Ki; + float lastDifference; }; //#if !defined(NO_GLOBAL_INSTANCES) && !defined(NO_GLOBAL_CAR) diff --git a/src/Infrared.cpp b/src/Infrared.cpp index 8f39ea6..99dceef 100644 --- a/src/Infrared.cpp +++ b/src/Infrared.cpp @@ -64,7 +64,7 @@ void elegoo::IRremote(){ void elegoo::IRremoteT(int time){ for(int i = time; i > 0; i--){ - remote(); + IRremote(); delay(1); } } \ No newline at end of file diff --git a/src/movement.cpp b/src/Movement.cpp similarity index 65% rename from src/movement.cpp rename to src/Movement.cpp index c348049..42d7a0e 100644 --- a/src/movement.cpp +++ b/src/Movement.cpp @@ -1,6 +1,8 @@ -#include "movement.h" +#include "Movement.h" -float i_val, lastDifference; +//TODO logging of movement + +controler libPID(0.3, 0.2, 0.0005, 255); /*define forward function*/ void elegoo::forward(int speed){ @@ -84,15 +86,39 @@ void elegoo::rightT(int speed, float time){ stop(); } +//TODO PID + +bool elegoo::forwardDistance(int distance, int speed){ + int startDistance = getDistance(90); + + if (startDistance > (distance + keepDistanceValue)){ + return 0; + } -float elegoo::controler(float ist, float soll, int p_faktor, float d_faktor, float i_faktor){ - float abweichung = soll - ist; + int should = startDistance - distance; + forward(speed); - float d_val = abweichung - lastDifference; + while(getDistance(90)>should){ + elegoo::delay(1); + } + stop(); +} - float ergebnis = (abweichung * p_faktor) + (d_val * d_faktor)+(i_val*i_faktor); +void elegoo::backDistance(int distance, int speed){ + int startDistance = getDistance(90); + int should = startDistance + distance; + + back(speed); + + while(getDistance(90) \ No newline at end of file diff --git a/src/Sensors.cpp b/src/Sensors.cpp index 74f856a..0792af5 100644 --- a/src/Sensors.cpp +++ b/src/Sensors.cpp @@ -3,7 +3,7 @@ bool keepDistanceState = 1; bool IRremoteState = 0; bool BTremoteState = 0; -int keepDistanceValue = 10; +int max = 180; long elegoo::getDistance() { digitalWrite(trig, LOW); @@ -17,12 +17,18 @@ long elegoo::getDistance() { } long elegoo::getDistance(int angle) { + if(angle > max){ + angle = 180; + } libServo->write(angle); return getDistance(); } long elegoo::getDistanceReturn(int angle){ - + int current = libServo->read(); + long val = getDistance(angle); + libServo->write(current); + return val; } int elegoo::getLightR(){ @@ -102,6 +108,4 @@ void elegoo::enableBTremote(){ void elegoo::disableBTremote(){ BTremoteState = 0; -} - -//TODO delay for IRremote \ No newline at end of file +} \ No newline at end of file