Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@0:8491169be8fc, 2018-04-16 (annotated)

Committer:
Helvis
Date:
Mon Apr 16 10:27:10 2018 +0000
Revision:
0:8491169be8fc
Child:
1:2b5f79285a3e
Algorithmus Erste Fahrt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Helvis 0:8491169be8fc 1 #include <mbed.h>
Helvis 0:8491169be8fc 2
Helvis 0:8491169be8fc 3 #include "EncoderCounter.h"
Helvis 0:8491169be8fc 4 #include "Controller.h"
Helvis 0:8491169be8fc 5 #include "IRSensor.h"
Helvis 0:8491169be8fc 6 #include "Motion.h"
Helvis 0:8491169be8fc 7
Helvis 0:8491169be8fc 8 //User Button
Helvis 0:8491169be8fc 9 InterruptIn button(USER_BUTTON);
Helvis 0:8491169be8fc 10
Helvis 0:8491169be8fc 11 //Sensors:
Helvis 0:8491169be8fc 12
Helvis 0:8491169be8fc 13 AnalogIn lineSensor(PA_4);
Helvis 0:8491169be8fc 14 AnalogIn distance2(PC_3);
Helvis 0:8491169be8fc 15 AnalogIn distance4(PB_1);
Helvis 0:8491169be8fc 16 AnalogIn distance1(PC_2);
Helvis 0:8491169be8fc 17 AnalogIn distance3(PC_5);
Helvis 0:8491169be8fc 18
Helvis 0:8491169be8fc 19 IRSensor irSensorL (distance2);
Helvis 0:8491169be8fc 20 IRSensor irSensorC (distance4);
Helvis 0:8491169be8fc 21 IRSensor irSensorR (distance1);
Helvis 0:8491169be8fc 22 IRSensor irSensorB (distance3);
Helvis 0:8491169be8fc 23
Helvis 0:8491169be8fc 24 //Motors:
Helvis 0:8491169be8fc 25 DigitalOut myled(LED1);
Helvis 0:8491169be8fc 26
Helvis 0:8491169be8fc 27 DigitalOut enableMotorDriver(PB_2);
Helvis 0:8491169be8fc 28
Helvis 0:8491169be8fc 29 DigitalIn motorDriverFault(PB_14);
Helvis 0:8491169be8fc 30 DigitalIn motorDriverWarning(PB_15);
Helvis 0:8491169be8fc 31
Helvis 0:8491169be8fc 32 PwmOut pwmRight(PA_8);
Helvis 0:8491169be8fc 33 PwmOut pwmLeft(PA_10);
Helvis 0:8491169be8fc 34
Helvis 0:8491169be8fc 35 EncoderCounter counterRight(PB_6, PB_7);
Helvis 0:8491169be8fc 36 EncoderCounter counterLeft(PA_0, PA_1);
Helvis 0:8491169be8fc 37
Helvis 0:8491169be8fc 38
Helvis 0:8491169be8fc 39 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Helvis 0:8491169be8fc 40
Helvis 0:8491169be8fc 41 Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR, enableMotorDriver);
Helvis 0:8491169be8fc 42
Helvis 0:8491169be8fc 43 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 44
Helvis 0:8491169be8fc 45 volatile int start = 0;
Helvis 0:8491169be8fc 46 short countsLeft = 0;
Helvis 0:8491169be8fc 47 short countsRight = 0;
Helvis 0:8491169be8fc 48 short countsLeftOld = 0;
Helvis 0:8491169be8fc 49 short countsRightOld = 0;
Helvis 0:8491169be8fc 50 //den jeweiligen Test auf 1 setzen
Helvis 0:8491169be8fc 51 int testSensor = 0;
Helvis 0:8491169be8fc 52 int testMotion = 0;
Helvis 0:8491169be8fc 53 int testRotation = 0;
Helvis 0:8491169be8fc 54 int testCounts = 0;
Helvis 0:8491169be8fc 55 int testClass = 0;
Helvis 0:8491169be8fc 56
Helvis 0:8491169be8fc 57 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 58
Helvis 0:8491169be8fc 59 //User button toggle
Helvis 0:8491169be8fc 60 void press() {
Helvis 0:8491169be8fc 61 start = !start;
Helvis 0:8491169be8fc 62 }
Helvis 0:8491169be8fc 63
Helvis 0:8491169be8fc 64
Helvis 0:8491169be8fc 65 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 66
Helvis 0:8491169be8fc 67 int main() {
Helvis 0:8491169be8fc 68
Helvis 0:8491169be8fc 69 /**
Helvis 0:8491169be8fc 70 Micromouse Test
Helvis 0:8491169be8fc 71 */
Helvis 0:8491169be8fc 72
Helvis 0:8491169be8fc 73
Helvis 0:8491169be8fc 74 while(1) {
Helvis 0:8491169be8fc 75
Helvis 0:8491169be8fc 76 button.fall(&press); //User button einlesen
Helvis 0:8491169be8fc 77
Helvis 0:8491169be8fc 78 while(0) {
Helvis 0:8491169be8fc 79 myled = 1;
Helvis 0:8491169be8fc 80 printf("Hai\n");
Helvis 0:8491169be8fc 81 if (start == 0) break;
Helvis 0:8491169be8fc 82 }
Helvis 0:8491169be8fc 83
Helvis 0:8491169be8fc 84
Helvis 0:8491169be8fc 85
Helvis 0:8491169be8fc 86 /*
Helvis 0:8491169be8fc 87 Sensoren Test
Helvis 0:8491169be8fc 88 */
Helvis 0:8491169be8fc 89 while(1) {
Helvis 0:8491169be8fc 90
Helvis 0:8491169be8fc 91
Helvis 0:8491169be8fc 92 float distanceL = irSensorL.readL();
Helvis 0:8491169be8fc 93 float distanceC = irSensorC.readC();
Helvis 0:8491169be8fc 94 float distanceR = irSensorR.readR();
Helvis 0:8491169be8fc 95 float distanceB = irSensorB.readC();
Helvis 0:8491169be8fc 96 printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
Helvis 0:8491169be8fc 97
Helvis 0:8491169be8fc 98 //wait(0.5f);
Helvis 0:8491169be8fc 99 }
Helvis 0:8491169be8fc 100
Helvis 0:8491169be8fc 101 /*
Helvis 0:8491169be8fc 102 Motoren Test: Gerade Bewegung
Helvis 0:8491169be8fc 103 */
Helvis 0:8491169be8fc 104 while(start == 1 && testMotion == 1) {
Helvis 0:8491169be8fc 105
Helvis 0:8491169be8fc 106 controller.setDesiredSpeedLeft(20.0f); //Drehzahl in [rpm]
Helvis 0:8491169be8fc 107 controller.setDesiredSpeedRight(-20.0f);
Helvis 0:8491169be8fc 108 enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
Helvis 0:8491169be8fc 109
Helvis 0:8491169be8fc 110 countsLeft = counterLeft.read();
Helvis 0:8491169be8fc 111 countsRight = counterRight.read();
Helvis 0:8491169be8fc 112
Helvis 0:8491169be8fc 113 printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
Helvis 0:8491169be8fc 114
Helvis 0:8491169be8fc 115 if (start == 0) {
Helvis 0:8491169be8fc 116
Helvis 0:8491169be8fc 117 motion.stop();
Helvis 0:8491169be8fc 118 }
Helvis 0:8491169be8fc 119 }
Helvis 0:8491169be8fc 120
Helvis 0:8491169be8fc 121 /*
Helvis 0:8491169be8fc 122 Motoren Test: 180° Rotation
Helvis 0:8491169be8fc 123 */
Helvis 0:8491169be8fc 124
Helvis 0:8491169be8fc 125 while(start == 1 && testRotation == 1) {
Helvis 0:8491169be8fc 126
Helvis 0:8491169be8fc 127
Helvis 0:8491169be8fc 128 while((countsLeft - countsLeftOld) > -1614 || (countsRight - countsRightOld) > -1614) {
Helvis 0:8491169be8fc 129
Helvis 0:8491169be8fc 130 countsLeft = counterLeft.read();
Helvis 0:8491169be8fc 131 countsRight = counterRight.read();
Helvis 0:8491169be8fc 132
Helvis 0:8491169be8fc 133 controller.setDesiredSpeedLeft(-50.0f);
Helvis 0:8491169be8fc 134 controller.setDesiredSpeedRight(-50.0f);
Helvis 0:8491169be8fc 135 enableMotorDriver = 1;
Helvis 0:8491169be8fc 136 }
Helvis 0:8491169be8fc 137
Helvis 0:8491169be8fc 138 motion.stop();
Helvis 0:8491169be8fc 139 enableMotorDriver = 0;
Helvis 0:8491169be8fc 140 printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
Helvis 0:8491169be8fc 141
Helvis 0:8491169be8fc 142 if (start == 0) {
Helvis 0:8491169be8fc 143 countsLeftOld = countsLeft - 100;
Helvis 0:8491169be8fc 144 countsRightOld = countsRight - 100;
Helvis 0:8491169be8fc 145
Helvis 0:8491169be8fc 146 break;
Helvis 0:8491169be8fc 147 }
Helvis 0:8491169be8fc 148 }
Helvis 0:8491169be8fc 149
Helvis 0:8491169be8fc 150
Helvis 0:8491169be8fc 151 /* Benötigte Ticks für 180* Rotation am Ort:
Helvis 0:8491169be8fc 152
Helvis 0:8491169be8fc 153 mm/count = 0.122
Helvis 0:8491169be8fc 154 Dr = 124mm
Helvis 0:8491169be8fc 155 Rr = 60mm
Helvis 0:8491169be8fc 156 180°=> 1597 counts
Helvis 0:8491169be8fc 157 90°=> 798 counts
Helvis 0:8491169be8fc 158
Helvis 0:8491169be8fc 159
Helvis 0:8491169be8fc 160 RotationsDistanz Rd= DistanzRäder*pi/2 = 194.8
Helvis 0:8491169be8fc 161
Helvis 0:8491169be8fc 162 UmfangRad = Drad*pi = 188.5
Helvis 0:8491169be8fc 163
Helvis 0:8491169be8fc 164
Helvis 0:8491169be8fc 165 Counts = Rd/mm pro Count
Helvis 0:8491169be8fc 166
Helvis 0:8491169be8fc 167 => Counts pro Drehung rausfinden */
Helvis 0:8491169be8fc 168 while(start == 1 && testCounts == 1) {
Helvis 0:8491169be8fc 169
Helvis 0:8491169be8fc 170 while(countsLeft < 5000 || countsRight > -5000) {
Helvis 0:8491169be8fc 171
Helvis 0:8491169be8fc 172 countsLeft = counterLeft.read();
Helvis 0:8491169be8fc 173 countsRight = counterRight.read();
Helvis 0:8491169be8fc 174
Helvis 0:8491169be8fc 175 controller.setDesiredSpeedLeft(20.0f);
Helvis 0:8491169be8fc 176 controller.setDesiredSpeedRight(-20.0f);
Helvis 0:8491169be8fc 177 enableMotorDriver = 1;
Helvis 0:8491169be8fc 178 }
Helvis 0:8491169be8fc 179
Helvis 0:8491169be8fc 180 controller.setDesiredSpeedLeft(0.0f);
Helvis 0:8491169be8fc 181 controller.setDesiredSpeedRight(0.0f);
Helvis 0:8491169be8fc 182 enableMotorDriver = 0;
Helvis 0:8491169be8fc 183
Helvis 0:8491169be8fc 184 }
Helvis 0:8491169be8fc 185
Helvis 0:8491169be8fc 186 //Class test
Helvis 0:8491169be8fc 187 if(start == 1 && testClass == 1) {
Helvis 0:8491169be8fc 188 motion.rotate180();
Helvis 0:8491169be8fc 189 counterLeft.reset();
Helvis 0:8491169be8fc 190 counterRight.reset();
Helvis 0:8491169be8fc 191 start = 0;
Helvis 0:8491169be8fc 192 }
Helvis 0:8491169be8fc 193
Helvis 0:8491169be8fc 194 //Sensor calib
Helvis 0:8491169be8fc 195 if(start == 1) {
Helvis 0:8491169be8fc 196 int i;
Helvis 0:8491169be8fc 197 myled = !myled;
Helvis 0:8491169be8fc 198 for (i=0;i<5;i++) {
Helvis 0:8491169be8fc 199 float distanceL = irSensorL.readL();
Helvis 0:8491169be8fc 200 float distanceC = irSensorC.readC();
Helvis 0:8491169be8fc 201 float distanceR = irSensorR.readR();
Helvis 0:8491169be8fc 202 float distanceB = irSensorB.readC();
Helvis 0:8491169be8fc 203 printf("Links: %f Mitte: %f Rechts: %f Hinten: %f Line: %f\n", distanceL, distanceC, distanceR, distanceB, lineSensor.read());
Helvis 0:8491169be8fc 204 }
Helvis 0:8491169be8fc 205 printf("------------------------\n");
Helvis 0:8491169be8fc 206 start = 0;
Helvis 0:8491169be8fc 207
Helvis 0:8491169be8fc 208 /* while((countsLeft - countsLeftOld) > -82 || (countsRight - countsRightOld) < 82) {
Helvis 0:8491169be8fc 209
Helvis 0:8491169be8fc 210 controller.setDesiredSpeedLeft(-10.0f); //Drehzahl in [rpm]
Helvis 0:8491169be8fc 211 controller.setDesiredSpeedRight(10.0f);
Helvis 0:8491169be8fc 212 enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
Helvis 0:8491169be8fc 213
Helvis 0:8491169be8fc 214 countsLeft = counterLeft.read();
Helvis 0:8491169be8fc 215 countsRight = counterRight.read();
Helvis 0:8491169be8fc 216 }
Helvis 0:8491169be8fc 217
Helvis 0:8491169be8fc 218 controller.setDesiredSpeedLeft(0.0f);
Helvis 0:8491169be8fc 219 controller.setDesiredSpeedRight(0.0f);
Helvis 0:8491169be8fc 220 enableMotorDriver = 0;
Helvis 0:8491169be8fc 221
Helvis 0:8491169be8fc 222 float distanceL = irSensorB.read();
Helvis 0:8491169be8fc 223 printf("Hinten: %f\n", distanceL);
Helvis 0:8491169be8fc 224
Helvis 0:8491169be8fc 225 countsLeftOld = countsLeft;
Helvis 0:8491169be8fc 226 countsRightOld = countsRight;
Helvis 0:8491169be8fc 227 start = 0;*/
Helvis 0:8491169be8fc 228
Helvis 0:8491169be8fc 229 }
Helvis 0:8491169be8fc 230 }
Helvis 0:8491169be8fc 231 }
Helvis 0:8491169be8fc 232
Helvis 0:8491169be8fc 233
Helvis 0:8491169be8fc 234
Helvis 0:8491169be8fc 235