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@15:31d09ee65cf1, 2018-07-04 (annotated)

Committer:
wengefa1
Date:
Wed Jul 04 09:14:37 2018 +0000
Revision:
15:31d09ee65cf1
Parent:
8:73c8188916dc 
Schalter f?r Moduswechsel implementiert

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