Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
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?
User | Revision | Line number | New 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 |