Fabian Wenger / Mbed 2 deprecated Rainbow

Grundfunktionen für Micromouse

Dependencies:   AutomationElements mbed

MotorDriver.cpp@0:e38b500d6e74, 2018-04-19 (annotated)

Committer:
wengefa1
Date:
Thu Apr 19 11:31:49 2018 +0000
Revision:
0:e38b500d6e74
Child:
1:4808f55970e8
keine Regelung vorhanden

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 0:e38b500d6e74 19 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
wengefa1 0:e38b500d6e74 20
wengefa1 0:e38b500d6e74 21 void resetCounter(void);
wengefa1 0:e38b500d6e74 22
wengefa1 0:e38b500d6e74 23 void startup (void)
wengefa1 0:e38b500d6e74 24 {
wengefa1 0:e38b500d6e74 25 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
wengefa1 0:e38b500d6e74 26 counterLeft.reset();
wengefa1 0:e38b500d6e74 27 counterRight.reset();
wengefa1 0:e38b500d6e74 28 }
wengefa1 0:e38b500d6e74 29
wengefa1 0:e38b500d6e74 30 void driveOne(int NumbField, int Speed)
wengefa1 0:e38b500d6e74 31 {
wengefa1 0:e38b500d6e74 32 switch(Speed) {
wengefa1 0:e38b500d6e74 33 case 1:
wengefa1 0:e38b500d6e74 34
wengefa1 0:e38b500d6e74 35 controller.setDesiredSpeedLeft(-30.0f); // Drehzahl in [rpm]
wengefa1 0:e38b500d6e74 36 controller.setDesiredSpeedRight(30.0f);
wengefa1 0:e38b500d6e74 37 printf("SpeedSet\n");
wengefa1 0:e38b500d6e74 38 //Fährt bis Anzahl umdrehungen erreicht sind
wengefa1 0:e38b500d6e74 39 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
wengefa1 0:e38b500d6e74 40
wengefa1 0:e38b500d6e74 41 }
wengefa1 0:e38b500d6e74 42 printf("number erreicht");
wengefa1 0:e38b500d6e74 43 stop();
wengefa1 0:e38b500d6e74 44
wengefa1 0:e38b500d6e74 45 break;
wengefa1 0:e38b500d6e74 46
wengefa1 0:e38b500d6e74 47
wengefa1 0:e38b500d6e74 48 case 2:
wengefa1 0:e38b500d6e74 49 controller.setDesiredSpeedLeft(-150.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 0:e38b500d6e74 50 controller.setDesiredSpeedRight(150.0f);
wengefa1 0:e38b500d6e74 51 //Fährt bis Anzahl umdrehungen erreicht sind
wengefa1 0:e38b500d6e74 52 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
wengefa1 0:e38b500d6e74 53
wengefa1 0:e38b500d6e74 54 }
wengefa1 0:e38b500d6e74 55 stop();
wengefa1 0:e38b500d6e74 56
wengefa1 0:e38b500d6e74 57 break;
wengefa1 0:e38b500d6e74 58
wengefa1 0:e38b500d6e74 59 case 3:
wengefa1 0:e38b500d6e74 60 controller.setDesiredSpeedLeft(-20.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 0:e38b500d6e74 61 controller.setDesiredSpeedRight(20.0f);
wengefa1 0:e38b500d6e74 62 //Fährt bis Anzahl umdrehungen erreicht sind
wengefa1 0:e38b500d6e74 63 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
wengefa1 0:e38b500d6e74 64 //do nothing
wengefa1 0:e38b500d6e74 65 }
wengefa1 0:e38b500d6e74 66 stop();
wengefa1 0:e38b500d6e74 67
wengefa1 0:e38b500d6e74 68 break;
wengefa1 0:e38b500d6e74 69
wengefa1 0:e38b500d6e74 70
wengefa1 0:e38b500d6e74 71 default:
wengefa1 0:e38b500d6e74 72
wengefa1 0:e38b500d6e74 73 break;
wengefa1 0:e38b500d6e74 74 }
wengefa1 0:e38b500d6e74 75 }
wengefa1 0:e38b500d6e74 76
wengefa1 0:e38b500d6e74 77 int turnRight(int direction)
wengefa1 0:e38b500d6e74 78 {
wengefa1 0:e38b500d6e74 79 controller.setDesiredSpeedLeft(15.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 0:e38b500d6e74 80 controller.setDesiredSpeedRight(15.0f);
wengefa1 0:e38b500d6e74 81 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
wengefa1 0:e38b500d6e74 82 //do nothing
wengefa1 0:e38b500d6e74 83 }
wengefa1 0:e38b500d6e74 84 stop();
wengefa1 0:e38b500d6e74 85 direction = direction + 1;
wengefa1 0:e38b500d6e74 86 return direction;
wengefa1 0:e38b500d6e74 87 }
wengefa1 0:e38b500d6e74 88
wengefa1 0:e38b500d6e74 89 int turnLeft(int direction)
wengefa1 0:e38b500d6e74 90 {
wengefa1 0:e38b500d6e74 91 controller.setDesiredSpeedLeft(-15.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
wengefa1 0:e38b500d6e74 92 controller.setDesiredSpeedRight(-15.0f);
wengefa1 0:e38b500d6e74 93 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
wengefa1 0:e38b500d6e74 94 //do nothing
wengefa1 0:e38b500d6e74 95 }
wengefa1 0:e38b500d6e74 96 stop();
wengefa1 0:e38b500d6e74 97 direction = direction - 1;
wengefa1 0:e38b500d6e74 98 return direction;
wengefa1 0:e38b500d6e74 99 }
wengefa1 0:e38b500d6e74 100
wengefa1 0:e38b500d6e74 101
wengefa1 0:e38b500d6e74 102 void stop(void)
wengefa1 0:e38b500d6e74 103 {
wengefa1 0:e38b500d6e74 104 controller.setDesiredSpeedRight(0.0f);
wengefa1 0:e38b500d6e74 105 controller.setDesiredSpeedLeft(0.0f);
wengefa1 0:e38b500d6e74 106 resetCounter();
wengefa1 0:e38b500d6e74 107
wengefa1 0:e38b500d6e74 108 }
wengefa1 0:e38b500d6e74 109
wengefa1 0:e38b500d6e74 110 void resetCounter(void)
wengefa1 0:e38b500d6e74 111 {
wengefa1 0:e38b500d6e74 112 controller.DetachTicker();
wengefa1 0:e38b500d6e74 113 counterLeft.reset();
wengefa1 0:e38b500d6e74 114 counterRight.reset();
wengefa1 0:e38b500d6e74 115 controller.setDesiredSpeedRight(0.0f);
wengefa1 0:e38b500d6e74 116 controller.setDesiredSpeedLeft(0.0f);
wengefa1 0:e38b500d6e74 117 controller.AttachTicker();
wengefa1 0:e38b500d6e74 118
wengefa1 0:e38b500d6e74 119 }
wengefa1 0:e38b500d6e74 120
wengefa1 0:e38b500d6e74 121 void driveDist(int Distance, int Direction)
wengefa1 0:e38b500d6e74 122 {
wengefa1 0:e38b500d6e74 123 controller.setDesiredSpeedLeft(-20.0f*Direction); // Drehzahl in [rpm]
wengefa1 0:e38b500d6e74 124 controller.setDesiredSpeedRight(20.0f*Direction);
wengefa1 0:e38b500d6e74 125
wengefa1 0:e38b500d6e74 126 while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 100*Distance) {
wengefa1 0:e38b500d6e74 127 //Fährt bis Anzahl umdrehungen erreicht sind
wengefa1 0:e38b500d6e74 128 }
wengefa1 0:e38b500d6e74 129 stop();
wengefa1 0:e38b500d6e74 130 }
wengefa1 0:e38b500d6e74 131
wengefa1 0:e38b500d6e74 132 void orientate(void)
wengefa1 0:e38b500d6e74 133 {
wengefa1 0:e38b500d6e74 134 while(ReadSensorValue(1) > ReadSensorValue(3))
wengefa1 0:e38b500d6e74 135 {
wengefa1 0:e38b500d6e74 136 driveDist(1,1);
wengefa1 0:e38b500d6e74 137 }
wengefa1 0:e38b500d6e74 138 while(ReadSensorValue(1) < ReadSensorValue(3))
wengefa1 0:e38b500d6e74 139 {
wengefa1 0:e38b500d6e74 140 driveDist(1,-1);
wengefa1 0:e38b500d6e74 141 }
wengefa1 0:e38b500d6e74 142 //Differenz Sensor zu Drehpunkt vorwärts fahren
wengefa1 0:e38b500d6e74 143
wengefa1 0:e38b500d6e74 144 }
wengefa1 0:e38b500d6e74 145