Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

main.cpp@4:e74c06e43485, 2018-04-11 (annotated)

Committer:
Helvis
Date:
Wed Apr 11 15:26:03 2018 +0000
Revision:
4:e74c06e43485
Parent:
3:c0f40a94fffa
Child:
5:47262622a9bf
V0.2

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