P1

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "IRSensor.h"
00003 #include "Controller.h"
00004 #include "LowpassFilter.h"
00005 
00006 DigitalOut myled(LED1);
00007 
00008 int main()
00009 {
00010     DigitalOut led0(PD_4);
00011     DigitalOut led1(PD_3);
00012     DigitalOut led2(PD_6);
00013     DigitalOut led3(PD_2);
00014     DigitalOut led4(PD_7);
00015     DigitalOut led5(PD_5);
00016 
00017     AnalogIn distance(PA_0); // Kreieren der Ein- und Ausgangsobjekte
00018     DigitalOut enable(PG_1);
00019     DigitalOut bit0(PF_0);
00020     DigitalOut bit1(PF_1);
00021     DigitalOut bit2(PF_2);
00022 
00023 
00024 
00025 
00026     IRSensor irSensor0(distance, bit0, bit1, bit2, 0); // Objekte kreieren
00027     IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
00028     IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
00029     IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
00030     IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
00031     IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
00032 
00033     enable = 1; // schaltet die Sensoren ein
00034 
00035     DigitalOut enableMotorDriver(PG_0);
00036     DigitalIn motorDriverFault(PD_1);
00037     DigitalIn motorDriverWarning(PD_0);
00038     PwmOut pwmLeft(PF_9);
00039     PwmOut pwmRight(PF_8);
00040     
00041     EncoderCounter counterLeft(PD_12, PD_13);
00042     EncoderCounter counterRight(PB_4, PC_7);
00043     
00044     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00045     
00046     while(1) {
00047         /*
00048         led0 = 1; // LED is ON
00049         led1 = 0;
00050         led2 = 0;
00051         led3 = 0;
00052         led4 = 0;
00053         led5 = 0;
00054 
00055         wait(0.2); // 200 ms
00056 
00057         led0 = 0; // LED is ON
00058         led1 = 1;
00059         led2 = 0;
00060         led3 = 0;
00061         led4 = 0;
00062         led5 = 0;
00063 
00064         wait(0.2); // 200 ms
00065 
00066         led0 = 0; // LED is ON
00067         led1 = 0;
00068         led2 = 1;
00069         led3 = 0;
00070         led4 = 0;
00071         led5 = 0;
00072 
00073         wait(0.2); // 200 ms
00074 
00075         led0 = 0; // LED is ON
00076         led1 = 0;
00077         led2 = 0;
00078         led3 = 1;
00079         led4 = 0;
00080         led5 = 0;
00081 
00082         wait(0.2); // 200 ms
00083 
00084         led0 = 0; // LED is ON
00085         led1 = 0;
00086         led2 = 0;
00087         led3 = 0;
00088         led4 = 1;
00089         led5 = 0;
00090 
00091         wait(0.2); // 200 ms
00092 
00093         led0 = 0; // LED is ON
00094         led1 = 0;
00095         led2 = 0;
00096         led3 = 0;
00097         led4 = 0;
00098         led5 = 1;
00099 
00100         wait(0.2); // 200 ms
00101 
00102 
00103         AnalogIn distance(PA_0); // Kreieren der Ein- und Ausgangsobjekte
00104         DigitalOut enable(PG_1);
00105         DigitalOut bit0(PF_0);
00106         DigitalOut bit1(PF_1);
00107         DigitalOut bit2(PF_2);
00108         enable = 1; // schaltet die Sensoren ein
00109         bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
00110         bit1 = 0;
00111         bit2 = 0;
00112         float d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
00113 
00114         printf("Distance hinten: %f \r \n",d);
00115 
00116         bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
00117         bit1 = 0;
00118         bit2 = 0;
00119         d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
00120 
00121         printf("Distance hinten-links: %f \r \n",d);
00122 
00123         bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
00124         bit1 = 1;
00125         bit2 = 0;
00126         d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
00127 
00128         printf("Distance vorne-links: %f \r \n",d);
00129 
00130         bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
00131         bit1 = 0;
00132         bit2 = 1;
00133         d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
00134 
00135         printf("Distance vorne-rechts: %f \r \n",d);
00136 
00137         bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
00138         bit1 = 0;
00139         bit2 = 1;
00140         d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
00141 
00142         printf("Distance hinten-rechts: %f \r \n",d);
00143 
00144         bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
00145         bit1 = 1;
00146         bit2 = 0;
00147         d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
00148 
00149         printf("Distance vorne: %f \r \n",d);
00150         */
00151       
00152         
00153         float distance0 = irSensor0.read(); // gibt die Distanz in [m] zurueck
00154         float distance1 = irSensor1.read();
00155         float distance2 = irSensor2.read();
00156         float distance3 = irSensor3.read();
00157         float distance4 = irSensor4.read();
00158         float distance5 = irSensor5.read();
00159         
00160         printf("Distance hinten: %f \r \n", distance0);
00161         printf("Distance hinten-links: %f \r \n", distance1);
00162         printf("Distance vorne-links: %f \r \n", distance2);
00163         printf("Distance vorne: %f \r \n", distance3);
00164         printf("Distance vorne-rechts: %f \r \n", distance4);
00165         printf("Distance hinten-rechts: %f \r \n", distance5);
00166         
00167         if (distance0 <= 0.1) { led0=1;
00168        
00169             }else {led0=0;}
00170             
00171         if (distance1 <= 0.1) { led1=1;
00172        
00173             }else {led1=0;}
00174         if (distance2 <= 0.1) { led2=1;
00175        
00176             }else {led2=0;}
00177         if (distance3 <= 0.1) { led3=1;
00178        
00179             }else {led3=0;}
00180         if (distance4 <= 0.1) { led4=1;
00181        
00182             }else {led4=0;}
00183         if (distance5 <= 0.1) { led5=1;
00184        
00185             }else {led5=0;}
00186        /*     
00187         pwmLeft.period(0.00005); // Setzt die Periode auf 50 μs
00188         pwmRight.period(0.00005);
00189         
00190         pwmLeft = 0.5; // Setzt die Duty-Cycle auf 50%
00191         pwmRight = 0.5;
00192         */
00193         enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
00194 
00195         //pwmLeft = 0.7; // Duty-Cycle von 70%
00196         //pwmRight = 0.3;
00197         
00198         
00199         controller.setDesiredSpeedLeft(100.0); // Drehzahl in [rpm]
00200         controller.setDesiredSpeedRight(-100.0);
00201         
00202         wait(0.2);
00203 
00204         }
00205 
00206 }