BikeControl.cpp

00001 #include "mbed.h"
00002 #include "BikeControl.h"
00003 #include "BikeData.h"
00004 #include "BatteryState.h"
00005 
00006 BikeControl::BikeControl(BikeData* bikeData, BatteryState* trailerBat, BatteryState* bikeBat, BatteryState* auxBat):
00007     trailerCtrl(PB_1),
00008     motorRightCtrl(PB_15),
00009     motorRightCounter(0),
00010     motorRightRpm(0),
00011     motorLeftCtrl(PB_13),
00012     motorLeftCounter(0),
00013     motorLeftRpm(0),
00014     motorRightHall(PC_13),
00015     motorLeftHall(PC_1),
00016     
00017     //BRAKE
00018     brakeFront(PC_10),
00019     brakeRear(PC_12),
00020     
00021     //GENERATOR
00022     generatorHallA(PD_2),
00023     generatorBrake(PC_9),
00024     generatorHallACounter(0),
00025     generatorHallARpm(0),
00026     generatorHallB(PC_3),
00027     generatorHallBCounter(0),
00028     generatorHallBRpm(0),
00029     
00030     //BUTTONS ON STEERING
00031     //userButton(USER_BUTTON),
00032     buttonGreen(PB_11),
00033     buttonRed(PB_12),
00034     buttonDirectionRight(PA_15),
00035     buttonDirectionLeft(PB_7),
00036     
00037     //SWITCH
00038     switchOn(PC_11),
00039     switchWalk(PB_2),
00040     
00041     //LIGHT
00042     lightFront(PB_14),
00043     lightBack(PA_11),
00044     lightLeft(PC_6),
00045     lightRight(PC_8),
00046     
00047     bd(bikeData),
00048     bikeBat(bikeBat),
00049     trailerBat(trailerBat),
00050     auxBat(auxBat)
00051 {
00052         //PullUp on userButton input
00053         //userButton.mode(PullUp);
00054         //PullUp on brake inputs
00055         brakeFront.mode(PullUp);
00056         brakeRear.mode(PullUp);
00057     
00058         //Call function on rise of interruptpin
00059         generatorHallA.rise(this, &BikeControl::generatorHallAPulsed);
00060         generatorHallB.rise(this, &BikeControl::generatorHallBPulsed);
00061         generatorBrake.period_us(50);
00062         generatorBrake.write(0.5f);
00063         generatorHallACounter=0;
00064         generatorHallARpm=0;
00065         generatorHallBCounter=0;
00066         generatorHallBRpm=0;
00067     
00068         //motor
00069         motorRightCtrl.period_ms(100);
00070         motorRightCtrl.write(0.0f);
00071         motorRightHall.rise(this, &BikeControl::motorRightPulsed);
00072         motorLeftCtrl.period_ms(100);
00073         motorLeftCtrl.write(0.0f);
00074         motorLeftHall.rise(this, &BikeControl::motorLeftPulsed);
00075     
00076         //Button inputs
00077         buttonGreen.mode(PullUp);
00078         buttonRed.mode(PullUp);
00079         buttonDirectionRight.mode(PullUp);
00080         buttonDirectionLeft.mode(PullUp);
00081     
00082         //SWITCH
00083         switchOn.mode(PullUp);
00084         switchWalk.mode(PullUp);
00085         
00086         //t.attach(this, &BikeControl::periodicCallback, 0.5);
00087     }
00088     
00089 void BikeControl::startControlLoop(){
00090     t.attach(this, &BikeControl::periodicCallback, 0.5);
00091     }
00092     
00093 void BikeControl::periodicCallback(){
00094         //printf("callback\n");
00095         generatorHallARpm = generatorHallACounter*2; //60 rising edges/ rotation means Rpm is equal to the counter value in 1 sec
00096         generatorHallACounter = 0;
00097         generatorHallBRpm = generatorHallBCounter*2; //60 rising edges/ rotation means Rpm is equal to the counter value in 1 sec
00098         generatorHallBCounter = 0;
00099         motorRightRpm = motorRightCounter*2;
00100         motorRightCounter = 0;
00101         motorLeftRpm = motorLeftCounter*2;
00102         motorLeftCounter = 0;
00103         if (generatorHallARpm>0) {
00104             printf("RPM generator Hall A = %i\n",generatorHallARpm);
00105         }
00106         if (generatorHallBRpm>0) {
00107             printf("RPM generator Hall B = %i\n",generatorHallBRpm);
00108         }
00109         if (motorRightRpm>0) {
00110             printf("RPM motor Right = %i\n",motorRightRpm);
00111         }
00112         if (motorLeftRpm>0) {
00113             printf("RPM motor Left = %i\n",motorLeftRpm);
00114         }
00115         //printf("light control\n");
00116         //Turning LIGHT to RIGHT
00117         if (!buttonDirectionRight) {
00118             lightRight=!lightRight;
00119         } else {
00120             lightRight=0;
00121         }
00122         //Turning LIGHT to Left
00123         if (!buttonDirectionLeft) {
00124             lightLeft=!lightLeft;
00125         } else {
00126             lightLeft=0;
00127         }
00128         // Check if the Lights need to be on
00129         if(!buttonRed) {
00130             lightFront=1;
00131             if (trailerBat->getBatteryPercentage()<30) {lightBack=!lightBack;}
00132             else {lightBack=1;}
00133         } else {
00134             lightFront=0;
00135             if (trailerBat->getBatteryPercentage()<30) {lightBack=!lightBack;}
00136             else {lightBack=0;}
00137         }
00138     }
00139     
00140 void BikeControl::generatorHallAPulsed()
00141 {
00142     generatorHallACounter++;
00143 }
00144 
00145 void BikeControl::generatorHallBPulsed()
00146 {
00147     generatorHallBCounter++;
00148 }
00149 
00150 void BikeControl::motorRightPulsed()
00151 {
00152     motorRightCounter++;
00153 }
00154 
00155 void BikeControl::motorLeftPulsed()
00156 {
00157     motorLeftCounter++;
00158 }
00159     
00160 void BikeControl::runTestLight()
00161 {
00162     printf("front\n");
00163     lightFront =1;
00164     //wait(1);
00165     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!
00166     lightFront = 0;
00167     printf("back\n");
00168     lightBack = 1;
00169     //wait(1);
00170     for(uint32_t i = 0; i<0xFFFFFF; i++);
00171     lightBack = 0;
00172     printf("left\n");
00173     lightLeft = 1;
00174     //wait(1);
00175     for(uint32_t i = 0; i<0xFFFFFF; i++);
00176     lightLeft = 0;
00177     printf("right\n");
00178     lightRight = 1;
00179     //wait(1);
00180     for(uint32_t i = 0; i<0xFFFFFF; i++);
00181     lightRight = 0;
00182     //wait(1);
00183 }
00184 
00185 void BikeControl::checkStatus(){
00186         
00187         //STATE MACHINE
00188         if (!brakeFront || !brakeRear) {
00189             if(!brakeFront) {
00190                 printf("BRAKE front \n");
00191             }
00192             if(!brakeRear) {
00193                 printf("BRAKE rear \n");
00194             }
00195             motorRightCtrl.write(0.0f);
00196             motorLeftCtrl.write(0.0f);
00197             trailerCtrl=0;
00198         } else if (!buttonGreen) {
00199             if (motorRightRpm<=61) {// motorRightRpm=61 means 6km/h
00200                 motorRightCtrl.write(1.0f);
00201                 motorLeftCtrl.write(1.0f);
00202                 trailerCtrl=1;
00203             } else {
00204                 if (generatorHallARpm<10) {
00205                     motorRightCtrl.write(0.0f);
00206                     motorLeftCtrl.write(0.0f);
00207                     trailerCtrl=0;
00208                 } else if(generatorHallARpm<=100) {
00209                     motorRightCtrl.write(generatorHallARpm*0.004f);
00210                     motorLeftCtrl.write(generatorHallARpm*0.004f);
00211                     if (motorRightRpm<190) { //boven de 18km/h enkel de trekker aansturen
00212                         trailerCtrl=1;
00213                     } else {
00214                         trailerCtrl=0;
00215                     }
00216                 } else if (generatorHallARpm>100) {
00217                     motorRightCtrl.write(0.4f);
00218                     motorLeftCtrl.write(0.4f);
00219                     if (motorRightRpm<190) { //boven de 18km/h enkel de trekker aansturen
00220                         trailerCtrl=1;
00221                     } else {
00222                         trailerCtrl=0;
00223                     }
00224                 }
00225             }
00226         } else if (generatorHallARpm>=0) {
00227             if (generatorHallARpm<10) {
00228                 motorRightCtrl.write(0.0f);
00229                 motorLeftCtrl.write(0.0f);
00230                 trailerCtrl=0;
00231             } else if(generatorHallARpm<=100) {
00232                 motorRightCtrl.write(generatorHallARpm*0.004f);
00233                 motorLeftCtrl.write(generatorHallARpm*0.004f);
00234                 if (motorRightRpm<190) {
00235                     trailerCtrl=1;
00236                 } else {
00237                     trailerCtrl=0;
00238                 }
00239             } else if (generatorHallARpm>100) {
00240                 motorRightCtrl.write(0.4f);
00241                 motorLeftCtrl.write(0.4f);
00242                 if (motorRightRpm<190) {
00243                     trailerCtrl=1;
00244                 } else {
00245                     trailerCtrl=0;
00246                 }
00247             }
00248         }
00249     
00250     }