added controler

UlmerMan · Feb 15, 2023 · bb1a4df · bb1a4df
1 parent 1e054ae
commit bb1a4df
Show file tree

Hide file tree

Showing 8 changed files with 150 additions and 58 deletions.
diff --git a/src/Controler.cpp 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;
+}
diff --git a/src/movement.h → src/Controler.h b/src/movement.h → src/Controler.h
diff --git a/src/Elegoo.cpp b/src/Elegoo.cpp
@@ -1,10 +1,8 @@
 #include "Elegoo.h"
 #include <Servo.h>
 
-//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
@@ -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
@@ -64,7 +64,7 @@ void elegoo::IRremote(){
 
 void elegoo::IRremoteT(int time){
   for(int i = time; i > 0; i--){
-    remote();
+    IRremote();
     delay(1);
   }
 }
diff --git a/src/movement.cpp → src/Movement.cpp b/src/movement.cpp → 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)<should);
+  stop();
+}
+
+bool elegoo::forwardDistance(int distance){
+  bool val = elegoo::forwardDistance(distance, 255);
+  return val;
+}
 
-  lastDifference = abweichung;
-  i_val += lastDifference;
-  return ergebnis;
+void elegoo::backDistance(int distance){
+  elegoo::backDistance(distance, 255);
 }
diff --git a/src/Movement.h b/src/Movement.h
@@ -0,0 +1 @@
+#include <Elegoo.h>
diff --git 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
+}