Diff: source/Movement.cpp

Revision:

34:40d8d29b44b8

Parent:

31:51f52ffa4b51

Child:

39:92723f7ea54f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/Movement.cpp	Wed Mar 29 12:10:32 2017 +0000
@@ -0,0 +1,135 @@
+/**
+ * Movement function library
+ * Handels Movement of the Robot
+**/
+/*
+
+#include "mbed.h"
+#include "movement.h"
+#include "EncoderCounter.h"
+
+static double time_counter = 0.0f;
+static double timer0 = 0.0f;
+static float PID_correction_value = 1.0f;
+
+static float power_value_slow = 0.6f;
+static float power_value_medium = 0.7f;
+static float power_value_fast = 0.8f;
+static float ludicrous_value = 1.0f;
+
+//Motor Encoders
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter counterRight(PA_6, PC_7);
+
+//motor stuff
+DigitalOut enableMotorDriver(PB_2);
+PwmOut pwmL(PA_8);
+PwmOut pwmR(PA_9);
+DigitalIn motorDriverFault(PB_14);
+DigitalIn motorDriverWarning(PB_15);
+
+//DigitalOut led(LED1); // Board LED
+
+DigitalOut led(LED1); // Board LED
+
+
+// ******************************************************************************
+
+void move_init()
+{
+    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
+    pwmR.period(0.00005f);
+
+    pwmL.write(0.5f); // Setzt die Duty-Cycle auf 50%
+    pwmR.write(0.5f);
+    enableMotorDriver = 1;
+
+    PID_correction_value = 1.0f;
+    
+}
+// ******************************************************************************
+void move_forward_slow(float correction_value)
+{
+    pwmL.write(power_value_slow);
+    pwmR.write(correction_value);
+    printf("Left: %f || Right: %f    value:%f \r\n",pwmL.read(), pwmR.read(), correction_value);
+
+}
+
+void move_forward_medium(float correction_value)
+{
+    pwmL = power_value_medium;
+    pwmR = 1-power_value_medium*correction_value;
+}
+
+void move_backward_slow(float correction_value)
+{
+    pwmL = 1-power_value_slow*correction_value;
+    pwmR = power_value_slow;
+}
+
+void move_backward_medium(float correction_value)
+{
+    pwmL = 1-power_value_slow*correction_value;
+    pwmR = power_value_slow;
+
+}
+// ******************************************************************************
+
+void stop_movement()
+{
+    pwmL = 0.5f;
+    pwmR = 0.5f;
+    counterLeft.reset();
+    counterRight.reset();
+}
+
+void sync_movement(bool speed, bool direction)
+{
+ if(counterLeft.read() > 30000 || -counterRight > 30000){
+     
+     }
+ printf("Left: %d || Right: %d\r\n",counterLeft.read(), -counterRight.read());
+    if(counterLeft.read() > -counterRight.read()) {
+        PID_correction_value += 0.001f;
+    } else {
+        if(counterLeft.read() < -counterRight.read()) {
+            PID_correction_value -= 0.001f;
+        } else {
+            // even 
+        }
+    }
+
+    if(PID_correction_value < 0.0f) {
+        PID_correction_value = 0;
+    }
+    if(PID_correction_value > 2.0f) {
+        PID_correction_value = 2;
+    }
+
+// Call movement:
+// direction 0 = backward, direction 1 = forward
+// speed 0 = slow, speed 1 = medium
+
+    if(direction && speed) {
+        move_forward_medium(PID_correction_value);
+    }
+    if(direction && !speed) {
+        float value = 1.0f-power_value_slow*PID_correction_value;
+        move_forward_slow(value);
+    }
+    if(!direction && speed) {
+        move_backward_medium(PID_correction_value);
+    }
+    if(!direction && !speed) {
+        move_backward_slow(PID_correction_value);
+    }
+}
+
+void terminate_movement()
+{
+    PID_correction_value = 1.0f;
+    pwmL.write(0.5f);
+    pwmR.write(0.5f);
+}
+*/