File content as of revision 4:aef786c67b2d:

#include <math.h>
#include "YellowMotors.h"
#include "mbed.h"


float motorLinearizationL(float desired)
{
    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*(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);
    //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);
}
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)
{
    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':'_');
    pc.printf(" ###\n\r");
    _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;
    if (_led != 0) *_led = 0;
};

void YellowMotors::attachled(DigitalOut & led)
{
    _led = &led;
}