RainbowTeam / Mbed 2 deprecated ProjectTheseus

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

MotorDriver.cpp@8:73c8188916dc, 2018-05-07 (annotated)

Committer:
wengefa1
Date:
Mon May 07 16:30:51 2018 +0000
Revision:
8:73c8188916dc
Parent:
7:b5bf886ae13c
Child:
11:68ee67d17320
Child:
15:31d09ee65cf1
es l?uft

Who changed what in which revision?

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