Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

main.cpp@5:47262622a9bf, 2018-04-16 (annotated)

Committer:
Helvis
Date:
Mon Apr 16 12:44:48 2018 +0000
Revision:
5:47262622a9bf
Parent:
4:e74c06e43485
Child:
6:1f084e6f7e80
testtestest;

Who changed what in which revision?

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