File content as of revision 8:2f81517a2d7c:

#include "mbed.h"
#include "Bertl14.h"

// V 3.0

Motor::Motor(PinName pwm, PinName fwd, PinName rev) :
    _pwm(pwm), _fwd(fwd), _rev(rev)
{
    _pwm.period(0.001);
    _pwm=0;
    _fwd=0;
    _rev=0;
    _running=0;
}

void Motor::SetBrake(int aOnOff)
{
    _pwm=0;
    if( aOnOff ) {
        _fwd=0;
        _rev=0;
    } else {
        _fwd=1;
        _rev=1;
    }
}

void Motor::SetPow(float aPow)
{
    if( aPow==0 ) {
        _pwm=0;
        _running=0;
        return;
    }
    if( aPow>=0.0 ) {
        _fwd=1;
        _rev=0;
        _pwm = aPow;
    } else {
        _fwd=0;
        _rev=1;
        _pwm = -aPow;
    }
    _running=1;
}

void Motor::SetPow2(float aPow)
{
    float pow;
    if( aPow==0 ) {
        _pwm=0;
        _running=0;
        return;
    }
    if( aPow>=0.0 ) {
        _fwd=1;
        _rev=0;
        pow = aPow;
    } else {
        _fwd=0;
        _rev=1;
        pow = -aPow;
    }
    if( !_running && (pow<0.3) ) {
        _pwm = 0.3;
        wait_ms(20); // 50
    }
    _pwm = pow;
    _running = 1;
}

BertlDrive::BertlDrive(PinName pwm, PinName fwd, PinName rev, PinName encoder) :
    Motor(pwm,fwd,rev) , _enc(encoder)
{
    encCnt = 0;
}

void BertlDrive::Init()
{
    _enc.rise(this, &BertlDrive::EncoderISR);
    _enc.fall(this, &BertlDrive::EncoderISR);
}

void BertlDrive::EncoderISR()
{
    encCnt++;
}



PortEx::PortEx() :
    _i2c(p28,p27), _p6Event(p6)
{
    btns=btnEvent=0;
    useISR = 1;
}

void PortEx::Init()
{
    char cmd[4];
    _i2c.frequency(100000);
    wait(0.01);
    // Port0 Config  Port0 Out    Port1 In
    cmd[0]=0x06;
    cmd[1]=0x00;
    cmd[2]=0xFF;
    _i2c.write(DEV, cmd, 3, false);
    SetLedPort(0);
    _p6Event.fall(this, &PortEx::p6ISR);
}

void PortEx::p6ISR()
{
    if( !useISR )
        return;
    int16_t prev = btns;
    ReadButtons();
    if( !btns )
        btns = prev;
    else
        btnEvent = 1;
}

void PortEx::SetLedPort(uint8_t aBitPattern)
{
    char cmd[4];
    cmd[0]=2;
    cmd[1]=~aBitPattern;
    _i2c.write(DEV, cmd, 2, false);
}

void PortEx::SetLeds(uint8_t aBitPattern)
{
    _currLeds |= aBitPattern;
    SetLedPort(_currLeds);
}

void PortEx::ToggleLeds(uint8_t aBitPattern)
{
    _currLeds ^= aBitPattern;
    SetLedPort(_currLeds);
}

void PortEx::ClearLeds()
{
    _currLeds=0;
    SetLedPort(0);
}


void PortEx::ReadButtons()
{
    char cmd[4];
    cmd[0]=1;
    _i2c.write(DEV, cmd, 1, true);
    _i2c.read(DEV|1, cmd, 1, false);
    btns = cmd[0];
}

void PortEx::WaitUntilButtonPressed()
{
    int prev = useISR;
    useISR = 0;
    btns = 0;
    while(1) {
        ReadButtons();
        if( btns )
            break;
        wait(0.01);
    }
    btns=btnEvent=0;
    useISR = prev;
}

void PortEx::WaitUntilFrontButtonPressed()
{
    int prev = useISR;
    useISR = 0;
    btns = 0;
    while(1) {
        ReadButtons();
        if( IsAnyFrontButton() )
            break;
        wait(0.01);
    }
    btns=btnEvent=0;
    useISR = prev;
}



UsDistSens::UsDistSens(PinName pinTrigger, PinName pinEcho) :
    trigger(pinTrigger),echo(pinEcho)
{
    echo.rise(this, &UsDistSens::RisingISR);
    echo.fall(this, &UsDistSens::FallingISR);
}

void UsDistSens::StartMeas()
{
    trigger=1;
    wait_us(12);
    trigger=0;
    stw.start();
}

void UsDistSens::RisingISR()
{
    stw.reset();
}

void UsDistSens::FallingISR()
{
    dist=stw.read_us();
    distCM = (float)dist*(343.2E-4/2.0);
}