Diff: Bertl14.cpp

Revision:

7:20c3213c3ada

Parent:

6:78243412d2b3

--- a/Bertl14.cpp	Thu Mar 05 17:39:16 2015 +0000
+++ b/Bertl14.cpp	Thu Nov 19 08:11:19 2015 +0000
@@ -2,160 +2,218 @@
 #include "mbed.h"
 #include "Bertl14.h"
 
+// V 3.0
+
 Motor::Motor(PinName pwm, PinName fwd, PinName rev) :
-_pwm(pwm), _fwd(fwd), _rev(rev) 
+    _pwm(pwm), _fwd(fwd), _rev(rev)
 {
-	_pwm.period(0.001); _pwm=0;
-	_fwd=0; _rev=0;
+    _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.0 ) {
-		_fwd=1; _rev=0;
-		_pwm = aPow;
-	}
-	else {
-		_fwd=0; _rev=1;
-		_pwm = -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)
+    Motor(pwm,fwd,rev) , _enc(encoder)
 {
-	encCnt = 0;
+    encCnt = 0;
 }
 
 void BertlDrive::Init()
 {
-	_enc.rise(this, &BertlDrive::EncoderISR);
-	_enc.fall(this, &BertlDrive::EncoderISR);
+    _enc.rise(this, &BertlDrive::EncoderISR);
+    _enc.fall(this, &BertlDrive::EncoderISR);
 }
 
 void BertlDrive::EncoderISR()
 {
-	encCnt++;
+    encCnt++;
 }
 
 
-PortEx::PortEx() : 
-_i2c(p28,p27), _p6Event(p6)
+
+PortEx::PortEx() :
+    _i2c(p28,p27), _p6Event(p6)
 {
-	btns=btnEvent=0;
-	useISR = 1;
+    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);
+    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;
+    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);
+    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);
+    _currLeds |= aBitPattern;
+    SetLedPort(_currLeds);
 }
 
 void PortEx::ToggleLeds(uint8_t aBitPattern)
 {
-	_currLeds ^= aBitPattern;
-	SetLedPort(_currLeds);
+    _currLeds ^= aBitPattern;
+    SetLedPort(_currLeds);
 }
 
 void PortEx::ClearLeds()
 {
-	_currLeds=0; SetLedPort(0);
+    _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];
+    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;
+    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;
+    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)
+    trigger(pinTrigger),echo(pinEcho)
 {
-	echo.rise(this, &UsDistSens::RisingISR);
-	echo.fall(this, &UsDistSens::FallingISR);
+    echo.rise(this, &UsDistSens::RisingISR);
+    echo.fall(this, &UsDistSens::FallingISR);
 }
 
 void UsDistSens::StartMeas()
 {
-	trigger=1; wait_us(12); trigger=0;
-	stw.start();
+    trigger=1;
+    wait_us(12);
+    trigger=0;
+    stw.start();
 }
 
 void UsDistSens::RisingISR()
-{ stw.reset(); }
+{
+    stw.reset();
+}
 
 void UsDistSens::FallingISR()
 {
-	dist=stw.read_us();
-	distCM = (float)dist*(343.2E-4/2.0);
+    dist=stw.read_us();
+    distCM = (float)dist*(343.2E-4/2.0);
 }