Diff: bertl14.cpp

Revision:

1:6af83dcf6691

Parent:

0:04250620f3d9

diff -r 04250620f3d9 -r 6af83dcf6691 bertl14.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bertl14.cpp	Mon Jan 25 11:37:16 2016 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "Pwm.h"
+// Philip Pollheimer, 23.12.2015
+// Controlling the engines of the BERTL14 using PWM
+
+// Declaration of outputs
+
+DigitalOut ML_R(P1_0);      //  IN1 EP10, MG1A => MG1 Motor-Pin 2
+DigitalOut ML_F(P1_1);      //  IN2 EP10, MG1B => MG1 Motor-Pin 1
+PwmOut ML_EN(P1_15);        //  EN1, P34
+
+DigitalOut MR_F(P1_3);      // IN4 EP13, MG2A => MG2 Motor-Pin 2
+DigitalOut MR_R(P1_4);      // IN3 EP14, MG2B => MG2 Motor-Pin 1
+PwmOut MR_EN(P0_21);        // EN2, P36
+
+
+void bertl_motor(int links, int rechts)     // subprogramm
+{
+    float v_motor_rechts;                   // v => velocity
+    float v_motor_links;
+    
+    // if-conditions when parameters are bigger than 0 then the first formula will be used.
+    // otherwise the otherone. Thats because if the parameter is negative the value will also be negative. 
+    // Therefore we write -255, the minus becomes a plus (-(-5) => +5) 
+    
+    if(links > 0) {    
+        v_motor_links = links / 255.0;            // Formula for the PWM. The variable v_motor_[name] is always < 1, due to the PWM function.
+    }   
+    if(rechts > 0){
+        v_motor_rechts = rechts / 255.0;                                  
+    }                                          // In PWM 1 equals 5V and 0 equals 0V. The formula calculates a value from 0 to 1 (which is converted to Volt)
+                                                  // and with this value the engine will be supplied.
+    if(links < 0) {
+        v_motor_links = links / -255.0;
+    }    
+    if(rechts < 0){
+        v_motor_rechts = rechts / -255.0;
+        
+    }                                       
+
+
+    // if-conditions for positive parameters (when the transfered parameters are positive, the engine turns forward)
+    if(links > 0 && rechts < links) {
+        
+        ML_EN = v_motor_links;
+        MR_EN = v_motor_rechts;
+        if(rechts < 0)
+        {
+            ML_F = MR_R = 1;
+            wait(1.0);
+            ML_F = MR_R = 0;             
+        }
+        ML_F = MR_F = 1;
+        wait(4.0);
+        ML_F = MR_F = 0;
+    }
+    if(rechts > 0 && rechts > links) {
+
+        ML_EN = v_motor_links;
+        MR_EN = v_motor_rechts;
+        if(links < 0)
+        {
+            ML_R = MR_F = 1;
+            wait(1.0);
+            ML_R = MR_F = 0;
+                 
+        }
+        ML_F = MR_F = 1;
+        wait(4.0);
+        ML_F = MR_F = 0;
+    }
+    // if-conditions for negative parameters(when the transfered parameters are negative, the engine turns reverse)
+    if(links < 0 && rechts > links) {
+        
+        ML_EN = v_motor_links;
+        MR_EN = v_motor_rechts;
+        ML_R = MR_R = 1;
+        wait(4.0);
+        ML_R = MR_R = 0;
+    }
+    if(rechts < 0 && rechts < links) {
+
+        ML_EN = v_motor_links;
+        MR_EN = v_motor_rechts;
+        ML_R = MR_R = 1;
+        wait(4.0);
+        ML_R = MR_R = 0;
+    }
+}