Revision 3:a292bdaf03f6, committed 2021-03-16

Comitter:

boro

Date:

Tue Mar 16 17:28:04 2021 +0100

Parent:

2:87ca9b0c9992

Commit message:

controller updated

Changed in this revision

--- a/Controller.cpp	Sat Mar 13 11:06:23 2021 +0100
+++ b/Controller.cpp	Tue Mar 16 17:28:04 2021 +0100
@@ -6,7 +6,7 @@
 const float Controller::COUNTS_PER_TURN = 1200.0f;         // encoder resolution
 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
 const float Controller::KN = 40.0f;                         // speed constant in [rpm/V]
-const float Controller::KP = 0.2f;                          // speed control parameter
+const float Controller::KP = 0.15f;                          // speed control parameter
 const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
 const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
 const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
@@ -14,7 +14,7 @@
 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight,
                         EncoderCounter& counterLeft, EncoderCounter& counterRight) :
                         pwmLeft(pwmLeft), pwmRight(pwmRight),
-                        counterLeft(counterLeft), counterRight(counterRight) {
+                        counterLeft(counterLeft), counterRight(counterRight), thread(osPriorityHigh, STACK_SIZE) {
 
     // Initialisieren der PWM Ausgaenge
     

--- a/Controller.h	Sat Mar 13 11:06:23 2021 +0100
+++ b/Controller.h	Tue Mar 16 17:28:04 2021 +0100
@@ -22,6 +22,7 @@
 
 private:
 
+    static const uint32_t   STACK_SIZE = 4096;  // stack size of thread, given in [bytes]
     static const float PERIOD;
     static const float COUNTS_PER_TURN;
     static const float LOWPASS_FILTER_FREQUENCY;

--- a/main.cpp	Sat Mar 13 11:06:23 2021 +0100
+++ b/main.cpp	Tue Mar 16 17:28:04 2021 +0100
@@ -12,10 +12,6 @@
 #include "Controller.h"
 
 
-// Blinking rate in milliseconds
-#define BLINKING_RATE_MS                                                    500
-
-
 int main()
 {
     
@@ -32,7 +28,7 @@
     EncoderCounter counter3(PA_0, PA_1);
     
     // create controller
-    Controller controller(pwm_motor1, pwm_motor2, counter1, counter1);
+    Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);
     
     DigitalOut enable(PB_15);
     
@@ -49,12 +45,12 @@
     DigitalOut myled(LED1);
     
     
-    
+    /*
     // initialise PWM
     pwm_motor1.period(0.00005f);// 0.05ms 20KHz
     pwm_motor1.write(0.5f);
     pwm_motor2.period(0.00005f);// 0.05ms 20KHz
-    pwm_motor2.write(0.5f);
+    pwm_motor2.write(0.5f);*/
     pwm_motor3.period(0.00005f);// 0.05ms 20KHz
     pwm_motor3.write(0.5f);
     
@@ -85,7 +81,7 @@
     
     // enable driver DC motors
     enable = 1;
-
+    
     while (true) {
         
         if(!user_button) {
@@ -93,11 +89,12 @@
             myled = 0;
             controller.setDesiredSpeedLeft(50.0f);
             controller.setDesiredSpeedRight(50.0f);
-            pwm_motor3.write(0.9f);
+            pwm_motor3.write(0.7f);
+            
 
-            S0.SetPosition(1200);
-            S1.SetPosition(1200);
-            S2.SetPosition(1200);
+            S0.SetPosition(1000);
+            S1.SetPosition(1000);
+            S2.SetPosition(1000);
 
         } else {
             // LED on, reset controller speed, pwm2, position servo
@@ -106,14 +103,16 @@
             controller.setDesiredSpeedRight(0.0f);
             pwm_motor3.write(0.5f);
 
-            S0.SetPosition(1900);
-            S1.SetPosition(1900);
-            S2.SetPosition(1900);
+            S0.SetPosition(1500);
+            S1.SetPosition(1500);
+            S2.SetPosition(1500);
 
         }
+
         
-        printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
+        printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
+        //printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
         
-        thread_sleep_for(BLINKING_RATE_MS);
+        thread_sleep_for(200);
     }
 }