Diff: YellowMotors.cpp

Revision:

3:c656543830df

Parent:

2:6f6e591f1838

Child:

4:aef786c67b2d

--- a/YellowMotors.cpp	Thu Feb 04 02:50:06 2016 +0000
+++ b/YellowMotors.cpp	Thu Feb 04 15:45:09 2016 +0000
@@ -15,23 +15,107 @@
     //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)//, _Lpwm(Lpwm), _Rpwm(Rpwm)
+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)
 {
+    _directions = 0xff;
     //PwmOut _Lpwm(D11);
     //PwmOut _Rpwm(D3);
     //DigitalOut _dat(dat);
     //DigitalOut _clk(clk);
     //DigitalOut _lat(lat);
 
+    //motorItself::motorItself L(Lpwm);
+    //motorItself::motorItself R(Rpwm);
     // 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::operator= (const float value)
+{
+    this->set(0, value);
+    this->set(1, value);
+};
+void YellowMotors::set(float value, int whichMotor )
+{
+    pc.printf("value: %.3f", value);
+    char directionsNew = _directions;
+    if(whichMotor == -1) { // case set both;
+        L = motorLinearizationL(abs(value));
+        R = motorLinearizationL(abs(value));
+        if(value > 0) {
+            directionsNew &= ~L_B;
+            directionsNew |= L_F;
+            directionsNew &= ~R_B;
+            directionsNew |= R_F;
+        } else if (value < 0) {
+            directionsNew &= ~L_F;
+            directionsNew |= L_B;
+            directionsNew &= ~R_F;
+            directionsNew |= R_B;
+        } else { // this and other " else value == 0 " update directions register to not allow any movement during reset/programming. Basiclly if the speed is to be 0, then turn the motors off.
+            L = 0;
+            R = 0;
+            directionsNew |= L_B;
+            directionsNew |= L_F;
+            directionsNew |= R_B;
+            directionsNew |= R_F;
+        }
+    } else {
+        if(whichMotor == 0) {
+            L = motorLinearizationL(abs(value));
+            if(value > 0) {
+                directionsNew &= ~L_B;
+                directionsNew |= L_F;
+            } else if (value < 0) {
+                directionsNew &= ~L_F;
+                directionsNew |= L_B;
+            } else {
+                L = 0;
+                directionsNew |= L_B;
+                directionsNew |= L_F;
+            }
+        } else if(whichMotor == 1) {
+            R = motorLinearizationR(abs(value));
+            if(value > 0) {
+                directionsNew &= ~R_B;
+                directionsNew |= R_F;
+            } else if (value < 0) {
+                directionsNew &= ~R_F;
+                directionsNew |= R_B;
+            } else {
+                R = 0;
+                directionsNew |= R_B;
+                directionsNew |= R_F;
+            }
+        }
+    }
+    if ( _directions != directionsNew ) { // only change register content when needed.
+        _directions = directionsNew;
+        setDirections(_directions);
+    }
+};
+
+float YellowMotors::get(int whichMotor)
+{
+    if(whichMotor == -1) { // case set both;
+        return this->L._pwmSigned + this->R._pwmSigned;
+    } else {
+        if(whichMotor == 0) {
+            return L._pwmSigned;
+        } else if(whichMotor == 1) {
+            return R._pwmSigned;
+        }
+    }
+    return -1;
+};
 
 void YellowMotors::setDirections(char directions)
 {
+    pc.printf("### set dirs: ");
+    for(int i=0; i<8; ++i)
+        pc.printf("%c", ((directions >> (7-i)) & 0x1)?'x':'_');
+    pc.printf(" ###\n\r");
     _directions = directions;
     _lat = 0;
     for (signed char i = 7; i >= 0; i--) {