P1
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 20 2022 10:34:32 by
1.7.2