Fertige Version mit geschwindigkeit 100
Dependencies: mbed
Fork of Micromouse_alpha_copy_copy by
main.cpp@0:a9fe4ef404bf, 2018-03-07 (annotated)
- Committer:
- ruesipat
- Date:
- Wed Mar 07 14:06:19 2018 +0000
- Revision:
- 0:a9fe4ef404bf
- Child:
- 1:d9e840c48b1e
hallo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ruesipat | 0:a9fe4ef404bf | 1 | #include "mbed.h" |
ruesipat | 0:a9fe4ef404bf | 2 | |
ruesipat | 0:a9fe4ef404bf | 3 | #include "IRSensor.h" |
ruesipat | 0:a9fe4ef404bf | 4 | #include "EncoderCounter.h" |
ruesipat | 0:a9fe4ef404bf | 5 | #include "LowpassFilter.h" |
ruesipat | 0:a9fe4ef404bf | 6 | #include "Controller.h" |
ruesipat | 0:a9fe4ef404bf | 7 | |
ruesipat | 0:a9fe4ef404bf | 8 | DigitalIn button(USER_BUTTON); |
ruesipat | 0:a9fe4ef404bf | 9 | DigitalOut myled(LED1); |
ruesipat | 0:a9fe4ef404bf | 10 | DigitalOut led0(PC_8); |
ruesipat | 0:a9fe4ef404bf | 11 | DigitalOut led1(PC_6); |
ruesipat | 0:a9fe4ef404bf | 12 | DigitalOut led2(PB_12); |
ruesipat | 0:a9fe4ef404bf | 13 | DigitalOut led3(PA_7); |
ruesipat | 0:a9fe4ef404bf | 14 | DigitalOut led4(PC_0); |
ruesipat | 0:a9fe4ef404bf | 15 | DigitalOut led5(PC_9); |
ruesipat | 0:a9fe4ef404bf | 16 | |
ruesipat | 0:a9fe4ef404bf | 17 | DigitalOut enableMotorDriver(PB_2); |
ruesipat | 0:a9fe4ef404bf | 18 | DigitalIn motorDriverFault(PB_14); |
ruesipat | 0:a9fe4ef404bf | 19 | DigitalIn motorDriverWarning(PB_15); |
ruesipat | 0:a9fe4ef404bf | 20 | |
ruesipat | 0:a9fe4ef404bf | 21 | PwmOut pwmLeft(PA_8); |
ruesipat | 0:a9fe4ef404bf | 22 | PwmOut pwmRight(PA_9); |
ruesipat | 0:a9fe4ef404bf | 23 | |
ruesipat | 0:a9fe4ef404bf | 24 | EncoderCounter controllerLeft(PB_6, PB_7); |
ruesipat | 0:a9fe4ef404bf | 25 | EncoderCounter controllerRight(PA_6, PC_7); |
ruesipat | 0:a9fe4ef404bf | 26 | |
ruesipat | 0:a9fe4ef404bf | 27 | Controller controller(pwmLeft, pwmRight, controllerLeft, controllerRight); |
ruesipat | 0:a9fe4ef404bf | 28 | |
ruesipat | 0:a9fe4ef404bf | 29 | |
ruesipat | 0:a9fe4ef404bf | 30 | //AnalogIn analog_value(A0); |
ruesipat | 0:a9fe4ef404bf | 31 | |
ruesipat | 0:a9fe4ef404bf | 32 | DigitalOut enable(PC_1); |
ruesipat | 0:a9fe4ef404bf | 33 | |
ruesipat | 0:a9fe4ef404bf | 34 | |
ruesipat | 0:a9fe4ef404bf | 35 | AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte |
ruesipat | 0:a9fe4ef404bf | 36 | DigitalOut bit0(PH_1); |
ruesipat | 0:a9fe4ef404bf | 37 | DigitalOut bit1(PC_2); |
ruesipat | 0:a9fe4ef404bf | 38 | DigitalOut bit2(PC_3); |
ruesipat | 0:a9fe4ef404bf | 39 | |
ruesipat | 0:a9fe4ef404bf | 40 | |
ruesipat | 0:a9fe4ef404bf | 41 | |
ruesipat | 0:a9fe4ef404bf | 42 | IRSensor irSensor0(distance, bit0, bit1, bit2, 0); |
ruesipat | 0:a9fe4ef404bf | 43 | IRSensor irSensor1(distance, bit0, bit1, bit2, 1); |
ruesipat | 0:a9fe4ef404bf | 44 | IRSensor irSensor2(distance, bit0, bit1, bit2, 2); |
ruesipat | 0:a9fe4ef404bf | 45 | IRSensor irSensor3(distance, bit0, bit1, bit2, 3); |
ruesipat | 0:a9fe4ef404bf | 46 | IRSensor irSensor4(distance, bit0, bit1, bit2, 4); |
ruesipat | 0:a9fe4ef404bf | 47 | IRSensor irSensor5(distance, bit0, bit1, bit2, 5); |
ruesipat | 0:a9fe4ef404bf | 48 | |
ruesipat | 0:a9fe4ef404bf | 49 | int main() { |
ruesipat | 0:a9fe4ef404bf | 50 | |
ruesipat | 0:a9fe4ef404bf | 51 | |
ruesipat | 0:a9fe4ef404bf | 52 | enableMotorDriver = 1; // Schaltet den Leistungstreiber ein |
ruesipat | 0:a9fe4ef404bf | 53 | |
ruesipat | 0:a9fe4ef404bf | 54 | enable = 1; |
ruesipat | 0:a9fe4ef404bf | 55 | |
ruesipat | 0:a9fe4ef404bf | 56 | while(1) { |
ruesipat | 0:a9fe4ef404bf | 57 | |
ruesipat | 0:a9fe4ef404bf | 58 | |
ruesipat | 0:a9fe4ef404bf | 59 | |
ruesipat | 0:a9fe4ef404bf | 60 | float distance0 = irSensor0.read(); |
ruesipat | 0:a9fe4ef404bf | 61 | float distance1 = irSensor1.read(); |
ruesipat | 0:a9fe4ef404bf | 62 | float distance2 = irSensor2.read(); |
ruesipat | 0:a9fe4ef404bf | 63 | float distance3 = irSensor3.read(); |
ruesipat | 0:a9fe4ef404bf | 64 | float distance4 = irSensor4.read(); |
ruesipat | 0:a9fe4ef404bf | 65 | float distance5 = irSensor5.read(); |
ruesipat | 0:a9fe4ef404bf | 66 | |
ruesipat | 0:a9fe4ef404bf | 67 | |
ruesipat | 0:a9fe4ef404bf | 68 | if ( distance0 < 0.1f){ |
ruesipat | 0:a9fe4ef404bf | 69 | led0 = 1; |
ruesipat | 0:a9fe4ef404bf | 70 | controller.setDesiredSpeedLeft(0.0f); //Drehzahl in (rpm) |
ruesipat | 0:a9fe4ef404bf | 71 | controller.setDesiredSpeedRight(0.0f); //Drehzahl in (rpm) |
ruesipat | 0:a9fe4ef404bf | 72 | } |
ruesipat | 0:a9fe4ef404bf | 73 | else{ |
ruesipat | 0:a9fe4ef404bf | 74 | led0 = 0; |
ruesipat | 0:a9fe4ef404bf | 75 | controller.setDesiredSpeedLeft(50.0f); |
ruesipat | 0:a9fe4ef404bf | 76 | controller.setDesiredSpeedRight(-50.0f); |
ruesipat | 0:a9fe4ef404bf | 77 | } |
ruesipat | 0:a9fe4ef404bf | 78 | |
ruesipat | 0:a9fe4ef404bf | 79 | if ( distance1 < 0.1f){ |
ruesipat | 0:a9fe4ef404bf | 80 | led1 = 1; |
ruesipat | 0:a9fe4ef404bf | 81 | controller.setDesiredSpeedLeft(-50.0f); |
ruesipat | 0:a9fe4ef404bf | 82 | // controller.setDesiredSpeedRight(0.0f); |
ruesipat | 0:a9fe4ef404bf | 83 | } |
ruesipat | 0:a9fe4ef404bf | 84 | else{ |
ruesipat | 0:a9fe4ef404bf | 85 | led1 = 0; |
ruesipat | 0:a9fe4ef404bf | 86 | // controller.setDesiredSpeedLeft(0.0f); |
ruesipat | 0:a9fe4ef404bf | 87 | // controller.setDesiredSpeedRight(0.0f); |
ruesipat | 0:a9fe4ef404bf | 88 | } |
ruesipat | 0:a9fe4ef404bf | 89 | if ( distance2 < 0.1f){ |
ruesipat | 0:a9fe4ef404bf | 90 | led2 = 1; |
ruesipat | 0:a9fe4ef404bf | 91 | //pwmLeft = 0.6; |
ruesipat | 0:a9fe4ef404bf | 92 | //pwmRight = 0.4; |
ruesipat | 0:a9fe4ef404bf | 93 | } |
ruesipat | 0:a9fe4ef404bf | 94 | else{ |
ruesipat | 0:a9fe4ef404bf | 95 | led2 = 0; |
ruesipat | 0:a9fe4ef404bf | 96 | //pwmLeft = 0.5; |
ruesipat | 0:a9fe4ef404bf | 97 | //pwmRight = 0.5; |
ruesipat | 0:a9fe4ef404bf | 98 | } |
ruesipat | 0:a9fe4ef404bf | 99 | if ( distance3 < 0.1f){ |
ruesipat | 0:a9fe4ef404bf | 100 | led3 = 1; |
ruesipat | 0:a9fe4ef404bf | 101 | //pwmLeft = 0.6; |
ruesipat | 0:a9fe4ef404bf | 102 | //pwmRight = 0.4; |
ruesipat | 0:a9fe4ef404bf | 103 | } |
ruesipat | 0:a9fe4ef404bf | 104 | else{ |
ruesipat | 0:a9fe4ef404bf | 105 | led3 = 0; |
ruesipat | 0:a9fe4ef404bf | 106 | //pwmLeft = 0.5; |
ruesipat | 0:a9fe4ef404bf | 107 | //pwmRight = 0.5; |
ruesipat | 0:a9fe4ef404bf | 108 | } |
ruesipat | 0:a9fe4ef404bf | 109 | if ( distance4 < 0.1f){ |
ruesipat | 0:a9fe4ef404bf | 110 | led4 = 1; |
ruesipat | 0:a9fe4ef404bf | 111 | //pwmLeft = 0.6; |
ruesipat | 0:a9fe4ef404bf | 112 | //pwmRight = 0.4; |
ruesipat | 0:a9fe4ef404bf | 113 | } |
ruesipat | 0:a9fe4ef404bf | 114 | else{ |
ruesipat | 0:a9fe4ef404bf | 115 | led4 = 0; |
ruesipat | 0:a9fe4ef404bf | 116 | //pwmLeft = 0.5; |
ruesipat | 0:a9fe4ef404bf | 117 | //pwmRight = 0.5; |
ruesipat | 0:a9fe4ef404bf | 118 | } |
ruesipat | 0:a9fe4ef404bf | 119 | |
ruesipat | 0:a9fe4ef404bf | 120 | if ( distance5 < 0.1f){ |
ruesipat | 0:a9fe4ef404bf | 121 | led5 = 1; |
ruesipat | 0:a9fe4ef404bf | 122 | controller.setDesiredSpeedRight(50.0f); |
ruesipat | 0:a9fe4ef404bf | 123 | //pwmRight = 0.4; |
ruesipat | 0:a9fe4ef404bf | 124 | } |
ruesipat | 0:a9fe4ef404bf | 125 | else{ |
ruesipat | 0:a9fe4ef404bf | 126 | led5 = 0; |
ruesipat | 0:a9fe4ef404bf | 127 | //pwmLeft = 0.5; |
ruesipat | 0:a9fe4ef404bf | 128 | //pwmRight = 0.5; |
ruesipat | 0:a9fe4ef404bf | 129 | } |
ruesipat | 0:a9fe4ef404bf | 130 | |
ruesipat | 0:a9fe4ef404bf | 131 | myled =! myled; // LED is ON |
ruesipat | 0:a9fe4ef404bf | 132 | wait(0.1f); // 100 ms |
ruesipat | 0:a9fe4ef404bf | 133 | |
ruesipat | 0:a9fe4ef404bf | 134 | |
ruesipat | 0:a9fe4ef404bf | 135 | } |
ruesipat | 0:a9fe4ef404bf | 136 | } |