Diff: Controller/MachineDirectionController.cpp

Revision:

13:34f7f783ad24

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller/MachineDirectionController.cpp	Fri Feb 05 16:06:44 2016 +0000
@@ -0,0 +1,53 @@
+#include "Controller/MachineDirectionController.h"
+
+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() {
+    timer_velocity_sampling_time=0.01;
+    
+    l_Ki=0.1*timer_velocity_sampling_time;
+}
+
+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;
+    }
+}
+
+void MachineDirectionController::cylic_control() {
+    check_queues();
+    
+    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;
+
+    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;
+    } else if(velocity_set < 0.1) {
+        l_PWM = 1500;
+    }
+    
+    pwmOut->pulsewidth_us(1700);
+    //pwmOut.pulsewidth_us(l_PWM);
+}
\ No newline at end of file