Lab 4CHEL / BertlLib

Ultraschall Test Geschwindigkeitsregelung

Fork of BertlLib by michael hollegha

Bertl14.cpp@6:78243412d2b3, 2015-03-05 (annotated)

Committer:
hollegha2
Date:
Thu Mar 05 17:39:16 2015 +0000
Revision:
6:78243412d2b3
Parent:
0:e0131e671ea0
Child:
7:01d2bc098b6d
FrontButton

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hollegha2 0:e0131e671ea0 1
hollegha2 0:e0131e671ea0 2 #include "mbed.h"
hollegha2 0:e0131e671ea0 3 #include "Bertl14.h"
hollegha2 0:e0131e671ea0 4
hollegha2 0:e0131e671ea0 5 Motor::Motor(PinName pwm, PinName fwd, PinName rev) :
hollegha2 0:e0131e671ea0 6 _pwm(pwm), _fwd(fwd), _rev(rev)
hollegha2 0:e0131e671ea0 7 {
hollegha2 0:e0131e671ea0 8 _pwm.period(0.001); _pwm=0;
hollegha2 0:e0131e671ea0 9 _fwd=0; _rev=0;
hollegha2 0:e0131e671ea0 10 }
hollegha2 0:e0131e671ea0 11
hollegha2 0:e0131e671ea0 12 void Motor::SetPow(float aPow)
hollegha2 0:e0131e671ea0 13 {
hollegha2 0:e0131e671ea0 14 if( aPow>=0.0 ) {
hollegha2 0:e0131e671ea0 15 _fwd=1; _rev=0;
hollegha2 0:e0131e671ea0 16 _pwm = aPow;
hollegha2 0:e0131e671ea0 17 }
hollegha2 0:e0131e671ea0 18 else {
hollegha2 0:e0131e671ea0 19 _fwd=0; _rev=1;
hollegha2 0:e0131e671ea0 20 _pwm = -aPow;
hollegha2 0:e0131e671ea0 21 }
hollegha2 0:e0131e671ea0 22 }
hollegha2 0:e0131e671ea0 23
hollegha2 0:e0131e671ea0 24 BertlDrive::BertlDrive(PinName pwm, PinName fwd, PinName rev, PinName encoder) :
hollegha2 0:e0131e671ea0 25 Motor(pwm,fwd,rev) , _enc(encoder)
hollegha2 0:e0131e671ea0 26 {
hollegha2 0:e0131e671ea0 27 encCnt = 0;
hollegha2 0:e0131e671ea0 28 }
hollegha2 0:e0131e671ea0 29
hollegha2 0:e0131e671ea0 30 void BertlDrive::Init()
hollegha2 0:e0131e671ea0 31 {
hollegha2 0:e0131e671ea0 32 _enc.rise(this, &BertlDrive::EncoderISR);
hollegha2 0:e0131e671ea0 33 _enc.fall(this, &BertlDrive::EncoderISR);
hollegha2 0:e0131e671ea0 34 }
hollegha2 0:e0131e671ea0 35
hollegha2 0:e0131e671ea0 36 void BertlDrive::EncoderISR()
hollegha2 0:e0131e671ea0 37 {
hollegha2 0:e0131e671ea0 38 encCnt++;
hollegha2 0:e0131e671ea0 39 }
hollegha2 0:e0131e671ea0 40
hollegha2 0:e0131e671ea0 41
hollegha2 0:e0131e671ea0 42 PortEx::PortEx() :
hollegha2 0:e0131e671ea0 43 _i2c(p28,p27), _p6Event(p6)
hollegha2 0:e0131e671ea0 44 {
hollegha2 0:e0131e671ea0 45 btns=btnEvent=0;
hollegha2 0:e0131e671ea0 46 useISR = 1;
hollegha2 0:e0131e671ea0 47 }
hollegha2 0:e0131e671ea0 48
hollegha2 0:e0131e671ea0 49 void PortEx::Init()
hollegha2 0:e0131e671ea0 50 {
hollegha2 0:e0131e671ea0 51 char cmd[4];
hollegha2 0:e0131e671ea0 52 _i2c.frequency(100000);
hollegha2 0:e0131e671ea0 53 wait(0.01);
hollegha2 0:e0131e671ea0 54 // Port0 Config Port0 Out Port1 In
hollegha2 0:e0131e671ea0 55 cmd[0]=0x06; cmd[1]=0x00; cmd[2]=0xFF;
hollegha2 0:e0131e671ea0 56 _i2c.write(DEV, cmd, 3, false);
hollegha2 0:e0131e671ea0 57 SetLedPort(0);
hollegha2 0:e0131e671ea0 58 _p6Event.fall(this, &PortEx::p6ISR);
hollegha2 0:e0131e671ea0 59 }
hollegha2 0:e0131e671ea0 60
hollegha2 0:e0131e671ea0 61 void PortEx::p6ISR()
hollegha2 0:e0131e671ea0 62 {
hollegha2 0:e0131e671ea0 63 if( !useISR )
hollegha2 0:e0131e671ea0 64 return;
hollegha2 0:e0131e671ea0 65 int16_t prev = btns;
hollegha2 0:e0131e671ea0 66 ReadButtons();
hollegha2 0:e0131e671ea0 67 if( !btns )
hollegha2 0:e0131e671ea0 68 btns = prev;
hollegha2 0:e0131e671ea0 69 else
hollegha2 0:e0131e671ea0 70 btnEvent = 1;
hollegha2 0:e0131e671ea0 71 }
hollegha2 0:e0131e671ea0 72
hollegha2 0:e0131e671ea0 73 void PortEx::SetLedPort(uint8_t aBitPattern)
hollegha2 0:e0131e671ea0 74 {
hollegha2 0:e0131e671ea0 75 char cmd[4];
hollegha2 0:e0131e671ea0 76 cmd[0]=2; cmd[1]=~aBitPattern;
hollegha2 0:e0131e671ea0 77 _i2c.write(DEV, cmd, 2, false);
hollegha2 0:e0131e671ea0 78 }
hollegha2 0:e0131e671ea0 79
hollegha2 0:e0131e671ea0 80 void PortEx::SetLeds(uint8_t aBitPattern)
hollegha2 0:e0131e671ea0 81 {
hollegha2 0:e0131e671ea0 82 _currLeds |= aBitPattern;
hollegha2 0:e0131e671ea0 83 SetLedPort(_currLeds);
hollegha2 0:e0131e671ea0 84 }
hollegha2 0:e0131e671ea0 85
hollegha2 0:e0131e671ea0 86 void PortEx::ToggleLeds(uint8_t aBitPattern)
hollegha2 0:e0131e671ea0 87 {
hollegha2 0:e0131e671ea0 88 _currLeds ^= aBitPattern;
hollegha2 0:e0131e671ea0 89 SetLedPort(_currLeds);
hollegha2 0:e0131e671ea0 90 }
hollegha2 0:e0131e671ea0 91
hollegha2 0:e0131e671ea0 92 void PortEx::ClearLeds()
hollegha2 0:e0131e671ea0 93 {
hollegha2 0:e0131e671ea0 94 _currLeds=0; SetLedPort(0);
hollegha2 0:e0131e671ea0 95 }
hollegha2 0:e0131e671ea0 96
hollegha2 0:e0131e671ea0 97
hollegha2 0:e0131e671ea0 98 void PortEx::ReadButtons()
hollegha2 0:e0131e671ea0 99 {
hollegha2 0:e0131e671ea0 100 char cmd[4];
hollegha2 0:e0131e671ea0 101 cmd[0]=1;
hollegha2 0:e0131e671ea0 102 _i2c.write(DEV, cmd, 1, true);
hollegha2 0:e0131e671ea0 103 _i2c.read(DEV|1, cmd, 1, false);
hollegha2 0:e0131e671ea0 104 btns = cmd[0];
hollegha2 0:e0131e671ea0 105 }
hollegha2 0:e0131e671ea0 106
hollegha2 0:e0131e671ea0 107 void PortEx::WaitUntilButtonPressed()
hollegha2 0:e0131e671ea0 108 {
hollegha2 0:e0131e671ea0 109 int prev = useISR;
hollegha2 0:e0131e671ea0 110 useISR = 0;
hollegha2 0:e0131e671ea0 111 btns = 0;
hollegha2 0:e0131e671ea0 112 while(1) {
hollegha2 0:e0131e671ea0 113 ReadButtons();
hollegha2 0:e0131e671ea0 114 if( btns )
hollegha2 0:e0131e671ea0 115 break;
hollegha2 0:e0131e671ea0 116 wait(0.01);
hollegha2 0:e0131e671ea0 117 }
hollegha2 0:e0131e671ea0 118 btns=btnEvent=0;
hollegha2 0:e0131e671ea0 119 useISR = prev;
hollegha2 0:e0131e671ea0 120 }
hollegha2 0:e0131e671ea0 121
hollegha2 6:78243412d2b3 122 void PortEx::WaitUntilFrontButtonPressed()
hollegha2 6:78243412d2b3 123 {
hollegha2 6:78243412d2b3 124 int prev = useISR;
hollegha2 6:78243412d2b3 125 useISR = 0;
hollegha2 6:78243412d2b3 126 btns = 0;
hollegha2 6:78243412d2b3 127 while(1) {
hollegha2 6:78243412d2b3 128 ReadButtons();
hollegha2 6:78243412d2b3 129 if( IsAnyFrontButton() )
hollegha2 6:78243412d2b3 130 break;
hollegha2 6:78243412d2b3 131 wait(0.01);
hollegha2 6:78243412d2b3 132 }
hollegha2 6:78243412d2b3 133 btns=btnEvent=0;
hollegha2 6:78243412d2b3 134 useISR = prev;
hollegha2 6:78243412d2b3 135 }
hollegha2 6:78243412d2b3 136
hollegha2 0:e0131e671ea0 137
hollegha2 0:e0131e671ea0 138
hollegha2 0:e0131e671ea0 139 UsDistSens::UsDistSens(PinName pinTrigger, PinName pinEcho) :
hollegha2 0:e0131e671ea0 140 trigger(pinTrigger),echo(pinEcho)
hollegha2 0:e0131e671ea0 141 {
hollegha2 0:e0131e671ea0 142 echo.rise(this, &UsDistSens::RisingISR);
hollegha2 0:e0131e671ea0 143 echo.fall(this, &UsDistSens::FallingISR);
hollegha2 0:e0131e671ea0 144 }
hollegha2 0:e0131e671ea0 145
hollegha2 0:e0131e671ea0 146 void UsDistSens::StartMeas()
hollegha2 0:e0131e671ea0 147 {
hollegha2 0:e0131e671ea0 148 trigger=1; wait_us(12); trigger=0;
hollegha2 0:e0131e671ea0 149 stw.start();
hollegha2 0:e0131e671ea0 150 }
hollegha2 0:e0131e671ea0 151
hollegha2 0:e0131e671ea0 152 void UsDistSens::RisingISR()
hollegha2 0:e0131e671ea0 153 { stw.reset(); }
hollegha2 0:e0131e671ea0 154
hollegha2 0:e0131e671ea0 155 void UsDistSens::FallingISR()
hollegha2 0:e0131e671ea0 156 {
hollegha2 0:e0131e671ea0 157 dist=stw.read_us();
hollegha2 0:e0131e671ea0 158 distCM = (float)dist*(343.2E-4/2.0);
hollegha2 0:e0131e671ea0 159 }
hollegha2 0:e0131e671ea0 160
hollegha2 0:e0131e671ea0 161
hollegha2 0:e0131e671ea0 162