Marco Oehler / Mbed 2 deprecated ROME2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:7ee4c6416e08, 2020-02-24 (annotated)

Committer:
oehlemar
Date:
Mon Feb 24 16:05:50 2020 +0000
Revision:
0:7ee4c6416e08
ROME2 P1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oehlemar 0:7ee4c6416e08 1 #include "mbed.h"
oehlemar 0:7ee4c6416e08 2 #include "IRSensor.h"
oehlemar 0:7ee4c6416e08 3 #include "Controller.h"
oehlemar 0:7ee4c6416e08 4 #include "LowpassFilter.h"
oehlemar 0:7ee4c6416e08 5
oehlemar 0:7ee4c6416e08 6 DigitalOut myled(LED1);
oehlemar 0:7ee4c6416e08 7
oehlemar 0:7ee4c6416e08 8 int main()
oehlemar 0:7ee4c6416e08 9 {
oehlemar 0:7ee4c6416e08 10 DigitalOut led0(PD_4);
oehlemar 0:7ee4c6416e08 11 DigitalOut led1(PD_3);
oehlemar 0:7ee4c6416e08 12 DigitalOut led2(PD_6);
oehlemar 0:7ee4c6416e08 13 DigitalOut led3(PD_2);
oehlemar 0:7ee4c6416e08 14 DigitalOut led4(PD_7);
oehlemar 0:7ee4c6416e08 15 DigitalOut led5(PD_5);
oehlemar 0:7ee4c6416e08 16
oehlemar 0:7ee4c6416e08 17 AnalogIn distance(PA_0); // Kreieren der Ein- und Ausgangsobjekte
oehlemar 0:7ee4c6416e08 18 DigitalOut enable(PG_1);
oehlemar 0:7ee4c6416e08 19 DigitalOut bit0(PF_0);
oehlemar 0:7ee4c6416e08 20 DigitalOut bit1(PF_1);
oehlemar 0:7ee4c6416e08 21 DigitalOut bit2(PF_2);
oehlemar 0:7ee4c6416e08 22
oehlemar 0:7ee4c6416e08 23
oehlemar 0:7ee4c6416e08 24
oehlemar 0:7ee4c6416e08 25
oehlemar 0:7ee4c6416e08 26 IRSensor irSensor0(distance, bit0, bit1, bit2, 0); // Objekte kreieren
oehlemar 0:7ee4c6416e08 27 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
oehlemar 0:7ee4c6416e08 28 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
oehlemar 0:7ee4c6416e08 29 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
oehlemar 0:7ee4c6416e08 30 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
oehlemar 0:7ee4c6416e08 31 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
oehlemar 0:7ee4c6416e08 32
oehlemar 0:7ee4c6416e08 33 enable = 1; // schaltet die Sensoren ein
oehlemar 0:7ee4c6416e08 34
oehlemar 0:7ee4c6416e08 35 DigitalOut enableMotorDriver(PG_0);
oehlemar 0:7ee4c6416e08 36 DigitalIn motorDriverFault(PD_1);
oehlemar 0:7ee4c6416e08 37 DigitalIn motorDriverWarning(PD_0);
oehlemar 0:7ee4c6416e08 38 PwmOut pwmLeft(PF_9);
oehlemar 0:7ee4c6416e08 39 PwmOut pwmRight(PF_8);
oehlemar 0:7ee4c6416e08 40
oehlemar 0:7ee4c6416e08 41 EncoderCounter counterLeft(PD_12, PD_13);
oehlemar 0:7ee4c6416e08 42 EncoderCounter counterRight(PB_4, PC_7);
oehlemar 0:7ee4c6416e08 43
oehlemar 0:7ee4c6416e08 44 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
oehlemar 0:7ee4c6416e08 45
oehlemar 0:7ee4c6416e08 46 while(1) {
oehlemar 0:7ee4c6416e08 47 /*
oehlemar 0:7ee4c6416e08 48 led0 = 1; // LED is ON
oehlemar 0:7ee4c6416e08 49 led1 = 0;
oehlemar 0:7ee4c6416e08 50 led2 = 0;
oehlemar 0:7ee4c6416e08 51 led3 = 0;
oehlemar 0:7ee4c6416e08 52 led4 = 0;
oehlemar 0:7ee4c6416e08 53 led5 = 0;
oehlemar 0:7ee4c6416e08 54
oehlemar 0:7ee4c6416e08 55 wait(0.2); // 200 ms
oehlemar 0:7ee4c6416e08 56
oehlemar 0:7ee4c6416e08 57 led0 = 0; // LED is ON
oehlemar 0:7ee4c6416e08 58 led1 = 1;
oehlemar 0:7ee4c6416e08 59 led2 = 0;
oehlemar 0:7ee4c6416e08 60 led3 = 0;
oehlemar 0:7ee4c6416e08 61 led4 = 0;
oehlemar 0:7ee4c6416e08 62 led5 = 0;
oehlemar 0:7ee4c6416e08 63
oehlemar 0:7ee4c6416e08 64 wait(0.2); // 200 ms
oehlemar 0:7ee4c6416e08 65
oehlemar 0:7ee4c6416e08 66 led0 = 0; // LED is ON
oehlemar 0:7ee4c6416e08 67 led1 = 0;
oehlemar 0:7ee4c6416e08 68 led2 = 1;
oehlemar 0:7ee4c6416e08 69 led3 = 0;
oehlemar 0:7ee4c6416e08 70 led4 = 0;
oehlemar 0:7ee4c6416e08 71 led5 = 0;
oehlemar 0:7ee4c6416e08 72
oehlemar 0:7ee4c6416e08 73 wait(0.2); // 200 ms
oehlemar 0:7ee4c6416e08 74
oehlemar 0:7ee4c6416e08 75 led0 = 0; // LED is ON
oehlemar 0:7ee4c6416e08 76 led1 = 0;
oehlemar 0:7ee4c6416e08 77 led2 = 0;
oehlemar 0:7ee4c6416e08 78 led3 = 1;
oehlemar 0:7ee4c6416e08 79 led4 = 0;
oehlemar 0:7ee4c6416e08 80 led5 = 0;
oehlemar 0:7ee4c6416e08 81
oehlemar 0:7ee4c6416e08 82 wait(0.2); // 200 ms
oehlemar 0:7ee4c6416e08 83
oehlemar 0:7ee4c6416e08 84 led0 = 0; // LED is ON
oehlemar 0:7ee4c6416e08 85 led1 = 0;
oehlemar 0:7ee4c6416e08 86 led2 = 0;
oehlemar 0:7ee4c6416e08 87 led3 = 0;
oehlemar 0:7ee4c6416e08 88 led4 = 1;
oehlemar 0:7ee4c6416e08 89 led5 = 0;
oehlemar 0:7ee4c6416e08 90
oehlemar 0:7ee4c6416e08 91 wait(0.2); // 200 ms
oehlemar 0:7ee4c6416e08 92
oehlemar 0:7ee4c6416e08 93 led0 = 0; // LED is ON
oehlemar 0:7ee4c6416e08 94 led1 = 0;
oehlemar 0:7ee4c6416e08 95 led2 = 0;
oehlemar 0:7ee4c6416e08 96 led3 = 0;
oehlemar 0:7ee4c6416e08 97 led4 = 0;
oehlemar 0:7ee4c6416e08 98 led5 = 1;
oehlemar 0:7ee4c6416e08 99
oehlemar 0:7ee4c6416e08 100 wait(0.2); // 200 ms
oehlemar 0:7ee4c6416e08 101
oehlemar 0:7ee4c6416e08 102
oehlemar 0:7ee4c6416e08 103 AnalogIn distance(PA_0); // Kreieren der Ein- und Ausgangsobjekte
oehlemar 0:7ee4c6416e08 104 DigitalOut enable(PG_1);
oehlemar 0:7ee4c6416e08 105 DigitalOut bit0(PF_0);
oehlemar 0:7ee4c6416e08 106 DigitalOut bit1(PF_1);
oehlemar 0:7ee4c6416e08 107 DigitalOut bit2(PF_2);
oehlemar 0:7ee4c6416e08 108 enable = 1; // schaltet die Sensoren ein
oehlemar 0:7ee4c6416e08 109 bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
oehlemar 0:7ee4c6416e08 110 bit1 = 0;
oehlemar 0:7ee4c6416e08 111 bit2 = 0;
oehlemar 0:7ee4c6416e08 112 float d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
oehlemar 0:7ee4c6416e08 113
oehlemar 0:7ee4c6416e08 114 printf("Distance hinten: %f \r \n",d);
oehlemar 0:7ee4c6416e08 115
oehlemar 0:7ee4c6416e08 116 bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
oehlemar 0:7ee4c6416e08 117 bit1 = 0;
oehlemar 0:7ee4c6416e08 118 bit2 = 0;
oehlemar 0:7ee4c6416e08 119 d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
oehlemar 0:7ee4c6416e08 120
oehlemar 0:7ee4c6416e08 121 printf("Distance hinten-links: %f \r \n",d);
oehlemar 0:7ee4c6416e08 122
oehlemar 0:7ee4c6416e08 123 bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
oehlemar 0:7ee4c6416e08 124 bit1 = 1;
oehlemar 0:7ee4c6416e08 125 bit2 = 0;
oehlemar 0:7ee4c6416e08 126 d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
oehlemar 0:7ee4c6416e08 127
oehlemar 0:7ee4c6416e08 128 printf("Distance vorne-links: %f \r \n",d);
oehlemar 0:7ee4c6416e08 129
oehlemar 0:7ee4c6416e08 130 bit0 = 0; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
oehlemar 0:7ee4c6416e08 131 bit1 = 0;
oehlemar 0:7ee4c6416e08 132 bit2 = 1;
oehlemar 0:7ee4c6416e08 133 d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
oehlemar 0:7ee4c6416e08 134
oehlemar 0:7ee4c6416e08 135 printf("Distance vorne-rechts: %f \r \n",d);
oehlemar 0:7ee4c6416e08 136
oehlemar 0:7ee4c6416e08 137 bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
oehlemar 0:7ee4c6416e08 138 bit1 = 0;
oehlemar 0:7ee4c6416e08 139 bit2 = 1;
oehlemar 0:7ee4c6416e08 140 d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
oehlemar 0:7ee4c6416e08 141
oehlemar 0:7ee4c6416e08 142 printf("Distance hinten-rechts: %f \r \n",d);
oehlemar 0:7ee4c6416e08 143
oehlemar 0:7ee4c6416e08 144 bit0 = 1; // Wahl des Sensors mit dem Multiplexer (Sensor hinten)
oehlemar 0:7ee4c6416e08 145 bit1 = 1;
oehlemar 0:7ee4c6416e08 146 bit2 = 0;
oehlemar 0:7ee4c6416e08 147 d = -0.58f*sqrt(distance)+0.58f; // Lesen der Distanz in [m]
oehlemar 0:7ee4c6416e08 148
oehlemar 0:7ee4c6416e08 149 printf("Distance vorne: %f \r \n",d);
oehlemar 0:7ee4c6416e08 150 */
oehlemar 0:7ee4c6416e08 151
oehlemar 0:7ee4c6416e08 152
oehlemar 0:7ee4c6416e08 153 float distance0 = irSensor0.read(); // gibt die Distanz in [m] zurueck
oehlemar 0:7ee4c6416e08 154 float distance1 = irSensor1.read();
oehlemar 0:7ee4c6416e08 155 float distance2 = irSensor2.read();
oehlemar 0:7ee4c6416e08 156 float distance3 = irSensor3.read();
oehlemar 0:7ee4c6416e08 157 float distance4 = irSensor4.read();
oehlemar 0:7ee4c6416e08 158 float distance5 = irSensor5.read();
oehlemar 0:7ee4c6416e08 159
oehlemar 0:7ee4c6416e08 160 printf("Distance hinten: %f \r \n", distance0);
oehlemar 0:7ee4c6416e08 161 printf("Distance hinten-links: %f \r \n", distance1);
oehlemar 0:7ee4c6416e08 162 printf("Distance vorne-links: %f \r \n", distance2);
oehlemar 0:7ee4c6416e08 163 printf("Distance vorne: %f \r \n", distance3);
oehlemar 0:7ee4c6416e08 164 printf("Distance vorne-rechts: %f \r \n", distance4);
oehlemar 0:7ee4c6416e08 165 printf("Distance hinten-rechts: %f \r \n", distance5);
oehlemar 0:7ee4c6416e08 166
oehlemar 0:7ee4c6416e08 167 if (distance0 <= 0.1) { led0=1;
oehlemar 0:7ee4c6416e08 168
oehlemar 0:7ee4c6416e08 169 }else {led0=0;}
oehlemar 0:7ee4c6416e08 170
oehlemar 0:7ee4c6416e08 171 if (distance1 <= 0.1) { led1=1;
oehlemar 0:7ee4c6416e08 172
oehlemar 0:7ee4c6416e08 173 }else {led1=0;}
oehlemar 0:7ee4c6416e08 174 if (distance2 <= 0.1) { led2=1;
oehlemar 0:7ee4c6416e08 175
oehlemar 0:7ee4c6416e08 176 }else {led2=0;}
oehlemar 0:7ee4c6416e08 177 if (distance3 <= 0.1) { led3=1;
oehlemar 0:7ee4c6416e08 178
oehlemar 0:7ee4c6416e08 179 }else {led3=0;}
oehlemar 0:7ee4c6416e08 180 if (distance4 <= 0.1) { led4=1;
oehlemar 0:7ee4c6416e08 181
oehlemar 0:7ee4c6416e08 182 }else {led4=0;}
oehlemar 0:7ee4c6416e08 183 if (distance5 <= 0.1) { led5=1;
oehlemar 0:7ee4c6416e08 184
oehlemar 0:7ee4c6416e08 185 }else {led5=0;}
oehlemar 0:7ee4c6416e08 186 /*
oehlemar 0:7ee4c6416e08 187 pwmLeft.period(0.00005); // Setzt die Periode auf 50 μs
oehlemar 0:7ee4c6416e08 188 pwmRight.period(0.00005);
oehlemar 0:7ee4c6416e08 189
oehlemar 0:7ee4c6416e08 190 pwmLeft = 0.5; // Setzt die Duty-Cycle auf 50%
oehlemar 0:7ee4c6416e08 191 pwmRight = 0.5;
oehlemar 0:7ee4c6416e08 192 */
oehlemar 0:7ee4c6416e08 193 enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
oehlemar 0:7ee4c6416e08 194
oehlemar 0:7ee4c6416e08 195 //pwmLeft = 0.7; // Duty-Cycle von 70%
oehlemar 0:7ee4c6416e08 196 //pwmRight = 0.3;
oehlemar 0:7ee4c6416e08 197
oehlemar 0:7ee4c6416e08 198
oehlemar 0:7ee4c6416e08 199 controller.setDesiredSpeedLeft(100.0); // Drehzahl in [rpm]
oehlemar 0:7ee4c6416e08 200 controller.setDesiredSpeedRight(-100.0);
oehlemar 0:7ee4c6416e08 201
oehlemar 0:7ee4c6416e08 202 wait(0.2);
oehlemar 0:7ee4c6416e08 203
oehlemar 0:7ee4c6416e08 204 }
oehlemar 0:7ee4c6416e08 205
oehlemar 0:7ee4c6416e08 206 }