Library to control the bike (just basic for now)
BikeControl.cpp@1:39f462024f10, 2016-07-18 (annotated)
- Committer:
- ptuytsch
- Date:
- Mon Jul 18 13:19:33 2016 +0000
- Revision:
- 1:39f462024f10
- Parent:
- 0:792a8f167ac0
adding full functionallity of tortuga in library (not the most beautiful way)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ptuytsch | 0:792a8f167ac0 | 1 | #include "mbed.h" |
ptuytsch | 0:792a8f167ac0 | 2 | #include "BikeControl.h" |
ptuytsch | 1:39f462024f10 | 3 | #include "BikeData.h" |
ptuytsch | 1:39f462024f10 | 4 | #include "BatteryState.h" |
ptuytsch | 0:792a8f167ac0 | 5 | |
ptuytsch | 1:39f462024f10 | 6 | BikeControl::BikeControl(BikeData* bikeData, BatteryState* trailerBat, BatteryState* bikeBat, BatteryState* auxBat): |
ptuytsch | 1:39f462024f10 | 7 | trailerCtrl(PB_1), |
ptuytsch | 1:39f462024f10 | 8 | motorRightCtrl(PB_15), |
ptuytsch | 1:39f462024f10 | 9 | motorRightCounter(0), |
ptuytsch | 1:39f462024f10 | 10 | motorRightRpm(0), |
ptuytsch | 1:39f462024f10 | 11 | motorLeftCtrl(PB_13), |
ptuytsch | 1:39f462024f10 | 12 | motorLeftCounter(0), |
ptuytsch | 1:39f462024f10 | 13 | motorLeftRpm(0), |
ptuytsch | 1:39f462024f10 | 14 | motorRightHall(PC_13), |
ptuytsch | 1:39f462024f10 | 15 | motorLeftHall(PC_1), |
ptuytsch | 1:39f462024f10 | 16 | |
ptuytsch | 1:39f462024f10 | 17 | //BRAKE |
ptuytsch | 1:39f462024f10 | 18 | brakeFront(PC_10), |
ptuytsch | 1:39f462024f10 | 19 | brakeRear(PC_12), |
ptuytsch | 1:39f462024f10 | 20 | |
ptuytsch | 1:39f462024f10 | 21 | //GENERATOR |
ptuytsch | 1:39f462024f10 | 22 | generatorHallA(PD_2), |
ptuytsch | 1:39f462024f10 | 23 | generatorBrake(PC_9), |
ptuytsch | 1:39f462024f10 | 24 | generatorHallACounter(0), |
ptuytsch | 1:39f462024f10 | 25 | generatorHallARpm(0), |
ptuytsch | 1:39f462024f10 | 26 | generatorHallB(PC_3), |
ptuytsch | 1:39f462024f10 | 27 | generatorHallBCounter(0), |
ptuytsch | 1:39f462024f10 | 28 | generatorHallBRpm(0), |
ptuytsch | 1:39f462024f10 | 29 | |
ptuytsch | 1:39f462024f10 | 30 | //BUTTONS ON STEERING |
ptuytsch | 1:39f462024f10 | 31 | //userButton(USER_BUTTON), |
ptuytsch | 1:39f462024f10 | 32 | buttonGreen(PB_11), |
ptuytsch | 1:39f462024f10 | 33 | buttonRed(PB_12), |
ptuytsch | 1:39f462024f10 | 34 | buttonDirectionRight(PA_15), |
ptuytsch | 1:39f462024f10 | 35 | buttonDirectionLeft(PB_7), |
ptuytsch | 1:39f462024f10 | 36 | |
ptuytsch | 1:39f462024f10 | 37 | //SWITCH |
ptuytsch | 1:39f462024f10 | 38 | switchOn(PC_11), |
ptuytsch | 1:39f462024f10 | 39 | switchWalk(PB_2), |
ptuytsch | 1:39f462024f10 | 40 | |
ptuytsch | 1:39f462024f10 | 41 | //LIGHT |
ptuytsch | 1:39f462024f10 | 42 | lightFront(PB_14), |
ptuytsch | 1:39f462024f10 | 43 | lightBack(PA_11), |
ptuytsch | 1:39f462024f10 | 44 | lightLeft(PC_6), |
ptuytsch | 1:39f462024f10 | 45 | lightRight(PC_8), |
ptuytsch | 1:39f462024f10 | 46 | |
ptuytsch | 1:39f462024f10 | 47 | bd(bikeData), |
ptuytsch | 1:39f462024f10 | 48 | bikeBat(bikeBat), |
ptuytsch | 1:39f462024f10 | 49 | trailerBat(trailerBat), |
ptuytsch | 1:39f462024f10 | 50 | auxBat(auxBat) |
ptuytsch | 1:39f462024f10 | 51 | { |
ptuytsch | 0:792a8f167ac0 | 52 | //PullUp on userButton input |
ptuytsch | 1:39f462024f10 | 53 | //userButton.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 54 | //PullUp on brake inputs |
ptuytsch | 0:792a8f167ac0 | 55 | brakeFront.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 56 | brakeRear.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 57 | |
ptuytsch | 0:792a8f167ac0 | 58 | //Call function on rise of interruptpin |
ptuytsch | 1:39f462024f10 | 59 | generatorHallA.rise(this, &BikeControl::generatorHallAPulsed); |
ptuytsch | 1:39f462024f10 | 60 | generatorHallB.rise(this, &BikeControl::generatorHallBPulsed); |
ptuytsch | 0:792a8f167ac0 | 61 | generatorBrake.period_us(50); |
ptuytsch | 0:792a8f167ac0 | 62 | generatorBrake.write(0.5f); |
ptuytsch | 0:792a8f167ac0 | 63 | generatorHallACounter=0; |
ptuytsch | 0:792a8f167ac0 | 64 | generatorHallARpm=0; |
ptuytsch | 0:792a8f167ac0 | 65 | generatorHallBCounter=0; |
ptuytsch | 0:792a8f167ac0 | 66 | generatorHallBRpm=0; |
ptuytsch | 0:792a8f167ac0 | 67 | |
ptuytsch | 0:792a8f167ac0 | 68 | //motor |
ptuytsch | 0:792a8f167ac0 | 69 | motorRightCtrl.period_ms(100); |
ptuytsch | 0:792a8f167ac0 | 70 | motorRightCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 71 | motorRightHall.rise(this, &BikeControl::motorRightPulsed); |
ptuytsch | 0:792a8f167ac0 | 72 | motorLeftCtrl.period_ms(100); |
ptuytsch | 0:792a8f167ac0 | 73 | motorLeftCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 74 | motorLeftHall.rise(this, &BikeControl::motorLeftPulsed); |
ptuytsch | 0:792a8f167ac0 | 75 | |
ptuytsch | 0:792a8f167ac0 | 76 | //Button inputs |
ptuytsch | 0:792a8f167ac0 | 77 | buttonGreen.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 78 | buttonRed.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 79 | buttonDirectionRight.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 80 | buttonDirectionLeft.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 81 | |
ptuytsch | 0:792a8f167ac0 | 82 | //SWITCH |
ptuytsch | 0:792a8f167ac0 | 83 | switchOn.mode(PullUp); |
ptuytsch | 0:792a8f167ac0 | 84 | switchWalk.mode(PullUp); |
ptuytsch | 1:39f462024f10 | 85 | |
ptuytsch | 1:39f462024f10 | 86 | //t.attach(this, &BikeControl::periodicCallback, 0.5); |
ptuytsch | 1:39f462024f10 | 87 | } |
ptuytsch | 1:39f462024f10 | 88 | |
ptuytsch | 1:39f462024f10 | 89 | void BikeControl::startControlLoop(){ |
ptuytsch | 1:39f462024f10 | 90 | t.attach(this, &BikeControl::periodicCallback, 0.5); |
ptuytsch | 1:39f462024f10 | 91 | } |
ptuytsch | 1:39f462024f10 | 92 | |
ptuytsch | 1:39f462024f10 | 93 | void BikeControl::periodicCallback(){ |
ptuytsch | 1:39f462024f10 | 94 | //printf("callback\n"); |
ptuytsch | 1:39f462024f10 | 95 | generatorHallARpm = generatorHallACounter*2; //60 rising edges/ rotation means Rpm is equal to the counter value in 1 sec |
ptuytsch | 1:39f462024f10 | 96 | generatorHallACounter = 0; |
ptuytsch | 1:39f462024f10 | 97 | generatorHallBRpm = generatorHallBCounter*2; //60 rising edges/ rotation means Rpm is equal to the counter value in 1 sec |
ptuytsch | 1:39f462024f10 | 98 | generatorHallBCounter = 0; |
ptuytsch | 1:39f462024f10 | 99 | motorRightRpm = motorRightCounter*2; |
ptuytsch | 1:39f462024f10 | 100 | motorRightCounter = 0; |
ptuytsch | 1:39f462024f10 | 101 | motorLeftRpm = motorLeftCounter*2; |
ptuytsch | 1:39f462024f10 | 102 | motorLeftCounter = 0; |
ptuytsch | 1:39f462024f10 | 103 | if (generatorHallARpm>0) { |
ptuytsch | 1:39f462024f10 | 104 | printf("RPM generator Hall A = %i\n",generatorHallARpm); |
ptuytsch | 1:39f462024f10 | 105 | } |
ptuytsch | 1:39f462024f10 | 106 | if (generatorHallBRpm>0) { |
ptuytsch | 1:39f462024f10 | 107 | printf("RPM generator Hall B = %i\n",generatorHallBRpm); |
ptuytsch | 1:39f462024f10 | 108 | } |
ptuytsch | 1:39f462024f10 | 109 | if (motorRightRpm>0) { |
ptuytsch | 1:39f462024f10 | 110 | printf("RPM motor Right = %i\n",motorRightRpm); |
ptuytsch | 1:39f462024f10 | 111 | } |
ptuytsch | 1:39f462024f10 | 112 | if (motorLeftRpm>0) { |
ptuytsch | 1:39f462024f10 | 113 | printf("RPM motor Left = %i\n",motorLeftRpm); |
ptuytsch | 1:39f462024f10 | 114 | } |
ptuytsch | 1:39f462024f10 | 115 | //printf("light control\n"); |
ptuytsch | 1:39f462024f10 | 116 | //Turning LIGHT to RIGHT |
ptuytsch | 1:39f462024f10 | 117 | if (!buttonDirectionRight) { |
ptuytsch | 1:39f462024f10 | 118 | lightRight=!lightRight; |
ptuytsch | 1:39f462024f10 | 119 | } else { |
ptuytsch | 1:39f462024f10 | 120 | lightRight=0; |
ptuytsch | 1:39f462024f10 | 121 | } |
ptuytsch | 1:39f462024f10 | 122 | //Turning LIGHT to Left |
ptuytsch | 1:39f462024f10 | 123 | if (!buttonDirectionLeft) { |
ptuytsch | 1:39f462024f10 | 124 | lightLeft=!lightLeft; |
ptuytsch | 1:39f462024f10 | 125 | } else { |
ptuytsch | 1:39f462024f10 | 126 | lightLeft=0; |
ptuytsch | 1:39f462024f10 | 127 | } |
ptuytsch | 1:39f462024f10 | 128 | // Check if the Lights need to be on |
ptuytsch | 1:39f462024f10 | 129 | if(!buttonRed) { |
ptuytsch | 1:39f462024f10 | 130 | lightFront=1; |
ptuytsch | 1:39f462024f10 | 131 | if (trailerBat->getBatteryPercentage()<30) {lightBack=!lightBack;} |
ptuytsch | 1:39f462024f10 | 132 | else {lightBack=1;} |
ptuytsch | 1:39f462024f10 | 133 | } else { |
ptuytsch | 1:39f462024f10 | 134 | lightFront=0; |
ptuytsch | 1:39f462024f10 | 135 | if (trailerBat->getBatteryPercentage()<30) {lightBack=!lightBack;} |
ptuytsch | 1:39f462024f10 | 136 | else {lightBack=0;} |
ptuytsch | 1:39f462024f10 | 137 | } |
ptuytsch | 1:39f462024f10 | 138 | } |
ptuytsch | 1:39f462024f10 | 139 | |
ptuytsch | 1:39f462024f10 | 140 | void BikeControl::generatorHallAPulsed() |
ptuytsch | 1:39f462024f10 | 141 | { |
ptuytsch | 1:39f462024f10 | 142 | generatorHallACounter++; |
ptuytsch | 1:39f462024f10 | 143 | } |
ptuytsch | 1:39f462024f10 | 144 | |
ptuytsch | 1:39f462024f10 | 145 | void BikeControl::generatorHallBPulsed() |
ptuytsch | 1:39f462024f10 | 146 | { |
ptuytsch | 1:39f462024f10 | 147 | generatorHallBCounter++; |
ptuytsch | 1:39f462024f10 | 148 | } |
ptuytsch | 1:39f462024f10 | 149 | |
ptuytsch | 1:39f462024f10 | 150 | void BikeControl::motorRightPulsed() |
ptuytsch | 1:39f462024f10 | 151 | { |
ptuytsch | 1:39f462024f10 | 152 | motorRightCounter++; |
ptuytsch | 1:39f462024f10 | 153 | } |
ptuytsch | 1:39f462024f10 | 154 | |
ptuytsch | 1:39f462024f10 | 155 | void BikeControl::motorLeftPulsed() |
ptuytsch | 1:39f462024f10 | 156 | { |
ptuytsch | 1:39f462024f10 | 157 | motorLeftCounter++; |
ptuytsch | 1:39f462024f10 | 158 | } |
ptuytsch | 1:39f462024f10 | 159 | |
ptuytsch | 1:39f462024f10 | 160 | void BikeControl::runTestLight() |
ptuytsch | 1:39f462024f10 | 161 | { |
ptuytsch | 1:39f462024f10 | 162 | printf("front\n"); |
ptuytsch | 1:39f462024f10 | 163 | lightFront =1; |
ptuytsch | 1:39f462024f10 | 164 | //wait(1); |
ptuytsch | 1:39f462024f10 | 165 | for(uint32_t i = 0; i<0xFFFFFF; i++); //WAIT STATEMENT CAUSES FREEZING!!!!! USE FOR LOOP IN STEAD, device can not handle wait and interrupts at same time! |
ptuytsch | 1:39f462024f10 | 166 | lightFront = 0; |
ptuytsch | 1:39f462024f10 | 167 | printf("back\n"); |
ptuytsch | 1:39f462024f10 | 168 | lightBack = 1; |
ptuytsch | 1:39f462024f10 | 169 | //wait(1); |
ptuytsch | 1:39f462024f10 | 170 | for(uint32_t i = 0; i<0xFFFFFF; i++); |
ptuytsch | 1:39f462024f10 | 171 | lightBack = 0; |
ptuytsch | 1:39f462024f10 | 172 | printf("left\n"); |
ptuytsch | 1:39f462024f10 | 173 | lightLeft = 1; |
ptuytsch | 1:39f462024f10 | 174 | //wait(1); |
ptuytsch | 1:39f462024f10 | 175 | for(uint32_t i = 0; i<0xFFFFFF; i++); |
ptuytsch | 1:39f462024f10 | 176 | lightLeft = 0; |
ptuytsch | 1:39f462024f10 | 177 | printf("right\n"); |
ptuytsch | 1:39f462024f10 | 178 | lightRight = 1; |
ptuytsch | 1:39f462024f10 | 179 | //wait(1); |
ptuytsch | 1:39f462024f10 | 180 | for(uint32_t i = 0; i<0xFFFFFF; i++); |
ptuytsch | 1:39f462024f10 | 181 | lightRight = 0; |
ptuytsch | 1:39f462024f10 | 182 | //wait(1); |
ptuytsch | 1:39f462024f10 | 183 | } |
ptuytsch | 1:39f462024f10 | 184 | |
ptuytsch | 1:39f462024f10 | 185 | void BikeControl::checkStatus(){ |
ptuytsch | 1:39f462024f10 | 186 | |
ptuytsch | 1:39f462024f10 | 187 | //STATE MACHINE |
ptuytsch | 1:39f462024f10 | 188 | if (!brakeFront || !brakeRear) { |
ptuytsch | 1:39f462024f10 | 189 | if(!brakeFront) { |
ptuytsch | 1:39f462024f10 | 190 | printf("BRAKE front \n"); |
ptuytsch | 1:39f462024f10 | 191 | } |
ptuytsch | 1:39f462024f10 | 192 | if(!brakeRear) { |
ptuytsch | 1:39f462024f10 | 193 | printf("BRAKE rear \n"); |
ptuytsch | 1:39f462024f10 | 194 | } |
ptuytsch | 1:39f462024f10 | 195 | motorRightCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 196 | motorLeftCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 197 | trailerCtrl=0; |
ptuytsch | 1:39f462024f10 | 198 | } else if (!buttonGreen) { |
ptuytsch | 1:39f462024f10 | 199 | if (motorRightRpm<=61) {// motorRightRpm=61 means 6km/h |
ptuytsch | 1:39f462024f10 | 200 | motorRightCtrl.write(1.0f); |
ptuytsch | 1:39f462024f10 | 201 | motorLeftCtrl.write(1.0f); |
ptuytsch | 1:39f462024f10 | 202 | trailerCtrl=1; |
ptuytsch | 1:39f462024f10 | 203 | } else { |
ptuytsch | 1:39f462024f10 | 204 | if (generatorHallARpm<10) { |
ptuytsch | 1:39f462024f10 | 205 | motorRightCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 206 | motorLeftCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 207 | trailerCtrl=0; |
ptuytsch | 1:39f462024f10 | 208 | } else if(generatorHallARpm<=100) { |
ptuytsch | 1:39f462024f10 | 209 | motorRightCtrl.write(generatorHallARpm*0.004f); |
ptuytsch | 1:39f462024f10 | 210 | motorLeftCtrl.write(generatorHallARpm*0.004f); |
ptuytsch | 1:39f462024f10 | 211 | if (motorRightRpm<190) { //boven de 18km/h enkel de trekker aansturen |
ptuytsch | 1:39f462024f10 | 212 | trailerCtrl=1; |
ptuytsch | 1:39f462024f10 | 213 | } else { |
ptuytsch | 1:39f462024f10 | 214 | trailerCtrl=0; |
ptuytsch | 1:39f462024f10 | 215 | } |
ptuytsch | 1:39f462024f10 | 216 | } else if (generatorHallARpm>100) { |
ptuytsch | 1:39f462024f10 | 217 | motorRightCtrl.write(0.4f); |
ptuytsch | 1:39f462024f10 | 218 | motorLeftCtrl.write(0.4f); |
ptuytsch | 1:39f462024f10 | 219 | if (motorRightRpm<190) { //boven de 18km/h enkel de trekker aansturen |
ptuytsch | 1:39f462024f10 | 220 | trailerCtrl=1; |
ptuytsch | 1:39f462024f10 | 221 | } else { |
ptuytsch | 1:39f462024f10 | 222 | trailerCtrl=0; |
ptuytsch | 1:39f462024f10 | 223 | } |
ptuytsch | 1:39f462024f10 | 224 | } |
ptuytsch | 1:39f462024f10 | 225 | } |
ptuytsch | 1:39f462024f10 | 226 | } else if (generatorHallARpm>=0) { |
ptuytsch | 1:39f462024f10 | 227 | if (generatorHallARpm<10) { |
ptuytsch | 1:39f462024f10 | 228 | motorRightCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 229 | motorLeftCtrl.write(0.0f); |
ptuytsch | 1:39f462024f10 | 230 | trailerCtrl=0; |
ptuytsch | 1:39f462024f10 | 231 | } else if(generatorHallARpm<=100) { |
ptuytsch | 1:39f462024f10 | 232 | motorRightCtrl.write(generatorHallARpm*0.004f); |
ptuytsch | 1:39f462024f10 | 233 | motorLeftCtrl.write(generatorHallARpm*0.004f); |
ptuytsch | 1:39f462024f10 | 234 | if (motorRightRpm<190) { |
ptuytsch | 1:39f462024f10 | 235 | trailerCtrl=1; |
ptuytsch | 1:39f462024f10 | 236 | } else { |
ptuytsch | 1:39f462024f10 | 237 | trailerCtrl=0; |
ptuytsch | 1:39f462024f10 | 238 | } |
ptuytsch | 1:39f462024f10 | 239 | } else if (generatorHallARpm>100) { |
ptuytsch | 1:39f462024f10 | 240 | motorRightCtrl.write(0.4f); |
ptuytsch | 1:39f462024f10 | 241 | motorLeftCtrl.write(0.4f); |
ptuytsch | 1:39f462024f10 | 242 | if (motorRightRpm<190) { |
ptuytsch | 1:39f462024f10 | 243 | trailerCtrl=1; |
ptuytsch | 1:39f462024f10 | 244 | } else { |
ptuytsch | 1:39f462024f10 | 245 | trailerCtrl=0; |
ptuytsch | 1:39f462024f10 | 246 | } |
ptuytsch | 1:39f462024f10 | 247 | } |
ptuytsch | 1:39f462024f10 | 248 | } |
ptuytsch | 1:39f462024f10 | 249 | |
ptuytsch | 0:792a8f167ac0 | 250 | } |