michael hollegha / BertlLib

2020 Abend Lab

Bertl14.cpp@8:2f81517a2d7c, 2020-06-18 (annotated)

Committer:
hollegha3
Date:
Thu Jun 18 17:17:39 2020 +0000
Revision:
8:2f81517a2d7c
Parent:
7:20c3213c3ada 
2020  Abend Lab

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 7:20c3213c3ada 5 // V 3.0
hollegha2 7:20c3213c3ada 6
hollegha2 0:e0131e671ea0 7 Motor::Motor(PinName pwm, PinName fwd, PinName rev) :
hollegha2 7:20c3213c3ada 8 _pwm(pwm), _fwd(fwd), _rev(rev)
hollegha2 0:e0131e671ea0 9 {
hollegha2 7:20c3213c3ada 10 _pwm.period(0.001);
hollegha2 7:20c3213c3ada 11 _pwm=0;
hollegha2 7:20c3213c3ada 12 _fwd=0;
hollegha2 7:20c3213c3ada 13 _rev=0;
hollegha2 7:20c3213c3ada 14 _running=0;
hollegha2 7:20c3213c3ada 15 }
hollegha2 7:20c3213c3ada 16
hollegha2 7:20c3213c3ada 17 void Motor::SetBrake(int aOnOff)
hollegha2 7:20c3213c3ada 18 {
hollegha2 7:20c3213c3ada 19 _pwm=0;
hollegha2 7:20c3213c3ada 20 if( aOnOff ) {
hollegha2 7:20c3213c3ada 21 _fwd=0;
hollegha2 7:20c3213c3ada 22 _rev=0;
hollegha2 7:20c3213c3ada 23 } else {
hollegha2 7:20c3213c3ada 24 _fwd=1;
hollegha2 7:20c3213c3ada 25 _rev=1;
hollegha2 7:20c3213c3ada 26 }
hollegha2 0:e0131e671ea0 27 }
hollegha2 0:e0131e671ea0 28
hollegha2 0:e0131e671ea0 29 void Motor::SetPow(float aPow)
hollegha2 0:e0131e671ea0 30 {
hollegha2 7:20c3213c3ada 31 if( aPow==0 ) {
hollegha2 7:20c3213c3ada 32 _pwm=0;
hollegha2 7:20c3213c3ada 33 _running=0;
hollegha2 7:20c3213c3ada 34 return;
hollegha2 7:20c3213c3ada 35 }
hollegha2 7:20c3213c3ada 36 if( aPow>=0.0 ) {
hollegha2 7:20c3213c3ada 37 _fwd=1;
hollegha2 7:20c3213c3ada 38 _rev=0;
hollegha2 7:20c3213c3ada 39 _pwm = aPow;
hollegha2 7:20c3213c3ada 40 } else {
hollegha2 7:20c3213c3ada 41 _fwd=0;
hollegha2 7:20c3213c3ada 42 _rev=1;
hollegha2 7:20c3213c3ada 43 _pwm = -aPow;
hollegha2 7:20c3213c3ada 44 }
hollegha2 7:20c3213c3ada 45 _running=1;
hollegha2 7:20c3213c3ada 46 }
hollegha2 7:20c3213c3ada 47
hollegha2 7:20c3213c3ada 48 void Motor::SetPow2(float aPow)
hollegha2 7:20c3213c3ada 49 {
hollegha2 7:20c3213c3ada 50 float pow;
hollegha2 7:20c3213c3ada 51 if( aPow==0 ) {
hollegha2 7:20c3213c3ada 52 _pwm=0;
hollegha2 7:20c3213c3ada 53 _running=0;
hollegha2 7:20c3213c3ada 54 return;
hollegha2 7:20c3213c3ada 55 }
hollegha2 7:20c3213c3ada 56 if( aPow>=0.0 ) {
hollegha2 7:20c3213c3ada 57 _fwd=1;
hollegha2 7:20c3213c3ada 58 _rev=0;
hollegha2 7:20c3213c3ada 59 pow = aPow;
hollegha2 7:20c3213c3ada 60 } else {
hollegha2 7:20c3213c3ada 61 _fwd=0;
hollegha2 7:20c3213c3ada 62 _rev=1;
hollegha2 7:20c3213c3ada 63 pow = -aPow;
hollegha2 7:20c3213c3ada 64 }
hollegha2 7:20c3213c3ada 65 if( !_running && (pow<0.3) ) {
hollegha2 7:20c3213c3ada 66 _pwm = 0.3;
hollegha2 7:20c3213c3ada 67 wait_ms(20); // 50
hollegha2 7:20c3213c3ada 68 }
hollegha2 7:20c3213c3ada 69 _pwm = pow;
hollegha2 7:20c3213c3ada 70 _running = 1;
hollegha2 0:e0131e671ea0 71 }
hollegha2 0:e0131e671ea0 72
hollegha2 0:e0131e671ea0 73 BertlDrive::BertlDrive(PinName pwm, PinName fwd, PinName rev, PinName encoder) :
hollegha2 7:20c3213c3ada 74 Motor(pwm,fwd,rev) , _enc(encoder)
hollegha2 0:e0131e671ea0 75 {
hollegha2 7:20c3213c3ada 76 encCnt = 0;
hollegha2 0:e0131e671ea0 77 }
hollegha2 0:e0131e671ea0 78
hollegha2 0:e0131e671ea0 79 void BertlDrive::Init()
hollegha2 0:e0131e671ea0 80 {
hollegha2 7:20c3213c3ada 81 _enc.rise(this, &BertlDrive::EncoderISR);
hollegha2 7:20c3213c3ada 82 _enc.fall(this, &BertlDrive::EncoderISR);
hollegha2 0:e0131e671ea0 83 }
hollegha2 0:e0131e671ea0 84
hollegha2 0:e0131e671ea0 85 void BertlDrive::EncoderISR()
hollegha2 0:e0131e671ea0 86 {
hollegha2 7:20c3213c3ada 87 encCnt++;
hollegha2 0:e0131e671ea0 88 }
hollegha2 0:e0131e671ea0 89
hollegha2 0:e0131e671ea0 90
hollegha2 7:20c3213c3ada 91
hollegha2 7:20c3213c3ada 92 PortEx::PortEx() :
hollegha2 7:20c3213c3ada 93 _i2c(p28,p27), _p6Event(p6)
hollegha2 0:e0131e671ea0 94 {
hollegha2 7:20c3213c3ada 95 btns=btnEvent=0;
hollegha2 7:20c3213c3ada 96 useISR = 1;
hollegha2 0:e0131e671ea0 97 }
hollegha2 0:e0131e671ea0 98
hollegha2 0:e0131e671ea0 99 void PortEx::Init()
hollegha2 0:e0131e671ea0 100 {
hollegha2 7:20c3213c3ada 101 char cmd[4];
hollegha2 7:20c3213c3ada 102 _i2c.frequency(100000);
hollegha2 7:20c3213c3ada 103 wait(0.01);
hollegha2 7:20c3213c3ada 104 // Port0 Config Port0 Out Port1 In
hollegha2 7:20c3213c3ada 105 cmd[0]=0x06;
hollegha2 7:20c3213c3ada 106 cmd[1]=0x00;
hollegha2 7:20c3213c3ada 107 cmd[2]=0xFF;
hollegha2 7:20c3213c3ada 108 _i2c.write(DEV, cmd, 3, false);
hollegha2 7:20c3213c3ada 109 SetLedPort(0);
hollegha2 7:20c3213c3ada 110 _p6Event.fall(this, &PortEx::p6ISR);
hollegha2 0:e0131e671ea0 111 }
hollegha2 0:e0131e671ea0 112
hollegha2 0:e0131e671ea0 113 void PortEx::p6ISR()
hollegha2 0:e0131e671ea0 114 {
hollegha2 7:20c3213c3ada 115 if( !useISR )
hollegha2 7:20c3213c3ada 116 return;
hollegha2 7:20c3213c3ada 117 int16_t prev = btns;
hollegha2 7:20c3213c3ada 118 ReadButtons();
hollegha2 7:20c3213c3ada 119 if( !btns )
hollegha2 7:20c3213c3ada 120 btns = prev;
hollegha2 7:20c3213c3ada 121 else
hollegha2 7:20c3213c3ada 122 btnEvent = 1;
hollegha2 0:e0131e671ea0 123 }
hollegha2 0:e0131e671ea0 124
hollegha2 0:e0131e671ea0 125 void PortEx::SetLedPort(uint8_t aBitPattern)
hollegha2 0:e0131e671ea0 126 {
hollegha2 7:20c3213c3ada 127 char cmd[4];
hollegha2 7:20c3213c3ada 128 cmd[0]=2;
hollegha2 7:20c3213c3ada 129 cmd[1]=~aBitPattern;
hollegha2 7:20c3213c3ada 130 _i2c.write(DEV, cmd, 2, false);
hollegha2 0:e0131e671ea0 131 }
hollegha2 0:e0131e671ea0 132
hollegha2 0:e0131e671ea0 133 void PortEx::SetLeds(uint8_t aBitPattern)
hollegha2 0:e0131e671ea0 134 {
hollegha2 7:20c3213c3ada 135 _currLeds |= aBitPattern;
hollegha2 7:20c3213c3ada 136 SetLedPort(_currLeds);
hollegha2 0:e0131e671ea0 137 }
hollegha2 0:e0131e671ea0 138
hollegha2 0:e0131e671ea0 139 void PortEx::ToggleLeds(uint8_t aBitPattern)
hollegha2 0:e0131e671ea0 140 {
hollegha2 7:20c3213c3ada 141 _currLeds ^= aBitPattern;
hollegha2 7:20c3213c3ada 142 SetLedPort(_currLeds);
hollegha2 0:e0131e671ea0 143 }
hollegha2 0:e0131e671ea0 144
hollegha2 0:e0131e671ea0 145 void PortEx::ClearLeds()
hollegha2 0:e0131e671ea0 146 {
hollegha2 7:20c3213c3ada 147 _currLeds=0;
hollegha2 7:20c3213c3ada 148 SetLedPort(0);
hollegha2 0:e0131e671ea0 149 }
hollegha2 0:e0131e671ea0 150
hollegha2 0:e0131e671ea0 151
hollegha2 0:e0131e671ea0 152 void PortEx::ReadButtons()
hollegha2 0:e0131e671ea0 153 {
hollegha2 7:20c3213c3ada 154 char cmd[4];
hollegha2 7:20c3213c3ada 155 cmd[0]=1;
hollegha2 7:20c3213c3ada 156 _i2c.write(DEV, cmd, 1, true);
hollegha2 7:20c3213c3ada 157 _i2c.read(DEV|1, cmd, 1, false);
hollegha2 7:20c3213c3ada 158 btns = cmd[0];
hollegha2 0:e0131e671ea0 159 }
hollegha2 0:e0131e671ea0 160
hollegha2 0:e0131e671ea0 161 void PortEx::WaitUntilButtonPressed()
hollegha2 0:e0131e671ea0 162 {
hollegha2 7:20c3213c3ada 163 int prev = useISR;
hollegha2 7:20c3213c3ada 164 useISR = 0;
hollegha2 7:20c3213c3ada 165 btns = 0;
hollegha2 7:20c3213c3ada 166 while(1) {
hollegha2 7:20c3213c3ada 167 ReadButtons();
hollegha2 7:20c3213c3ada 168 if( btns )
hollegha2 7:20c3213c3ada 169 break;
hollegha2 7:20c3213c3ada 170 wait(0.01);
hollegha2 7:20c3213c3ada 171 }
hollegha2 7:20c3213c3ada 172 btns=btnEvent=0;
hollegha2 7:20c3213c3ada 173 useISR = prev;
hollegha2 0:e0131e671ea0 174 }
hollegha2 0:e0131e671ea0 175
hollegha2 6:78243412d2b3 176 void PortEx::WaitUntilFrontButtonPressed()
hollegha2 6:78243412d2b3 177 {
hollegha2 7:20c3213c3ada 178 int prev = useISR;
hollegha2 7:20c3213c3ada 179 useISR = 0;
hollegha2 7:20c3213c3ada 180 btns = 0;
hollegha2 7:20c3213c3ada 181 while(1) {
hollegha2 7:20c3213c3ada 182 ReadButtons();
hollegha2 7:20c3213c3ada 183 if( IsAnyFrontButton() )
hollegha2 7:20c3213c3ada 184 break;
hollegha2 7:20c3213c3ada 185 wait(0.01);
hollegha2 7:20c3213c3ada 186 }
hollegha2 7:20c3213c3ada 187 btns=btnEvent=0;
hollegha2 7:20c3213c3ada 188 useISR = prev;
hollegha2 6:78243412d2b3 189 }
hollegha2 6:78243412d2b3 190
hollegha2 0:e0131e671ea0 191
hollegha2 0:e0131e671ea0 192
hollegha2 0:e0131e671ea0 193 UsDistSens::UsDistSens(PinName pinTrigger, PinName pinEcho) :
hollegha2 7:20c3213c3ada 194 trigger(pinTrigger),echo(pinEcho)
hollegha2 0:e0131e671ea0 195 {
hollegha2 7:20c3213c3ada 196 echo.rise(this, &UsDistSens::RisingISR);
hollegha2 7:20c3213c3ada 197 echo.fall(this, &UsDistSens::FallingISR);
hollegha2 0:e0131e671ea0 198 }
hollegha2 0:e0131e671ea0 199
hollegha2 0:e0131e671ea0 200 void UsDistSens::StartMeas()
hollegha2 0:e0131e671ea0 201 {
hollegha2 7:20c3213c3ada 202 trigger=1;
hollegha2 7:20c3213c3ada 203 wait_us(12);
hollegha2 7:20c3213c3ada 204 trigger=0;
hollegha2 7:20c3213c3ada 205 stw.start();
hollegha2 0:e0131e671ea0 206 }
hollegha2 0:e0131e671ea0 207
hollegha2 0:e0131e671ea0 208 void UsDistSens::RisingISR()
hollegha2 7:20c3213c3ada 209 {
hollegha2 7:20c3213c3ada 210 stw.reset();
hollegha2 7:20c3213c3ada 211 }
hollegha2 0:e0131e671ea0 212
hollegha2 0:e0131e671ea0 213 void UsDistSens::FallingISR()
hollegha2 0:e0131e671ea0 214 {
hollegha2 7:20c3213c3ada 215 dist=stw.read_us();
hollegha2 7:20c3213c3ada 216 distCM = (float)dist*(343.2E-4/2.0);
hollegha2 0:e0131e671ea0 217 }
hollegha2 0:e0131e671ea0 218
hollegha2 0:e0131e671ea0 219
hollegha2 0:e0131e671ea0 220