Diff: Motors_Out.cpp

Revision:

3:8a5c76437c38

Parent:

2:a5effe31eec0

--- a/Motors_Out.cpp	Sun Jul 01 22:06:07 2018 +0000
+++ b/Motors_Out.cpp	Wed Dec 19 09:13:41 2018 +0000
@@ -1,56 +1,143 @@
 #include "mbed.h"
 #include "Motors_Out.h"
 
-
 Motors_Out::Motors_Out(PinName pwm1, PinName pwm2, PinName pwm3, PinName pwm4, PinName pwms, PinName en):
- DC_A1(pwm1),DC_A2(pwm2),DC_B1(pwm3),DC_B2(pwm4),SERVO(pwms),H_ENABLE(en)
+    DC_A1(pwm1),DC_A2(pwm2),DC_B1(pwm3),DC_B2(pwm4),SERVO(pwms),H_ENABLE(en)
 {
     init();
+    t.attach(this, &Motors_Out::caller, 0.02);
 }
 
 void Motors_Out::init()
 {
-    smj=3;
-    pol=16;
-    brz=1;
-}
-
-void Motors_Out::slanjePodataka(int smj1, int pol1, int brz1)
-{
-    smj=smj1;
-    pol=pol1;
-    brz=brz1;
+    dir = 0;
+    aim = 0;
+    dir1 = 0;
+    aim1 = 0;
+    self_drive = false;
+    on = true;
+    DC_B1 = 0;          //v=0 m/s
+    DC_B2 = 0;
+    DC_A1.period_us(100);
+    SERVO.period(0.02);
+    angle = 64;
 }
 
-void Motors_Out::brzina_DC_motora()
+void Motors_Out::manualInfo(int a)
 {
-    DC_A1.period_us(100);
-    switch(smj) {
-        case 1:
-            H_ENABLE=1;
-            DC_B2=0;
-            DC_A2=DC_B2;
-            DC_A1=(float)(brz-1)/14;
-            DC_B1=DC_A1;
-            break;
-        case 2:
-            H_ENABLE=1;
-            DC_B1=0;
-            DC_A1=DC_B1;
-            DC_A2=(float)(brz-1)/14;
-            DC_B2=DC_A2;
-            break;
-        default:
-            H_ENABLE=0;
-            DC_B1=0;
-            DC_B2=0;
-            DC_A1=DC_B1;
-            DC_A2=DC_B2;
+    self_drive = false;
+    dir1 = (a & 0x6);
+    aim1 = (a & 0x9);
+    if(dir1 != 6) {
+        dir = dir1;
+        aim = aim1;
+    } else {
+        dir = 0;
+        aim = 0;
+    }
+    dir1 = 0;
+    aim1 = 0;
+}
+
+void Motors_Out::selfDrive(int path)
+{
+    self_drive = true;
+    angle = path;
+}
+
+void Motors_Out::caller()
+{
+    if(self_drive) {
+        if(angle > 10 && angle < 117) Motors_Out::autonomous();
+    } else {
+        Motors_Out::DCSpeed();
+        if(SERVO < 0.034 || SERVO > 0.064) SERVO = 0.049;
+        Motors_Out::servoAim();
     }
 }
 
-void Motors_Out::polozaj_serva()
+void Motors_Out::DCSpeed()
+{
+    DC_A1.period_us(100);                 //frequency of DC motor = 10 kHz
+    switch(dir) {
+        case 4:
+            H_ENABLE = 1;
+            DC_B2 = 0;
+            DC_A2 = DC_B2;
+            if(DC_A1 >= 0.3) DC_A1 = 0.3; //max speed limited 30%
+            else DC_A1 = DC_A1 + 0.015;   //step change of 5%
+            DC_B1=  DC_A1;
+            break;
+        case 2:
+            H_ENABLE = 1;
+            DC_B1 = 0;
+            DC_A1 = DC_B1;
+            if(DC_A2 >= 0.3) DC_A2 = 0.3;
+            else DC_A2 = DC_A2 + 0.015;
+            DC_B2 = DC_A2;
+            break;
+        default:
+            if(DC_B1 > 0 || DC_B2 > 0) {
+                DC_B1 = DC_B1 - 0.015;
+                DC_B2 = DC_B2 - 0.015;
+            }
+            if(DC_B1 <= 0) DC_B1 = 0;
+            if(DC_B2 <= 0) DC_B2 = 0;
+            DC_A1 = DC_B1;
+            DC_A2 = DC_B2;
+            if(DC_B1 == 0 && DC_B2 == 0) H_ENABLE = 0;
+    }
+}
+
+void Motors_Out::servoAim()
 {
-    SERVO.period(0.010);
-    SERVO=(float)(pol+29)/300;
+    SERVO.period(0.02);                                 //frequency of servo = 50 Hz
+    if(aim == 8) {
+        SERVO = SERVO - 0.0005;
+        if(SERVO < 0.035) SERVO = 0.035;                //max left
+    } else if(aim == 1) {
+        SERVO = SERVO + 0.0005;
+        if(SERVO > 0.063) SERVO = 0.063;                //max right
+    } else {
+        if(SERVO > 0.0515) SERVO = SERVO - 0.0005;      //step change of 2%  
+         else if(SERVO < 0.0465) SERVO = SERVO + 0.0005;  
+         else SERVO = 0.049;                            //forward
+    }
 }
+
+void Motors_Out::autonomous()   //this function can
+{                               //be much improved
+    SERVO.period(0.02);         //with additional testing
+    DC_A1.period_us(100);
+    H_ENABLE = 1;
+    DC_B2 = 0;
+    DC_A2 = DC_B2;
+
+    if(angle < 30) angle = 30;
+    if(angle > 97) angle = 97;
+    if(angle >= 60 && angle <= 67) serv = 0.049;                        //forward
+    else if(angle < 60) serv = 0.049 - ((60 - ((float)angle))*7/15000); //left
+    else if(angle > 67) serv = 0.049 + ((((float)angle) - 67)*7/15000); //right
+    timeout.attach(this,&Motors_Out::servoSig,0.0015); //used to put SERVO to 0 after
+    if(on) {                                           //the wanted SERVO signal is sent.
+        on = false;                                    //sometimes another SERVO pulse
+        SERVO = serv;                                  //would just pop up right after 
+    }                                                  //the good one.
+    if(angle < 40) {                            //electronic differential
+        if(DC_B1 >= 0.25) DC_B1 = 0.25;         //which can be a lot better.
+    } else if(angle > 87) {                     //with this basic solution
+        if(DC_A1 >= 0.25) DC_A1 = 0.25;         //performance has got better
+    } else if(DC_A1 >= 0.35) {
+        DC_A1 = 0.35;
+        DC_B1=  DC_A1;
+    } else {
+        DC_A1 = DC_A1 + 0.015;              //step change of 5%
+        DC_B1=  DC_A1;
+    }
+}
+
+void Motors_Out::servoSig()
+{
+    SERVO = 0;
+    on = true;
+}
\ No newline at end of file