dreamworld / Mbed 2 deprecated Praktikum2

afg

Dependencies:   Libary mbed

main.cpp@0:66569508393e, 2018-03-09 (annotated)

Committer:
opplichr
Date:
Fri Mar 09 15:53:48 2018 +0000
Revision:
0:66569508393e
sadgt;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
opplichr 0:66569508393e 1 #include "mbed.h"
opplichr 0:66569508393e 2 #include "IRSensor.h"
opplichr 0:66569508393e 3 #include "EncoderCounter.h"
opplichr 0:66569508393e 4 #include "LowpassFilter.h"
opplichr 0:66569508393e 5 #include "Controller.h"
opplichr 0:66569508393e 6 #include "Motion.h"
opplichr 0:66569508393e 7
opplichr 0:66569508393e 8 //Serial
opplichr 0:66569508393e 9 Serial pc(USBTX, USBRX);
opplichr 0:66569508393e 10 AnalogIn distance(PB_1); // Kreieren der Ein- und Ausgangsobjekte
opplichr 0:66569508393e 11 DigitalOut enable(PC_1);
opplichr 0:66569508393e 12 DigitalOut bit0(PH_1);
opplichr 0:66569508393e 13 DigitalOut bit1(PC_2);
opplichr 0:66569508393e 14 DigitalOut bit2(PC_3);
opplichr 0:66569508393e 15
opplichr 0:66569508393e 16 //LED
opplichr 0:66569508393e 17 DigitalOut led0(PC_8);
opplichr 0:66569508393e 18 DigitalOut led1(PC_6);
opplichr 0:66569508393e 19 DigitalOut led2(PB_12);
opplichr 0:66569508393e 20 DigitalOut led3(PA_7);
opplichr 0:66569508393e 21 DigitalOut led4(PC_0);
opplichr 0:66569508393e 22 DigitalOut led5(PC_9);
opplichr 0:66569508393e 23
opplichr 0:66569508393e 24 //Ansteuerung
opplichr 0:66569508393e 25 DigitalOut enableMotorDriver(PB_2);
opplichr 0:66569508393e 26 DigitalIn motorDriverFault(PB_14);
opplichr 0:66569508393e 27 DigitalIn motorDriverWarning(PB_15);
opplichr 0:66569508393e 28 PwmOut pwmLeft(PA_8);
opplichr 0:66569508393e 29 PwmOut pwmRight(PA_9);
opplichr 0:66569508393e 30
opplichr 0:66569508393e 31 DigitalIn Button(USER_BUTTON);
opplichr 0:66569508393e 32
opplichr 0:66569508393e 33
opplichr 0:66569508393e 34
opplichr 0:66569508393e 35 int main() {
opplichr 0:66569508393e 36 //Entcoder
opplichr 0:66569508393e 37 EncoderCounter counterLeft(PB_6, PB_7);
opplichr 0:66569508393e 38 EncoderCounter counterRight(PA_6, PC_7);
opplichr 0:66569508393e 39 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
opplichr 0:66569508393e 40 int init=0;
opplichr 0:66569508393e 41 int state=1;
opplichr 0:66569508393e 42
opplichr 0:66569508393e 43 while(1) {
opplichr 0:66569508393e 44
opplichr 0:66569508393e 45 IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
opplichr 0:66569508393e 46 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
opplichr 0:66569508393e 47 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
opplichr 0:66569508393e 48 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
opplichr 0:66569508393e 49 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
opplichr 0:66569508393e 50 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
opplichr 0:66569508393e 51 // Objekte kreieren
opplichr 0:66569508393e 52 enable = 1;
opplichr 0:66569508393e 53 float distance0 = irSensor0.read();
opplichr 0:66569508393e 54 float distance1 = irSensor1.read();
opplichr 0:66569508393e 55 float distance2 = irSensor2.read();
opplichr 0:66569508393e 56 float distance3 = irSensor3.read();
opplichr 0:66569508393e 57 float distance4 = irSensor4.read();
opplichr 0:66569508393e 58 float distance5 = irSensor5.read();
opplichr 0:66569508393e 59
opplichr 0:66569508393e 60 switch (state){
opplichr 0:66569508393e 61 case 1:
opplichr 0:66569508393e 62 enableMotorDriver = 0; // Schaltet den Leistungstreiber aus
opplichr 0:66569508393e 63
opplichr 0:66569508393e 64 if (Button==0){
opplichr 0:66569508393e 65 enableMotorDriver = 1; state=2;
opplichr 0:66569508393e 66 }
opplichr 0:66569508393e 67 //if (init==1){enableMotorDriver = 0; state=2;}
opplichr 0:66569508393e 68
opplichr 0:66569508393e 69 break;
opplichr 0:66569508393e 70 case 2:
opplichr 0:66569508393e 71 controller.setTranslationalVelocity(-5);
opplichr 0:66569508393e 72 if (distance3 <= 0.2){
opplichr 0:66569508393e 73 state=6;
opplichr 0:66569508393e 74 }
opplichr 0:66569508393e 75 if (Button==0){
opplichr 0:66569508393e 76 state=5;
opplichr 0:66569508393e 77 }
opplichr 0:66569508393e 78 if (distance4 <=0.2){
opplichr 0:66569508393e 79 state=4;
opplichr 0:66569508393e 80 controller.setTranslationalVelocity(0);
opplichr 0:66569508393e 81 }
opplichr 0:66569508393e 82 if (distance2 <=0.2){
opplichr 0:66569508393e 83 state=3;
opplichr 0:66569508393e 84 controller.setTranslationalVelocity(0);
opplichr 0:66569508393e 85 }
opplichr 0:66569508393e 86 break;
opplichr 0:66569508393e 87 case 3:
opplichr 0:66569508393e 88 controller.setRotationalVelocity(-0.1);
opplichr 0:66569508393e 89 if (Button==0){
opplichr 0:66569508393e 90 state=5;controller.setRotationalVelocity(0);
opplichr 0:66569508393e 91 }
opplichr 0:66569508393e 92
opplichr 0:66569508393e 93 if (distance2 <=0.2){
opplichr 0:66569508393e 94 state=3;
opplichr 0:66569508393e 95 }else { controller.setRotationalVelocity(0); state=2;}
opplichr 0:66569508393e 96 break;
opplichr 0:66569508393e 97 case 4:
opplichr 0:66569508393e 98 controller.setRotationalVelocity(0.1);
opplichr 0:66569508393e 99 if (Button==0){
opplichr 0:66569508393e 100 state=5;controller.setRotationalVelocity(0);
opplichr 0:66569508393e 101 }
opplichr 0:66569508393e 102 if (distance4 <=0.2){
opplichr 0:66569508393e 103 state=4;
opplichr 0:66569508393e 104 } else { controller.setRotationalVelocity(0); state=2;}
opplichr 0:66569508393e 105
opplichr 0:66569508393e 106 break;
opplichr 0:66569508393e 107 case 5:
opplichr 0:66569508393e 108 controller.setTranslationalVelocity(0);
opplichr 0:66569508393e 109 state=1;
opplichr 0:66569508393e 110 break;
opplichr 0:66569508393e 111 case 6:
opplichr 0:66569508393e 112 controller.setTranslationalVelocity(5);
opplichr 0:66569508393e 113 wait(1);
opplichr 0:66569508393e 114 controller.setTranslationalVelocity(0);
opplichr 0:66569508393e 115 controller.setRotationalVelocity(0.1);
opplichr 0:66569508393e 116 wait(0.2);
opplichr 0:66569508393e 117 controller.setRotationalVelocity(0);
opplichr 0:66569508393e 118 state =2;
opplichr 0:66569508393e 119 break;
opplichr 0:66569508393e 120 }
opplichr 0:66569508393e 121
opplichr 0:66569508393e 122
opplichr 0:66569508393e 123 }
opplichr 0:66569508393e 124 }