Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

main.cpp@1:1adf5dfcc7bb, 2018-04-10 (annotated)

Committer:
Helvis
Date:
Tue Apr 10 12:24:23 2018 +0000
Revision:
1:1adf5dfcc7bb
Parent:
0:9a3e7847a4be
Child:
2:4ccdc62022c2
Klasse Motion geschrieben: Bewegung, Rotation; Test erg?nzt: Encoder Counts auslesen

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 1:1adf5dfcc7bb 9 DigitalIn button(PC_13);
Helvis 1:1adf5dfcc7bb 10
Helvis 1:1adf5dfcc7bb 11 //Sensors:
Helvis 0:9a3e7847a4be 12
Helvis 0:9a3e7847a4be 13 AnalogIn lineSensor(PA_0);
Helvis 1:1adf5dfcc7bb 14 AnalogIn distanceL(PA_1);
Helvis 1:1adf5dfcc7bb 15 AnalogIn distanceC(PA_4);
Helvis 1:1adf5dfcc7bb 16 AnalogIn distanceR(PB_0);
Helvis 0:9a3e7847a4be 17
Helvis 1:1adf5dfcc7bb 18 IRSensor irSensorL (distanceL);
Helvis 1:1adf5dfcc7bb 19 IRSensor irSensorC (distanceC);
Helvis 1:1adf5dfcc7bb 20 IRSensor irSensorR (distanceR);
Helvis 0:9a3e7847a4be 21
Helvis 1:1adf5dfcc7bb 22 //Motors:
Helvis 0:9a3e7847a4be 23 DigitalOut myled(LED1);
Helvis 0:9a3e7847a4be 24
Helvis 0:9a3e7847a4be 25 DigitalOut enableMotorDriver(PB_2);
Helvis 0:9a3e7847a4be 26
Helvis 0:9a3e7847a4be 27 DigitalIn motorDriverFault(PB_14);
Helvis 0:9a3e7847a4be 28 DigitalIn motorDriverWarning(PB_15);
Helvis 0:9a3e7847a4be 29
Helvis 0:9a3e7847a4be 30 PwmOut pwmLeft(PA_8);
Helvis 0:9a3e7847a4be 31 PwmOut pwmRight(PA_9);
Helvis 0:9a3e7847a4be 32
Helvis 0:9a3e7847a4be 33 EncoderCounter counterLeft(PB_6, PB_7);
Helvis 0:9a3e7847a4be 34 EncoderCounter counterRight(PA_6, PC_7);
Helvis 0:9a3e7847a4be 35
Helvis 0:9a3e7847a4be 36 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Helvis 1:1adf5dfcc7bb 37
Helvis 1:1adf5dfcc7bb 38 Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, irSensorR);
Helvis 0:9a3e7847a4be 39
Helvis 0:9a3e7847a4be 40 int main() {
Helvis 0:9a3e7847a4be 41
Helvis 1:1adf5dfcc7bb 42 /** Micromouse Test
Helvis 1:1adf5dfcc7bb 43
Helvis 1:1adf5dfcc7bb 44 für den jeweiligen Test 0 im while Argument auf 1 setzen
Helvis 1:1adf5dfcc7bb 45 */
Helvis 1:1adf5dfcc7bb 46
Helvis 1:1adf5dfcc7bb 47 //User Button einlesen: start Signal
Helvis 1:1adf5dfcc7bb 48 int start;
Helvis 1:1adf5dfcc7bb 49 short countsLeft = 0;
Helvis 1:1adf5dfcc7bb 50 short countsRight = 0;
Helvis 1:1adf5dfcc7bb 51
Helvis 1:1adf5dfcc7bb 52 if (button.read() || start != 1) start = 1;
Helvis 1:1adf5dfcc7bb 53 else if(button.read() || start == 1) start = 0;
Helvis 0:9a3e7847a4be 54
Helvis 0:9a3e7847a4be 55
Helvis 1:1adf5dfcc7bb 56 /*
Helvis 1:1adf5dfcc7bb 57 Sensoren Test
Helvis 1:1adf5dfcc7bb 58 */
Helvis 1:1adf5dfcc7bb 59 while(start && 1) {
Helvis 0:9a3e7847a4be 60
Helvis 0:9a3e7847a4be 61 float distanceL = irSensorL.read();
Helvis 0:9a3e7847a4be 62 float distanceC = irSensorC.read();
Helvis 0:9a3e7847a4be 63 float distanceR = irSensorR.read();
Helvis 1:1adf5dfcc7bb 64 printf("Rechts: %f Mitte: %f Links: %f\n", distanceL, distanceC, distanceR);
Helvis 0:9a3e7847a4be 65
Helvis 0:9a3e7847a4be 66 wait(0.5f);
Helvis 0:9a3e7847a4be 67 }
Helvis 0:9a3e7847a4be 68
Helvis 1:1adf5dfcc7bb 69 /*
Helvis 1:1adf5dfcc7bb 70 Motoren Test: Gerade Bewegung
Helvis 1:1adf5dfcc7bb 71 */
Helvis 1:1adf5dfcc7bb 72 while(start && 0) {
Helvis 0:9a3e7847a4be 73
Helvis 0:9a3e7847a4be 74 controller.setDesiredSpeedLeft(50.0f); //Drehzahl in [rpm]
Helvis 1:1adf5dfcc7bb 75 controller.setDesiredSpeedRight(50.0f);
Helvis 0:9a3e7847a4be 76 enableMotorDriver = 1; //Schaltet den Leistungstreiber ein
Helvis 0:9a3e7847a4be 77 while(1) {
Helvis 0:9a3e7847a4be 78
Helvis 0:9a3e7847a4be 79 myled =! myled;
Helvis 0:9a3e7847a4be 80 wait(0.1f); //200 ms
Helvis 1:1adf5dfcc7bb 81
Helvis 1:1adf5dfcc7bb 82 countsLeft = counterLeft.read();
Helvis 1:1adf5dfcc7bb 83 countsRight = counterRight.read();
Helvis 1:1adf5dfcc7bb 84 printf("countsLeft: %hd countsRight: %hd\n", countsLeft, countsRight);
Helvis 1:1adf5dfcc7bb 85 }
Helvis 1:1adf5dfcc7bb 86 }
Helvis 1:1adf5dfcc7bb 87
Helvis 1:1adf5dfcc7bb 88 /*
Helvis 1:1adf5dfcc7bb 89 Motoren Test: 180° Rotation
Helvis 1:1adf5dfcc7bb 90 */
Helvis 1:1adf5dfcc7bb 91
Helvis 1:1adf5dfcc7bb 92 while(start && 0) {
Helvis 1:1adf5dfcc7bb 93
Helvis 1:1adf5dfcc7bb 94 while(countsLeft < 200 || countsRight > -200) {
Helvis 1:1adf5dfcc7bb 95
Helvis 1:1adf5dfcc7bb 96 countsLeft = counterLeft.read();
Helvis 1:1adf5dfcc7bb 97 countsRight = counterRight.read();
Helvis 1:1adf5dfcc7bb 98
Helvis 1:1adf5dfcc7bb 99 controller.setDesiredSpeedLeft(50.0f);
Helvis 1:1adf5dfcc7bb 100 controller.setDesiredSpeedRight(-50.0f);
Helvis 1:1adf5dfcc7bb 101 }
Helvis 1:1adf5dfcc7bb 102
Helvis 1:1adf5dfcc7bb 103 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:1adf5dfcc7bb 104 controller.setDesiredSpeedRight(0.0f);
Helvis 0:9a3e7847a4be 105
Helvis 0:9a3e7847a4be 106 }
Helvis 0:9a3e7847a4be 107
Helvis 1:1adf5dfcc7bb 108
Helvis 1:1adf5dfcc7bb 109 /* Benötigte Ticks für 180* Rotation am Ort:
Helvis 0:9a3e7847a4be 110
Helvis 1:1adf5dfcc7bb 111 RotationsDistanz Rd= DistanzRäder*pi/2
Helvis 1:1adf5dfcc7bb 112
Helvis 1:1adf5dfcc7bb 113 UmfangRad = Drad*pi
Helvis 1:1adf5dfcc7bb 114
Helvis 1:1adf5dfcc7bb 115 mm pro Count = UmfangRad/Counts pro Drehung
Helvis 1:1adf5dfcc7bb 116
Helvis 1:1adf5dfcc7bb 117 Counts = Rd/mm pro Count
Helvis 0:9a3e7847a4be 118
Helvis 1:1adf5dfcc7bb 119 => Counts pro Drehung rausfinden */
Helvis 1:1adf5dfcc7bb 120 while(start && 0) {
Helvis 1:1adf5dfcc7bb 121
Helvis 1:1adf5dfcc7bb 122 while(countsLeft < 100 || countsRight < 100) {
Helvis 1:1adf5dfcc7bb 123
Helvis 1:1adf5dfcc7bb 124 countsLeft = counterLeft.read();
Helvis 1:1adf5dfcc7bb 125 countsRight = counterRight.read();
Helvis 1:1adf5dfcc7bb 126
Helvis 1:1adf5dfcc7bb 127 controller.setDesiredSpeedLeft(50.0f);
Helvis 1:1adf5dfcc7bb 128 controller.setDesiredSpeedRight(50.0f);
Helvis 1:1adf5dfcc7bb 129 }
Helvis 1:1adf5dfcc7bb 130
Helvis 1:1adf5dfcc7bb 131 controller.setDesiredSpeedLeft(0.0f);
Helvis 1:1adf5dfcc7bb 132 controller.setDesiredSpeedRight(0.0f);
Helvis 1:1adf5dfcc7bb 133
Helvis 1:1adf5dfcc7bb 134 }
Helvis 1:1adf5dfcc7bb 135 }
Helvis 0:9a3e7847a4be 136
Helvis 0:9a3e7847a4be 137
Helvis 0:9a3e7847a4be 138
Helvis 0:9a3e7847a4be 139