Diff: Controller/MachineDirectionController.cpp

Revision:

18:7b1787f740d9

Parent:

17:76636aaf80de

--- a/Controller/MachineDirectionController.cpp	Mon Feb 08 14:56:34 2016 +0000
+++ b/Controller/MachineDirectionController.cpp	Tue Feb 09 05:03:23 2016 +0000
@@ -1,60 +1,65 @@
 #include "Controller/MachineDirectionController.h"
 
-MachineDirectionController::MachineDirectionController(PwmOut *pwmOut, Queue<float, 2> *machine_direction_queue, Queue<float, 2> *imu_queue_velocity) {
+MachineDirectionController::MachineDirectionController(PwmOut *pwmOut, Queue<float, 2> *machine_direction_queue, Queue<float, 2> *imu_queue_velocity)
+{
     this->pwmOut = pwmOut;
     this->machine_direction_queue = machine_direction_queue;
     this->imu_queue_velocity = imu_queue_velocity;
     init();
 }
 
-void MachineDirectionController::init() {
+void MachineDirectionController::init()
+{
     timer_velocity_sampling_time=0.01;
     max_velocity = 2.0;
     l_Ki=0.1*timer_velocity_sampling_time;
 }
 
-void MachineDirectionController::check_queues() {
+void MachineDirectionController::check_queues()
+{
     velocity_set_event = machine_direction_queue->get(0);
     if (velocity_set_event.status == osEventMessage) {
         velocity_set = *(float*)velocity_set_event.value.p;
     }
-    
-   /* velocity_current_event = imu_queue_velocity->get(0);
-    if (velocity_current_event.status == osEventMessage) {
-        velocity_current = *(float *)velocity_current_event.value.p;
-    }*/
+
+    /* velocity_current_event = imu_queue_velocity->get(0);
+     if (velocity_current_event.status == osEventMessage) {
+         velocity_current = *(float *)velocity_current_event.value.p;
+     }*/
 }
 
-void MachineDirectionController::cylic_control() {
+void MachineDirectionController::cylic_control()
+{
     check_queues();
-    if( velocity_set < 0.1) {
-        velocity_set = velocity_set_alt;
-    } else if(velocity_set > max_velocity){
-        velocity_set = max_velocity;
+    if( velocity_set < 0.01) {
+        l_PWM = 1500;
+    } else {
+        if(velocity_set > max_velocity) {
+            velocity_set = max_velocity;
+        }
+        Vorsteuerung=88.316*velocity_set+/*175.17*/200;
+        
+        l_PWM=1500+Vorsteuerung;  // nur für die Vorsteuerung alleine!
+        
+        //l_e = velocity_set-velocity_current;
+        //l_esum = l_esum + l_e;
+
+        //PI_Regler =l_Kp*l_e+l_Ki * l_esum;
+
+        //l_output=Vorsteuerung+PI_Regler;
+
+
+        //l_PWM = 1500+l_output;
+        if(l_PWM<1500) {
+            l_PWM = 1500;
+            l_esum = l_esum-2*l_e;
+        } else if(l_PWM>2000) {
+            l_PWM = 2000;
+            l_esum = l_esum-2*l_e;
+        }
     }
     
-    velocity_set = 0.5;
+    serialMinnow.printf("%f\r\n", l_PWM);
     
-    Vorsteuerung=88.316*velocity_set+175.17;
-    //l_e = velocity_set-velocity_current;
-    //l_esum = l_esum + l_e;
-
-    //PI_Regler =l_Kp*l_e+l_Ki * l_esum;
-
-    //l_output=Vorsteuerung+PI_Regler;
-
-
-    //l_PWM = 1500+l_output;
-    l_PWM=1500+Vorsteuerung;
-    if(l_PWM<1500) {
-        l_PWM = 1500;
-        l_esum = l_esum-2*l_e;
-    } else if(l_PWM>2000) {
-        l_PWM = 2000;
-        l_esum = l_esum-2*l_e;
-    }
-    velocity_set_alt=velocity_set;
-    
-    //pwmOut->pulsewidth_us(1700);
     pwmOut->pulsewidth_us(l_PWM);
 }
\ No newline at end of file