Diff: controls.cpp

Revision:

0:9c49bdc7e402

Child:

4:c8929825a5f4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controls.cpp	Wed Nov 22 02:18:07 2017 +0000
@@ -0,0 +1,134 @@
+#include "controls.h"
+
+void dir_control_M1(controls dir)
+{
+    switch(dir) {
+        case forward:
+            M1F=1;
+            M1B=0;
+            break;
+        case backward:
+            M1F=0;
+            M1B=1;
+            break;
+        case brake:
+            M1F=1;
+            M1B=1;
+            break;
+        case stall:
+        default:
+            M1F=0;
+            M1B=0;
+            break;
+    }
+}
+void dir_control_M2(controls dir)
+{
+    switch(dir) {
+        case forward:
+            M2F=1;
+            M2B=0;
+            break;
+        case backward:
+            M2F=0;
+            M2B=1;
+            break;
+        case brake:
+            M2F=1;
+            M2B=1;
+            break;
+        case stall:
+        default:
+            M2F=0;
+            M2B=0;
+            break;
+    }
+}
+
+const double pulse_cycle = 10; // ms
+
+void move(double cycle_speed, int duration_ms, controls dir_M1, controls dir_M2, double (*PID_error)())   // cycle_speed from 0 - 1
+{
+    Timer T;
+    Timer T_M1;
+    Timer T_M2;
+
+    //start moving first
+    bool High_M1=true;
+    bool High_M2=true;
+
+    double pulse_M1;
+    double period_M1;
+    double pulse_M2;
+    double period_M2;
+
+    double cycle_speed_M1;
+    double cycle_speed_M2;
+
+    dir_control_M1(dir_M1);
+    dir_control_M2(dir_M2);
+
+    T.start();
+    T_M1.start();
+    T_M2.start();
+
+    while(T.read_ms()<(duration_ms)) {
+
+        cycle_speed_M1= cycle_speed - (*PID_error)(); //PID_error() should be under 1 most/all of the time ideally
+        cycle_speed_M2= cycle_speed + (*PID_error)();
+ 
+        if(cycle_speed_M1>1) // Can't go over 1 as that is the max speed
+            cycle_speed_M1=1;
+        if(cycle_speed_M1<0)  //Can't go under o either
+            cycle_speed_M1=0;
+        if(cycle_speed_M2>1)
+            cycle_speed_M2=1;
+        if(cycle_speed_M2<0)
+            cycle_speed_M2=0;
+
+       
+/*
+        //adjusting delays based off PID_error corrections
+        pulse_M1 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M1));
+        period_M1 = (pulse_cycle-pulse_cycle*cycle_speed_M1);
+
+
+        pulse_M2 = (pulse_cycle-pulse_cycle*(1-cycle_speed_M2));
+        period_M2 = (pulse_cycle-pulse_cycle*cycle_speed_M2);
+
+        //controlling the speed for motor 1
+        if(High_M1) {
+            High_M1=pulse_M1 > T_M1.read_ms();
+            if(!High_M1) { //pulse has finished and going LOW
+                dir_control_M1(stall);
+                T_M1.reset();  //start the delay between pulse (note that it continues counting)
+            }
+        } else {
+            High_M1=period_M1 < T_M1.read_ms();
+            if(High_M1) { //delay has finished and going HIGH
+                dir_control_M1(dir_M1);
+                T_M1.reset(); //start the pulse and its clock
+            }
+        }
+
+        //repeated for Motor 2
+        if(High_M2) {
+            High_M2=pulse_M2 > T_M2.read_ms();
+            if(!High_M2) {
+                dir_control_M2(stall);
+                T_M2.reset();
+            }
+        } else {
+            High_M2=period_M2 < T_M2.read_ms();
+            if(High_M2) {
+                dir_control_M2(dir_M2);
+                T_M2.reset();
+            }
+        }
+    }
+    T.stop();
+    T_M1.stop();
+    T_M2.stop();
+    */
+}
+