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@11:68ee67d17320, 2018-05-12 (annotated)

Committer:
wengefa1
Date:
Sat May 12 12:19:26 2018 +0000
Revision:
11:68ee67d17320
Parent:
8:73c8188916dc
Child:
13:060d2a90d1c5
regelung an rechter Wand eingef?gt;

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 11:68ee67d17320 63 distance_front = readSensorValue(2); //Distanz zur vorderen Wand wird gemessen
wengefa1 11:68ee67d17320 64 if(readSensorValue(1) < 15.0f)
wengefa1 11:68ee67d17320 65 {
wengefa1 1:056cd61800e9 66 distance_side= readSensorValue(1); //Distanz zur linken Wand wird gemessen
wengefa1 1:056cd61800e9 67 AnzahlZyklen = AnzahlZyklen+1; //Anzahl Regelvorgänge werden gezählt
wengefa1 8:73c8188916dc 68 distance_side = distance_side - 6.9f; //Distanz zwischen Position und Ideallinie wird berechnet
wengefa1 11:68ee67d17320 69 CorrectFactor = distance_side*2.0f; //P Faktor des P-Reglers
wengefa1 11:68ee67d17320 70 }
wengefa1 11:68ee67d17320 71 else
wengefa1 11:68ee67d17320 72 {
wengefa1 11:68ee67d17320 73 distance_side= readSensorValue(3); //Distanz zur linken Wand wird gemessen
wengefa1 11:68ee67d17320 74 AnzahlZyklen = AnzahlZyklen+1; //Anzahl Regelvorgänge werden gezählt
wengefa1 11:68ee67d17320 75 distance_side = distance_side - 6.9f; //Distanz zwischen Position und Ideallinie wird berechnet
wengefa1 11:68ee67d17320 76 CorrectFactor = distance_side*-2.0f; //P Faktor des P-Reglers
wengefa1 11:68ee67d17320 77 }
wengefa1 11:68ee67d17320 78
wengefa1 1:056cd61800e9 79
wengefa1 1:056cd61800e9 80 //printf("%f\n",CorrectFactor);
wengefa1 11:68ee67d17320 81 if(abs(CorrectFactor) <= 16.0f) { //erkennt Wand oder Lücke auf linker Seite
wengefa1 7:b5bf886ae13c 82 controller.setDesiredSpeedLeft((Speed) - CorrectFactor); //Bei Wand wird korrigiert
wengefa1 7:b5bf886ae13c 83 controller.setDesiredSpeedRight((Speed * (-1.0f)) - CorrectFactor);
wengefa1 1:056cd61800e9 84 } else {
wengefa1 7:b5bf886ae13c 85 controller.setDesiredSpeedRight(Speed * (-1.0f)); //Bei Lücke wird nach Counter gefahren
wengefa1 7:b5bf886ae13c 86 controller.setDesiredSpeedLeft(Speed);
wengefa1 1:056cd61800e9 87 }
wengefa1 1:056cd61800e9 88
wengefa1 1:056cd61800e9 89 if(distance_front <= 3)
wengefa1 1:056cd61800e9 90 {
wengefa1 1:056cd61800e9 91 break;
wengefa1 1:056cd61800e9 92 }
wengefa1 1:056cd61800e9 93
wengefa1 1:056cd61800e9 94
wengefa1 1:056cd61800e9 95 }
wengefa1 1:056cd61800e9 96 stop();
wengefa1 8:73c8188916dc 97 //printf("Das Programm wurde: %d durchlaufen\n", AnzahlZyklen);
wengefa1 1:056cd61800e9 98 }
wengefa1 1:056cd61800e9 99
wengefa1 1:056cd61800e9 100 int turnRight(int direction)
wengefa1 1:056cd61800e9 101 {
wengefa1 8:73c8188916dc 102 controller.setDesiredSpeedLeft(40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 8:73c8188916dc 103 controller.setDesiredSpeedRight(40.0f);
wengefa1 11:68ee67d17320 104 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
wengefa1 1:056cd61800e9 105 //do nothing
wengefa1 1:056cd61800e9 106 }
wengefa1 1:056cd61800e9 107 stop();
wengefa1 1:056cd61800e9 108 direction = direction + 1;
wengefa1 1:056cd61800e9 109 if(direction == 5)
wengefa1 1:056cd61800e9 110 {
wengefa1 1:056cd61800e9 111 direction = 1;
wengefa1 1:056cd61800e9 112 }
wengefa1 1:056cd61800e9 113 return direction;
wengefa1 1:056cd61800e9 114 }
wengefa1 1:056cd61800e9 115
wengefa1 1:056cd61800e9 116 int turnLeft(int direction)
wengefa1 1:056cd61800e9 117 {
wengefa1 8:73c8188916dc 118 controller.setDesiredSpeedRight(-40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 8:73c8188916dc 119 controller.setDesiredSpeedLeft(-40.0f);
wengefa1 11:68ee67d17320 120 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
wengefa1 1:056cd61800e9 121 //do nothing
wengefa1 1:056cd61800e9 122 }
wengefa1 1:056cd61800e9 123 stop();
wengefa1 1:056cd61800e9 124 direction = direction - 1;
wengefa1 1:056cd61800e9 125 if(direction == 0)
wengefa1 1:056cd61800e9 126 {
wengefa1 1:056cd61800e9 127 direction = 4;
wengefa1 1:056cd61800e9 128 }
wengefa1 1:056cd61800e9 129 return direction;
wengefa1 1:056cd61800e9 130 }
wengefa1 1:056cd61800e9 131
wengefa1 1:056cd61800e9 132
wengefa1 1:056cd61800e9 133 void stop(void)
wengefa1 1:056cd61800e9 134 {
wengefa1 1:056cd61800e9 135 controller.setDesiredSpeedRight(0.0f);
wengefa1 1:056cd61800e9 136 controller.setDesiredSpeedLeft(0.0f);
wengefa1 1:056cd61800e9 137 resetCounter();
wengefa1 1:056cd61800e9 138
wengefa1 1:056cd61800e9 139 }
wengefa1 1:056cd61800e9 140
wengefa1 1:056cd61800e9 141 void resetCounter(void)
wengefa1 1:056cd61800e9 142 {
wengefa1 1:056cd61800e9 143 controller.DetachTicker();
wengefa1 1:056cd61800e9 144 counterLeft.reset();
wengefa1 1:056cd61800e9 145 counterRight.reset();
wengefa1 1:056cd61800e9 146 controller.setDesiredSpeedRight(0.0f);
wengefa1 1:056cd61800e9 147 controller.setDesiredSpeedLeft(0.0f);
wengefa1 1:056cd61800e9 148 controller.AttachTicker();
wengefa1 1:056cd61800e9 149
wengefa1 1:056cd61800e9 150 }
wengefa1 1:056cd61800e9 151
wengefa1 1:056cd61800e9 152 void driveDist(int Distance, int Direction)
wengefa1 1:056cd61800e9 153 {
wengefa1 7:b5bf886ae13c 154 controller.setDesiredSpeedRight(-20.0f*Direction); // Drehzahl in [rpm]
wengefa1 7:b5bf886ae13c 155 controller.setDesiredSpeedLeft(20.0f*Direction);
wengefa1 1:056cd61800e9 156
wengefa1 1:056cd61800e9 157 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 100*Distance) {
wengefa1 1:056cd61800e9 158 //Fährt bis Anzahl umdrehungen erreicht sind
wengefa1 1:056cd61800e9 159 }
wengefa1 1:056cd61800e9 160 stop();
wengefa1 1:056cd61800e9 161 }
wengefa1 1:056cd61800e9 162