Sensoren auslesen/umwandeln/codieren und State Maschine
Dependencies: mbed
Fork of StateMachine_1 by
Diff: main.cpp
- Revision:
- 0:e611a0b9f02a
- Child:
- 2:a227732e1412
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 16 13:29:27 2018 +0000 @@ -0,0 +1,77 @@ +#include <mbed.h> + +#include "EncoderCounter.h" +#include "Controller.h" +#include "IRSensor.h" + +DigitalOut myled(LED1); + +DigitalOut led0(PC_8); +DigitalOut led1(PC_6); +DigitalOut led2(PB_12); +DigitalOut led3(PA_7); +DigitalOut led4(PC_0); +DigitalOut led5(PC_9); + +AnalogIn distance(PB_1); +DigitalOut enable(PC_1); +DigitalOut bit0(PH_1); +DigitalOut bit1(PC_2); +DigitalOut bit2(PC_3); + +IRSensor irSensor0(distance, bit0, bit1, bit2, 0); +IRSensor irSensor1(distance, bit0, bit1, bit2, 1); +IRSensor irSensor2(distance, bit0, bit1, bit2, 2); +IRSensor irSensor3(distance, bit0, bit1, bit2, 3); +IRSensor irSensor4(distance, bit0, bit1, bit2, 4); +IRSensor irSensor5(distance, bit0, bit1, bit2, 5); + + +DigitalOut enableMotorDriver(PB_2); +DigitalIn motorDriverFault(PB_14); +DigitalIn motorDriverWarning(PB_15); + +PwmOut pwmLeft(PA_8); +PwmOut pwmRight(PA_9); + +EncoderCounter counterLeft(PB_6, PB_7); +EncoderCounter counterRight(PA_6, PC_7); + +Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); + +int main() +{ + + controller.setDesiredSpeedLeft(150.0f); // Drehzahl in [rpm] + controller.setDesiredSpeedRight(-150.0f); + enable = 1; + enableMotorDriver = 1; // Schaltet den Leistungstreiber ein + + while(1) { + + myled =! myled; + + if (irSensor0.read() < 0.2f) led0 = 1; + else led0 = 0; + + if (irSensor1.read() < 0.2f) led1 = 1; + else led1 = 0; + + if (irSensor2.read() < 0.2f) led2 = 1; + else led2 = 0; + + if (irSensor3.read() < 0.2f) led3 = 1; + else led3 = 0; + + if (irSensor4.read() < 0.2f) led4 = 1; + else led4 = 0; + + if (irSensor5.read() < 0.2f) led5 = 1; + else led0 = 5; + + printf("iSumLeft: %f, iSumRight: %f, counterDiff; %d \n\r", controller.getIntegralLeft(), controller.getIntegralRight(), (counterLeft.read()+counterRight.read())); + + wait(0.2f); // 100 ms + + } +}