Diff: YellowMotors.cpp

Revision:

2:6f6e591f1838

Parent:

0:85e85976c650

Child:

3:c656543830df

--- a/YellowMotors.cpp	Wed Feb 03 16:16:33 2016 +0000
+++ b/YellowMotors.cpp	Thu Feb 04 02:50:06 2016 +0000
@@ -1,13 +1,46 @@
 #include <math.h>
+#include "YellowMotors.h"
+#include "mbed.h"
+
 
 float motorLinearizationL(float desired)
 {
-    return (6.6691*(exp(0.0249053*(desired*100))+2.92508))/100.0;
-}
+    return (float)((6.6691*(exp(0.0249053*(desired*100))+2.92508))/100.0);
+};
 
 float motorLinearizationR(float desired)
 {
-    return 4.096509*(exp(0.0286952296*(desired*100))+5.073644964)/100.0; // \/ history of trial and error
+    return (float)(4.096509*(exp(0.0286952296*(desired*100))+5.073644964)/100.0); // \/ history of trial and error
     //return 5.9693939*(exp(0.0251906*(desired*100))+3.162519)/100.0; // that wasn't that bad at all! to early start, the rest ok
     //return (3.45183*(exp(0.0292461*(desired*100))+5.51727))/100.0;
-}
\ No newline at end of file
+};
+
+YellowMotors::YellowMotors(PinName clk, PinName  lat, PinName  dat, PinName ena, PinName Lpwm, PinName Rpwm) : _clk(clk), _lat(lat), _dat(dat), _ena(ena)//, _Lpwm(Lpwm), _Rpwm(Rpwm)
+{
+    //PwmOut _Lpwm(D11);
+    //PwmOut _Rpwm(D3);
+    //DigitalOut _dat(dat);
+    //DigitalOut _clk(clk);
+    //DigitalOut _lat(lat);
+
+    // and create motor objects
+    _ena.write(0);
+    //pc.printf("konstruktor yellow. Lpwm: %d\n\r", Lpwm);
+    //motorItself::motorItself L(D11);//Lpwm);
+    //motorItself::motorItself R(D3);//Rpwm);
+}
+
+void YellowMotors::setDirections(char directions)
+{
+    _directions = directions;
+    _lat = 0;
+    for (signed char i = 7; i >= 0; i--) {
+        _clk = 0;
+        _dat = (directions >> i) & 0x1;
+        wait(0.00001);
+        _clk = 1;
+    }
+    _lat = 1;
+    _clk = 0;
+};
+