Fabian Wenger / Mbed 2 deprecated Rainbow

Grundfunktionen für Micromouse

Dependencies:   AutomationElements mbed

MotorDriver.cpp@1:4808f55970e8, 2018-04-22 (annotated)

Committer:
wengefa1
Date:
Sun Apr 22 09:55:40 2018 +0000
Revision:
1:4808f55970e8
Parent:
0:e38b500d6e74 
Grundfunktionen funktionieren

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wengefa1 0:e38b500d6e74 1 #include "MotorDriver.h"
wengefa1 0:e38b500d6e74 2 #include "Controller.h"
wengefa1 0:e38b500d6e74 3 #include "ReadSensor.h"
wengefa1 0:e38b500d6e74 4 #include "mbed.h"
wengefa1 0:e38b500d6e74 5
wengefa1 0:e38b500d6e74 6 #define WheelDiameter 3;
wengefa1 0:e38b500d6e74 7 #define ratio 0.04;
wengefa1 0:e38b500d6e74 8
wengefa1 0:e38b500d6e74 9 DigitalOut enableMotorDriver(PB_2);
wengefa1 0:e38b500d6e74 10 DigitalIn motorDriverFault(PB_14);
wengefa1 0:e38b500d6e74 11 DigitalIn motorDriverWarning(PB_15);
wengefa1 0:e38b500d6e74 12
wengefa1 0:e38b500d6e74 13 PwmOut pwmLeft(PA_8);
wengefa1 0:e38b500d6e74 14 PwmOut pwmRight(PA_9);
wengefa1 0:e38b500d6e74 15
wengefa1 0:e38b500d6e74 16 EncoderCounter counterLeft(PB_6, PB_7);
wengefa1 0:e38b500d6e74 17 EncoderCounter counterRight(PA_6, PC_7);
wengefa1 0:e38b500d6e74 18
wengefa1 1:4808f55970e8 19 DigitalOut myled(LED2);
wengefa1 1:4808f55970e8 20
wengefa1 0:e38b500d6e74 21 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
wengefa1 0:e38b500d6e74 22
wengefa1 0:e38b500d6e74 23 void resetCounter(void);
wengefa1 0:e38b500d6e74 24
wengefa1 0:e38b500d6e74 25 void startup (void)
wengefa1 0:e38b500d6e74 26 {
wengefa1 0:e38b500d6e74 27 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
wengefa1 0:e38b500d6e74 28 counterLeft.reset();
wengefa1 0:e38b500d6e74 29 counterRight.reset();
wengefa1 0:e38b500d6e74 30 }
wengefa1 0:e38b500d6e74 31
wengefa1 1:4808f55970e8 32 void driveOne(int NumbField, int SpeedMode)
wengefa1 0:e38b500d6e74 33
wengefa1 1:4808f55970e8 34 {
wengefa1 1:4808f55970e8 35 int AnzahlZyklen = 0;
wengefa1 1:4808f55970e8 36 float distance;
wengefa1 1:4808f55970e8 37 float CorrectFactor;
wengefa1 1:4808f55970e8 38 float Speed = 0.0f;
wengefa1 0:e38b500d6e74 39
wengefa1 1:4808f55970e8 40 switch(SpeedMode) {
wengefa1 1:4808f55970e8 41 case 1:
wengefa1 1:4808f55970e8 42 Speed = 20.0f;
wengefa1 0:e38b500d6e74 43 break;
wengefa1 0:e38b500d6e74 44
wengefa1 0:e38b500d6e74 45
wengefa1 0:e38b500d6e74 46 case 2:
wengefa1 1:4808f55970e8 47 Speed = 40.0f;
wengefa1 0:e38b500d6e74 48 break;
wengefa1 0:e38b500d6e74 49
wengefa1 0:e38b500d6e74 50 case 3:
wengefa1 1:4808f55970e8 51 Speed = 80.0f;
wengefa1 0:e38b500d6e74 52 break;
wengefa1 0:e38b500d6e74 53
wengefa1 0:e38b500d6e74 54
wengefa1 0:e38b500d6e74 55 default:
wengefa1 0:e38b500d6e74 56
wengefa1 0:e38b500d6e74 57 break;
wengefa1 0:e38b500d6e74 58 }
wengefa1 1:4808f55970e8 59
wengefa1 1:4808f55970e8 60 //Fährt bis Anzahl umdrehungen erreicht sind
wengefa1 1:4808f55970e8 61 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
wengefa1 1:4808f55970e8 62 distance = readSensorValue(1); //Distanz zur linken Wand wird gemessen
wengefa1 1:4808f55970e8 63 AnzahlZyklen = AnzahlZyklen+1; //Anzahl Regelvorgänge werden gezählt
wengefa1 1:4808f55970e8 64 distance = distance - 7.8f; //Distanz zwischen Position und Ideallinie wird berechnet
wengefa1 1:4808f55970e8 65 CorrectFactor = distance*0.8f; //P Faktor des P-Reglers
wengefa1 1:4808f55970e8 66
wengefa1 1:4808f55970e8 67 //printf("%f\n",CorrectFactor);
wengefa1 1:4808f55970e8 68 if(abs(CorrectFactor) <= 5.0f) { //erkennt Wand oder Lücke auf linker Seite
wengefa1 1:4808f55970e8 69 controller.setDesiredSpeedLeft((Speed * (-1.0f)) - CorrectFactor); //Bei Wand wird korrigiert
wengefa1 1:4808f55970e8 70 controller.setDesiredSpeedRight(Speed - CorrectFactor);
wengefa1 1:4808f55970e8 71 } else {
wengefa1 1:4808f55970e8 72 controller.setDesiredSpeedLeft(Speed * (-1.0f)); //Bei Lücke wird nach Counter gefahren
wengefa1 1:4808f55970e8 73 controller.setDesiredSpeedRight(Speed);
wengefa1 1:4808f55970e8 74 }
wengefa1 1:4808f55970e8 75 }
wengefa1 1:4808f55970e8 76 stop();
wengefa1 1:4808f55970e8 77 printf("Das Programm wurde: %d durchlaufen", AnzahlZyklen);
wengefa1 0:e38b500d6e74 78 }
wengefa1 0:e38b500d6e74 79
wengefa1 0:e38b500d6e74 80 int turnRight(int direction)
wengefa1 0:e38b500d6e74 81 {
wengefa1 0:e38b500d6e74 82 controller.setDesiredSpeedLeft(15.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 0:e38b500d6e74 83 controller.setDesiredSpeedRight(15.0f);
wengefa1 0:e38b500d6e74 84 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
wengefa1 0:e38b500d6e74 85 //do nothing
wengefa1 0:e38b500d6e74 86 }
wengefa1 0:e38b500d6e74 87 stop();
wengefa1 0:e38b500d6e74 88 direction = direction + 1;
wengefa1 0:e38b500d6e74 89 return direction;
wengefa1 0:e38b500d6e74 90 }
wengefa1 0:e38b500d6e74 91
wengefa1 0:e38b500d6e74 92 int turnLeft(int direction)
wengefa1 0:e38b500d6e74 93 {
wengefa1 0:e38b500d6e74 94 controller.setDesiredSpeedLeft(-15.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 0:e38b500d6e74 95 controller.setDesiredSpeedRight(-15.0f);
wengefa1 0:e38b500d6e74 96 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
wengefa1 0:e38b500d6e74 97 //do nothing
wengefa1 0:e38b500d6e74 98 }
wengefa1 0:e38b500d6e74 99 stop();
wengefa1 0:e38b500d6e74 100 direction = direction - 1;
wengefa1 0:e38b500d6e74 101 return direction;
wengefa1 0:e38b500d6e74 102 }
wengefa1 0:e38b500d6e74 103
wengefa1 0:e38b500d6e74 104
wengefa1 0:e38b500d6e74 105 void stop(void)
wengefa1 0:e38b500d6e74 106 {
wengefa1 0:e38b500d6e74 107 controller.setDesiredSpeedRight(0.0f);
wengefa1 0:e38b500d6e74 108 controller.setDesiredSpeedLeft(0.0f);
wengefa1 0:e38b500d6e74 109 resetCounter();
wengefa1 0:e38b500d6e74 110
wengefa1 0:e38b500d6e74 111 }
wengefa1 0:e38b500d6e74 112
wengefa1 0:e38b500d6e74 113 void resetCounter(void)
wengefa1 0:e38b500d6e74 114 {
wengefa1 0:e38b500d6e74 115 controller.DetachTicker();
wengefa1 0:e38b500d6e74 116 counterLeft.reset();
wengefa1 0:e38b500d6e74 117 counterRight.reset();
wengefa1 0:e38b500d6e74 118 controller.setDesiredSpeedRight(0.0f);
wengefa1 0:e38b500d6e74 119 controller.setDesiredSpeedLeft(0.0f);
wengefa1 0:e38b500d6e74 120 controller.AttachTicker();
wengefa1 0:e38b500d6e74 121
wengefa1 0:e38b500d6e74 122 }
wengefa1 0:e38b500d6e74 123
wengefa1 0:e38b500d6e74 124 void driveDist(int Distance, int Direction)
wengefa1 0:e38b500d6e74 125 {
wengefa1 0:e38b500d6e74 126 controller.setDesiredSpeedLeft(-20.0f*Direction); // Drehzahl in [rpm]
wengefa1 0:e38b500d6e74 127 controller.setDesiredSpeedRight(20.0f*Direction);
wengefa1 0:e38b500d6e74 128
wengefa1 0:e38b500d6e74 129 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 100*Distance) {
wengefa1 0:e38b500d6e74 130 //Fährt bis Anzahl umdrehungen erreicht sind
wengefa1 0:e38b500d6e74 131 }
wengefa1 0:e38b500d6e74 132 stop();
wengefa1 0:e38b500d6e74 133 }
wengefa1 0:e38b500d6e74 134