Diff: YellowMotors.cpp

Revision:

4:aef786c67b2d

Parent:

3:c656543830df

Child:

5:6dbb019203e9

--- a/YellowMotors.cpp	Thu Feb 04 15:45:09 2016 +0000
+++ b/YellowMotors.cpp	Fri Feb 05 15:49:11 2016 +0000
@@ -5,18 +5,22 @@
 
 float motorLinearizationL(float desired)
 {
-    return (float)((6.6691*(exp(0.0249053*(desired*100))+2.92508))/100.0);
+    return (float)(((6.6691*(exp(0.0249053*(desired*100))+2.92508))/100.0)*(desired*10/9+0.1));
 };
 
 float motorLinearizationR(float desired)
 {
-    return (float)(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*(desired*10/9+0.1)); // \/ 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;
 };
 
 YellowMotors::YellowMotors(PinName clk, PinName  lat, PinName  dat, PinName ena, PinName Lpwm, PinName Rpwm) : _clk(clk), _lat(lat), _dat(dat), _ena(ena), L(D11), R(D3) //, _Lpwm(Lpwm), _Rpwm(Rpwm)
 {
+    _init();
+}
+void YellowMotors::_init()
+{
     _directions = 0xff;
     //PwmOut _Lpwm(D11);
     //PwmOut _Rpwm(D3);
@@ -29,7 +33,6 @@
     // and create motor objects
     _ena.write(0);
     //pc.printf("konstruktor yellow. Lpwm: %d\n\r", Lpwm);
-
 }
 void YellowMotors::operator= (const float value)
 {
@@ -112,6 +115,8 @@
 
 void YellowMotors::setDirections(char directions)
 {
+    if (_led != 0) *_led = 1;
+    pc.printf("\n\r_led ma adres: %d\n\r", _led);
     pc.printf("### set dirs: ");
     for(int i=0; i<8; ++i)
         pc.printf("%c", ((directions >> (7-i)) & 0x1)?'x':'_');
@@ -126,5 +131,10 @@
     }
     _lat = 1;
     _clk = 0;
+    if (_led != 0) *_led = 0;
 };
 
+void YellowMotors::attachled(DigitalOut & led)
+{
+    _led = &led;
+}
\ No newline at end of file