The Opportunity Factory / BikeControl

Library to control the bike (just basic for now)

Dependents:   TORTUGA_BLE

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?

UserRevisionLine numberNew 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 }