Format / 2013_11_Legosensor

h

Dependencies:   Motordriver TextLCD mbed

main.cpp@0:be945302062c, 2014-01-11 (annotated)

Committer:
Formatd
Date:
Sat Jan 11 13:07:18 2014 +0000
Revision:
0:be945302062c
h

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Formatd 0:be945302062c 1 // ######################### KeplerBRAIN V3 - 08.11.2013 #########################
Formatd 0:be945302062c 2 // #
Formatd 0:be945302062c 3 // # Funktionen und deren Nutzung der KeplerBRAIN Library
Formatd 0:be945302062c 4 // #
Formatd 0:be945302062c 5 // #
Formatd 0:be945302062c 6 // # MOTORAUSGÄNGE
Formatd 0:be945302062c 7 // #
Formatd 0:be945302062c 8 // # Zum Ansteuern der Motoren dienen die beiden angeführten Funktionen.
Formatd 0:be945302062c 9 // # Die erste steuert die Geschwindigkeit zwischen 0% und 90 %, die
Formatd 0:be945302062c 10 // # Drehrichtung wird durch das Vorzeichen des übergebenen Dezimalwerts
Formatd 0:be945302062c 11 // # festgelegt. Die zweite Funktion dient zum apprupten Anhalten eines
Formatd 0:be945302062c 12 // # Motors ohne dass dieser nachläuft.
Formatd 0:be945302062c 13 // #
Formatd 0:be945302062c 14 // # Funktion: void WRITE_MOTOR_SPEED(int number, float speed)
Formatd 0:be945302062c 15 // # Parameter number: int 1, 2, 3, 4
Formatd 0:be945302062c 16 // # Parameter speed: float -0.9 bis 0.9 (Vorzeichen: Drehrichtung)
Formatd 0:be945302062c 17 // #
Formatd 0:be945302062c 18 // # Funktion: void WRITE_MOTOR_STOP(int number)
Formatd 0:be945302062c 19 // # Parameter number: ganze Zahl 1, 2, 3, 4
Formatd 0:be945302062c 20 // #
Formatd 0:be945302062c 21 // #
Formatd 0:be945302062c 22 // # DIGITALE SENSOREN AN DEN IO-BUCHSEN
Formatd 0:be945302062c 23 // #
Formatd 0:be945302062c 24 // # Werden an einen der 12 IO-Buchsen digitale Sensoren, also Sensoren
Formatd 0:be945302062c 25 // # die beim Auslösen eine Verbindung zwischen dem IO-Pin des Mikrokontrollers
Formatd 0:be945302062c 26 // # und GND herstellen (Taster, Schalter, ...) angeschlossen, so muss der
Formatd 0:be945302062c 27 // # entsprechende # IO-Pin als digitaler Eingang konfiguriert werden. Dies
Formatd 0:be945302062c 28 // # geschieht am Anfang der Bibliothek im Block "Declatation of IO-Ports".
Formatd 0:be945302062c 29 // #
Formatd 0:be945302062c 30 // # Festlegen eines IO-Ports 1 als digitaler Eingang: DigitalIn io1(PTB0);
Formatd 0:be945302062c 31 // #
Formatd 0:be945302062c 32 // # Das Einlesen des Sensor-Status erfolgt über die folgende Funktion, der
Formatd 0:be945302062c 33 // # Status des IO-Eingangs wird in einer Variable gespeichert.
Formatd 0:be945302062c 34 // #
Formatd 0:be945302062c 35 // # Funktion: void READ_IO_DIGITAL_IN(char number)
Formatd 0:be945302062c 36 // # Parameter number: int 1, 2, 3, ..., 12
Formatd 0:be945302062c 37 // # Variable: bool BOOL_IO1_DIGITAL_VALUE true oder false
Formatd 0:be945302062c 38 // #
Formatd 0:be945302062c 39 // #
Formatd 0:be945302062c 40 // # ANALOGE SENSOREN AN DEN IO-BUCHSEN
Formatd 0:be945302062c 41 // #
Formatd 0:be945302062c 42 // # Werden an einen der ersten 5 IO-Buchsen Sensoren, die Spannungswerte
Formatd 0:be945302062c 43 // # zwischen 0 V und 3.3 V liefern, angeschlossen, so muss der entsprechende
Formatd 0:be945302062c 44 // # IO-Pin als analoger Eingang konfiguriert werden. Dies geschieht am Anfang
Formatd 0:be945302062c 45 // # der Bibliothek im Block "Declatation of IO-Ports".
Formatd 0:be945302062c 46 // #
Formatd 0:be945302062c 47 // # Festlegen eines IO-Ports 1 als digitaler Eingang: AnalogIn io1(PTB0);
Formatd 0:be945302062c 48 // #
Formatd 0:be945302062c 49 // # Das Einlesen des Sensor-Status erfolgt über die folgende Funktion, der
Formatd 0:be945302062c 50 // # entsprechende Spannungswert wird in einer Variable abgelegt.
Formatd 0:be945302062c 51 // #
Formatd 0:be945302062c 52 // # Funktion: void READ_IO_ANALOG_IN(char number)
Formatd 0:be945302062c 53 // # Parameter number: int 1, 2, 3, 4, 5
Formatd 0:be945302062c 54 // # Variable: float FLOAT_IO1_ANALOG_VALUE 0.0 ... 1.0
Formatd 0:be945302062c 55 // #
Formatd 0:be945302062c 56 // #
Formatd 0:be945302062c 57 // # LEUCHTDIODEN
Formatd 0:be945302062c 58 // #
Formatd 0:be945302062c 59 // # Zum Aus- und Einschalten der Leuchtdioden werden die folgenden
Formatd 0:be945302062c 60 // # drei Funtkionen verwendet:
Formatd 0:be945302062c 61 // #
Formatd 0:be945302062c 62 // # Funktion: void WRITE_LED_ROT(bool wert)
Formatd 0:be945302062c 63 // # Funktion: void WRITE_LED_GELB(bool wert)
Formatd 0:be945302062c 64 // # Funktion: void WRITE_LED_GRUEN(bool wert)
Formatd 0:be945302062c 65 // # Parameter number: bool true oder false
Formatd 0:be945302062c 66 // #
Formatd 0:be945302062c 67 // #
Formatd 0:be945302062c 68 // # TASTER
Formatd 0:be945302062c 69 // #
Formatd 0:be945302062c 70 // # Zum Auswerten der beiden Taster muss zuerst eine Funktion aufgerufen
Formatd 0:be945302062c 71 // # werden. Im Anschluss daran kann auf das Drücken eines Tasters durch
Formatd 0:be945302062c 72 // # Auslesen des Werts der beiden Tastervariablen reagiert werden.
Formatd 0:be945302062c 73 // #
Formatd 0:be945302062c 74 // # Funktion: void READ_TASTER()
Formatd 0:be945302062c 75 // #
Formatd 0:be945302062c 76 // # Variablen: BOOL_TASTER_LINKS_PRESSED, BOOL_TASTER_RECHTS_PRESSED
Formatd 0:be945302062c 77 // # Werte: bool true oder false
Formatd 0:be945302062c 78 // #
Formatd 0:be945302062c 79 // #
Formatd 0:be945302062c 80 // # DISPLAY
Formatd 0:be945302062c 81 // #
Formatd 0:be945302062c 82 // # Die Ausgabe von Text auf dem Display erfolgt über die folgende Funktion,
Formatd 0:be945302062c 83 // # der die Position der Ausgabe und ein String übergeben werden. Der
Formatd 0:be945302062c 84 // # String muss in einem eigens dafür definierten char-Array vorliegen.
Formatd 0:be945302062c 85 // #
Formatd 0:be945302062c 86 // # Funktion: void WRITE_DISPLAY(char zeile, char zeichen, const char *text)
Formatd 0:be945302062c 87 // # Parameter zeile: char 1, 2
Formatd 0:be945302062c 88 // # Parameter zeichen: char 1, 2, ..., 16
Formatd 0:be945302062c 89 // # Parameter *text: char[]
Formatd 0:be945302062c 90 // #
Formatd 0:be945302062c 91 // # Beispiel:
Formatd 0:be945302062c 92 // #
Formatd 0:be945302062c 93 // # char MeinText[16];
Formatd 0:be945302062c 94 // # MeinText = "Text Anzeige 123";
Formatd 0:be945302062c 95 // # WRITE_DISPLAY(1,1,MeinText);
Formatd 0:be945302062c 96 // #
Formatd 0:be945302062c 97 // #
Formatd 0:be945302062c 98 // # I2C PYROSENSOR MLX90614
Formatd 0:be945302062c 99 // #
Formatd 0:be945302062c 100 // # Dieser Sensor wird an den I2C Bus angeschlossen und kann die
Formatd 0:be945302062c 101 // # Objekttemperatur aufgrund dessen Infrarotstrahlung und die Umgebungs-
Formatd 0:be945302062c 102 // # temperatur messen. Den beiden Funktionen muss die Adresse übergeben
Formatd 0:be945302062c 103 // # werden und liefern die Temperatur in Grad Celsius. Diese werden in
Formatd 0:be945302062c 104 // # zwei Variablen abgelegt.
Formatd 0:be945302062c 105 // #
Formatd 0:be945302062c 106 // # Funktion: char READ_I2C_MLX90614_OBJ(char address)
Formatd 0:be945302062c 107 // # Parameter zeile: char 0xB4, 0xB6, 0xB8, 0xBA
Formatd 0:be945302062c 108 // # Variable: int INT_I2C_MLX90614_0xB4_OBJ_VALUE
Formatd 0:be945302062c 109 // #
Formatd 0:be945302062c 110 // # Funktion: char READ_I2C_MLX90614_ENV(char address)
Formatd 0:be945302062c 111 // # Parameter zeile: char 0xB4, 0xB6, 0xB8, 0xBA
Formatd 0:be945302062c 112 // # Variable: int INT_I2C_MLX90614_0xB4_ENV_VALUE
Formatd 0:be945302062c 113 // #
Formatd 0:be945302062c 114 // #
Formatd 0:be945302062c 115 // # I2C KOMPASSSENSOR HMC6352
Formatd 0:be945302062c 116 // #
Formatd 0:be945302062c 117 // # Dieser Sensor wird an den I2C Bus angeschlossen und liefert durch
Formatd 0:be945302062c 118 // # den Aufruf der folgenden Funktion die Richtung bezogen auf den
Formatd 0:be945302062c 119 // # magnetischen Nordpol in Grad.
Formatd 0:be945302062c 120 // #
Formatd 0:be945302062c 121 // # Funktion: void READ_I2C_HMC6352()
Formatd 0:be945302062c 122 // # Variable: int INT_I2C_HMC6352_VALUE 0, ..., 359
Formatd 0:be945302062c 123
Formatd 0:be945302062c 124
Formatd 0:be945302062c 125 // #################### Beginn Import externe Librarys ####################
Formatd 0:be945302062c 126
Formatd 0:be945302062c 127 #include "mbed.h"
Formatd 0:be945302062c 128 #include "TextLCD.h"
Formatd 0:be945302062c 129 #include "motordriver.h"
Formatd 0:be945302062c 130
Formatd 0:be945302062c 131 // #################### End Import externe Librarys ####################
Formatd 0:be945302062c 132
Formatd 0:be945302062c 133
Formatd 0:be945302062c 134 // #################### Beginn Declaration of IO-Ports ####################
Formatd 0:be945302062c 135
Formatd 0:be945302062c 136 DigitalIn io1(PTB0); // IO 1 DigitalIn, DigitalOut, AnalogIn
Formatd 0:be945302062c 137 AnalogIn io2(PTB1); // IO 2 DigitalIn, DigitalOut, AnalogIn
Formatd 0:be945302062c 138 DigitalIn io3(PTB2); // IO 3 DigitalIn, DigitalOut, AnalogIn
Formatd 0:be945302062c 139 DigitalIn io4(PTB3); // IO 4 DigitalIn, DigitalOut, AnalogIn
Formatd 0:be945302062c 140 DigitalIn io5(PTC2); // IO 5 DigitalIn, DigitalOut, AnalogIn
Formatd 0:be945302062c 141 DigitalIn io6(PTC10); // IO 6 DigitalIn, DigitalOut
Formatd 0:be945302062c 142 DigitalIn io7(PTE5); // IO 7 DigitalIn, DigitalOut
Formatd 0:be945302062c 143 DigitalIn io8(PTE20); // IO 8 DigitalIn, DigitalOut
Formatd 0:be945302062c 144 DigitalIn io9(PTE21); // IO 9 DigitalIn, DigitalOut
Formatd 0:be945302062c 145 DigitalIn io10(PTE30); // IO 10 DigitalIn, DigitalOut, AnalogOut // if AnalogOut REMOVE // in WRITE_IO_ANALOG_OUT
Formatd 0:be945302062c 146 DigitalIn io11(PTA5); // IO 11 DigitalIn, DigitalOut, PwmOut // if AnalogOut REMOVE // in WRITE_IO_PWM
Formatd 0:be945302062c 147 DigitalIn io12(PTA4); // IO 12 DigitalIn, DigitalOut, PwmOut // if AnalogOut REMOVE // in WRITE_IO_PWM
Formatd 0:be945302062c 148
Formatd 0:be945302062c 149 // #################### End Declaration of IO-Ports ####################
Formatd 0:be945302062c 150
Formatd 0:be945302062c 151
Formatd 0:be945302062c 152 // #################### Begin of KeplerBRAIN V3 Library ####################
Formatd 0:be945302062c 153
Formatd 0:be945302062c 154 // ********** KL25Z PIN-Belegungen **********
Formatd 0:be945302062c 155
Formatd 0:be945302062c 156 // PTA20/RST Taster Reset S1
Formatd 0:be945302062c 157 // GND0 GND
Formatd 0:be945302062c 158 // VIN_5_9V 5V
Formatd 0:be945302062c 159 // PTB8 Taster S4 links
Formatd 0:be945302062c 160 // PTB9 Taster S3 rechts
Formatd 0:be945302062c 161
Formatd 0:be945302062c 162 // PTE22 Led2 rot
Formatd 0:be945302062c 163 // PTE23 Led3 gelb
Formatd 0:be945302062c 164 // PTE29 Led1 grün
Formatd 0:be945302062c 165
Formatd 0:be945302062c 166 // PTD7 Display RS
Formatd 0:be945302062c 167 // PTD6 Display E
Formatd 0:be945302062c 168 // PTE31 Display D4
Formatd 0:be945302062c 169 // PTA17 Display D5
Formatd 0:be945302062c 170 // PTA16 Display D6
Formatd 0:be945302062c 171 // PTC17 Display D7
Formatd 0:be945302062c 172
Formatd 0:be945302062c 173 // PTD5 Motor1 IN1
Formatd 0:be945302062c 174 // PTA13 Motor1 IN2
Formatd 0:be945302062c 175 // PTC9 Motor1 EN
Formatd 0:be945302062c 176
Formatd 0:be945302062c 177 // PTC6 Motor2 IN1
Formatd 0:be945302062c 178 // PTC5 Motor2 IN2
Formatd 0:be945302062c 179 // PTC8 Motor2 EN
Formatd 0:be945302062c 180
Formatd 0:be945302062c 181 // PTC4 Motor3 IN1
Formatd 0:be945302062c 182 // PTC3 Motor3 IN2
Formatd 0:be945302062c 183 // PTA12 Motor3 EN
Formatd 0:be945302062c 184
Formatd 0:be945302062c 185 // PTC7 Motor4 IN1
Formatd 0:be945302062c 186 // PTC0 Motor4 IN2
Formatd 0:be945302062c 187 // PTD4 Motor4 EN
Formatd 0:be945302062c 188
Formatd 0:be945302062c 189 // PTB0 IO1/AIN
Formatd 0:be945302062c 190 // PTB1 IO2/AIN
Formatd 0:be945302062c 191 // PTB2 IO3/AIN
Formatd 0:be945302062c 192 // PTB3 IO4/AIN
Formatd 0:be945302062c 193 // PTC2 IO5/AIN
Formatd 0:be945302062c 194 // PTC10 IO6
Formatd 0:be945302062c 195
Formatd 0:be945302062c 196 // PTE5 IO7
Formatd 0:be945302062c 197 // PTE20 IO8
Formatd 0:be945302062c 198 // PTE21 IO9
Formatd 0:be945302062c 199 // PTE30 IO10/AOUT
Formatd 0:be945302062c 200 // PTA5 IO11/PWM
Formatd 0:be945302062c 201 // PTA4 IO12/PWM
Formatd 0:be945302062c 202
Formatd 0:be945302062c 203 // PTE0 I2C SCL
Formatd 0:be945302062c 204 // PTE1 I2C SDA
Formatd 0:be945302062c 205
Formatd 0:be945302062c 206 // PTD1 SPI SCK
Formatd 0:be945302062c 207 // PTD3 SPI MISO
Formatd 0:be945302062c 208 // PTD2 SPI MOSI
Formatd 0:be945302062c 209 // PTD0 SPI SEL
Formatd 0:be945302062c 210
Formatd 0:be945302062c 211 // PTA2 SERIAL TX
Formatd 0:be945302062c 212 // PTA1 SERIAL RX
Formatd 0:be945302062c 213
Formatd 0:be945302062c 214
Formatd 0:be945302062c 215 // ********** Deklaration Objekte Variablen intern **********
Formatd 0:be945302062c 216
Formatd 0:be945302062c 217 I2C i2c(PTE0, PTE1); // I2C Objekt
Formatd 0:be945302062c 218
Formatd 0:be945302062c 219 TextLCD display(PTD7, PTD6, PTE31, PTA17, PTA16, PTC17, TextLCD::LCD16x2); // Display RS, EN, D4, D5, D6, D7, Type
Formatd 0:be945302062c 220
Formatd 0:be945302062c 221 Motor motor1(PTC9, PTD5, PTA13, 1); // Motor 1 pwm, fwd, rev, can brake
Formatd 0:be945302062c 222 Motor motor2(PTC8, PTC6, PTC5, 1); // Motor 2 pwm, fwd, rev, can brake
Formatd 0:be945302062c 223 Motor motor3(PTA12, PTC4, PTC3, 1); // Motor 3 pwm, fwd, rev, can brake
Formatd 0:be945302062c 224 Motor motor4(PTD4, PTC7, PTC0, 1); // Motor 4 pwm, fwd, rev, can brake
Formatd 0:be945302062c 225
Formatd 0:be945302062c 226 bool bool_motor1_coast, bool_motor2_coast, bool_motor3_coast, bool_motor4_coast; // Motor wartet nach stop auf coast
Formatd 0:be945302062c 227
Formatd 0:be945302062c 228 Timeout wait_motor1_coast; // Timeout zwischen Motor stop und Motor coast
Formatd 0:be945302062c 229 Timeout wait_motor2_coast; // Timeout zwischen Motor stop und Motor coast
Formatd 0:be945302062c 230 Timeout wait_motor3_coast; // Timeout zwischen Motor stop und Motor coast
Formatd 0:be945302062c 231 Timeout wait_motor4_coast; // Timeout zwischen Motor stop und Motor coast
Formatd 0:be945302062c 232
Formatd 0:be945302062c 233 DigitalOut led_rot(PTE22); // Led rot
Formatd 0:be945302062c 234 DigitalOut led_gelb(PTE23); // Led gelb
Formatd 0:be945302062c 235 DigitalOut led_gruen(PTE29); // Led grün
Formatd 0:be945302062c 236
Formatd 0:be945302062c 237 DigitalIn taster_links(PTB8); // Taster links
Formatd 0:be945302062c 238 DigitalIn taster_rechts(PTB9); // Taster links
Formatd 0:be945302062c 239
Formatd 0:be945302062c 240 bool bool_taster_links_down;
Formatd 0:be945302062c 241 bool bool_taster_rechts_down;
Formatd 0:be945302062c 242
Formatd 0:be945302062c 243 // ********** Deklaration Funktionen intern **********
Formatd 0:be945302062c 244
Formatd 0:be945302062c 245 void motor1_coast()
Formatd 0:be945302062c 246 {
Formatd 0:be945302062c 247 if (bool_motor1_coast) motor1.coast();
Formatd 0:be945302062c 248 }
Formatd 0:be945302062c 249
Formatd 0:be945302062c 250 void motor2_coast()
Formatd 0:be945302062c 251 {
Formatd 0:be945302062c 252 if (bool_motor2_coast) motor2.coast();
Formatd 0:be945302062c 253 }
Formatd 0:be945302062c 254
Formatd 0:be945302062c 255 void motor3_coast()
Formatd 0:be945302062c 256 {
Formatd 0:be945302062c 257 if (bool_motor3_coast) motor3.coast();
Formatd 0:be945302062c 258 }
Formatd 0:be945302062c 259
Formatd 0:be945302062c 260 void motor4_coast()
Formatd 0:be945302062c 261 {
Formatd 0:be945302062c 262 if (bool_motor4_coast) motor4.coast();
Formatd 0:be945302062c 263 }
Formatd 0:be945302062c 264
Formatd 0:be945302062c 265
Formatd 0:be945302062c 266 // ********** Deklaration Variablen Library **********
Formatd 0:be945302062c 267
Formatd 0:be945302062c 268 bool BOOL_TASTER_LINKS_PRESSED, BOOL_TASTER_RECHTS_PRESSED;
Formatd 0:be945302062c 269 bool BOOL_IO1_DIGITAL_VALUE, BOOL_IO2_DIGITAL_VALUE, BOOL_IO3_DIGITAL_VALUE, BOOL_IO4_DIGITAL_VALUE,
Formatd 0:be945302062c 270 BOOL_IO5_DIGITAL_VALUE, BOOL_IO6_DIGITAL_VALUE, BOOL_IO7_DIGITAL_VALUE, BOOL_IO8_DIGITAL_VALUE,
Formatd 0:be945302062c 271 BOOL_IO9_DIGITAL_VALUE, BOOL_IO10_DIGITAL_VALUE, BOOL_IO11_DIGITAL_VALUE, BOOL_IO12_DIGITAL_VALUE;
Formatd 0:be945302062c 272 float BOOL_IO1_ANALOG_VALUE, BOOL_IO2_ANALOG_VALUE, BOOL_IO3_ANALOG_VALUE, BOOL_IO4_ANALOG_VALUE, BOOL_IO5_ANALOG_VALUE;
Formatd 0:be945302062c 273 int INT_I2C_MLX90614_0xB4_OBJ_VALUE, INT_I2C_MLX90614_0xB4_ENV_VALUE;
Formatd 0:be945302062c 274 int INT_I2C_MLX90614_0xB6_OBJ_VALUE, INT_I2C_MLX90614_0xB6_ENV_VALUE;
Formatd 0:be945302062c 275 int INT_I2C_HMC6352_VALUE;
Formatd 0:be945302062c 276 char CHAR_I2C_LINE_S1_VALUE,CHAR_I2C_LINE_S2_VALUE,CHAR_I2C_LINE_S3_VALUE,CHAR_I2C_LINE_S4_VALUE,
Formatd 0:be945302062c 277 CHAR_I2C_LINE_S5_VALUE,CHAR_I2C_LINE_S6_VALUE,CHAR_I2C_LINE_S7_VALUE,CHAR_I2C_LINE_S8_VALUE;
Formatd 0:be945302062c 278 char CHAR_IRSEEKER2_DIRECTION, CHAR_IRSEEKER2_S1_VALUE, CHAR_IRSEEKER2_S2_VALUE, CHAR_IRSEEKER2_S3_VALUE,
Formatd 0:be945302062c 279 CHAR_IRSEEKER2_S4_VALUE, CHAR_IRSEEKER2_S5_VALUE;
Formatd 0:be945302062c 280
Formatd 0:be945302062c 281 // ********** Deklaration Funktionen Library **********
Formatd 0:be945302062c 282
Formatd 0:be945302062c 283 void KEPLERBRAIN_INIT()
Formatd 0:be945302062c 284 {
Formatd 0:be945302062c 285 bool_taster_links_down = false;
Formatd 0:be945302062c 286 bool_taster_rechts_down = false;
Formatd 0:be945302062c 287
Formatd 0:be945302062c 288 BOOL_TASTER_LINKS_PRESSED = false;
Formatd 0:be945302062c 289 BOOL_TASTER_RECHTS_PRESSED = false;
Formatd 0:be945302062c 290
Formatd 0:be945302062c 291 BOOL_IO1_DIGITAL_VALUE = false;
Formatd 0:be945302062c 292 BOOL_IO2_DIGITAL_VALUE = false;
Formatd 0:be945302062c 293 BOOL_IO3_DIGITAL_VALUE = false;
Formatd 0:be945302062c 294 BOOL_IO4_DIGITAL_VALUE = false;
Formatd 0:be945302062c 295 BOOL_IO5_DIGITAL_VALUE = false;
Formatd 0:be945302062c 296 BOOL_IO6_DIGITAL_VALUE = false;
Formatd 0:be945302062c 297 BOOL_IO7_DIGITAL_VALUE = false;
Formatd 0:be945302062c 298 BOOL_IO8_DIGITAL_VALUE = false;
Formatd 0:be945302062c 299 BOOL_IO9_DIGITAL_VALUE = false;
Formatd 0:be945302062c 300 BOOL_IO10_DIGITAL_VALUE = false;
Formatd 0:be945302062c 301 BOOL_IO11_DIGITAL_VALUE = false;
Formatd 0:be945302062c 302 BOOL_IO12_DIGITAL_VALUE = false;
Formatd 0:be945302062c 303
Formatd 0:be945302062c 304 BOOL_IO1_ANALOG_VALUE = 0.0;
Formatd 0:be945302062c 305 BOOL_IO2_ANALOG_VALUE = 0.0;
Formatd 0:be945302062c 306 BOOL_IO3_ANALOG_VALUE = 0.0;
Formatd 0:be945302062c 307 BOOL_IO4_ANALOG_VALUE = 0.0;
Formatd 0:be945302062c 308 BOOL_IO5_ANALOG_VALUE = 0.0;
Formatd 0:be945302062c 309
Formatd 0:be945302062c 310 CHAR_IRSEEKER2_DIRECTION = 0;
Formatd 0:be945302062c 311 CHAR_IRSEEKER2_S1_VALUE = 0;
Formatd 0:be945302062c 312 CHAR_IRSEEKER2_S2_VALUE = 0;
Formatd 0:be945302062c 313 CHAR_IRSEEKER2_S3_VALUE = 0;
Formatd 0:be945302062c 314 CHAR_IRSEEKER2_S4_VALUE = 0;
Formatd 0:be945302062c 315 CHAR_IRSEEKER2_S5_VALUE = 0;
Formatd 0:be945302062c 316
Formatd 0:be945302062c 317 INT_I2C_MLX90614_0xB4_OBJ_VALUE = 0;
Formatd 0:be945302062c 318 INT_I2C_MLX90614_0xB4_ENV_VALUE = 0;
Formatd 0:be945302062c 319 INT_I2C_MLX90614_0xB6_OBJ_VALUE = 0;
Formatd 0:be945302062c 320 INT_I2C_MLX90614_0xB6_ENV_VALUE = 0;
Formatd 0:be945302062c 321
Formatd 0:be945302062c 322 INT_I2C_HMC6352_VALUE = 0;
Formatd 0:be945302062c 323
Formatd 0:be945302062c 324 // Initialisierung Motoren
Formatd 0:be945302062c 325
Formatd 0:be945302062c 326 bool_motor1_coast = false;
Formatd 0:be945302062c 327 bool_motor2_coast = false;
Formatd 0:be945302062c 328 bool_motor3_coast = false;
Formatd 0:be945302062c 329 bool_motor4_coast = false;
Formatd 0:be945302062c 330 }
Formatd 0:be945302062c 331
Formatd 0:be945302062c 332
Formatd 0:be945302062c 333 void READ_TASTER()
Formatd 0:be945302062c 334 {
Formatd 0:be945302062c 335 BOOL_TASTER_LINKS_PRESSED = false;
Formatd 0:be945302062c 336 if (!taster_links)
Formatd 0:be945302062c 337 {
Formatd 0:be945302062c 338 if (bool_taster_links_down==false)
Formatd 0:be945302062c 339 {
Formatd 0:be945302062c 340 BOOL_TASTER_LINKS_PRESSED = true;
Formatd 0:be945302062c 341 }
Formatd 0:be945302062c 342 bool_taster_links_down = true;
Formatd 0:be945302062c 343 }
Formatd 0:be945302062c 344 else
Formatd 0:be945302062c 345 {
Formatd 0:be945302062c 346 bool_taster_links_down = false;
Formatd 0:be945302062c 347 }
Formatd 0:be945302062c 348
Formatd 0:be945302062c 349 BOOL_TASTER_RECHTS_PRESSED = false;
Formatd 0:be945302062c 350 if (!taster_rechts)
Formatd 0:be945302062c 351 {
Formatd 0:be945302062c 352 if (bool_taster_rechts_down==false)
Formatd 0:be945302062c 353 {
Formatd 0:be945302062c 354 BOOL_TASTER_RECHTS_PRESSED = true;
Formatd 0:be945302062c 355 }
Formatd 0:be945302062c 356 bool_taster_rechts_down = true;
Formatd 0:be945302062c 357 }
Formatd 0:be945302062c 358 else
Formatd 0:be945302062c 359 {
Formatd 0:be945302062c 360 bool_taster_rechts_down = false;
Formatd 0:be945302062c 361 }
Formatd 0:be945302062c 362 }
Formatd 0:be945302062c 363
Formatd 0:be945302062c 364
Formatd 0:be945302062c 365 void READ_IO_DIGITAL_IN(char number)
Formatd 0:be945302062c 366 {
Formatd 0:be945302062c 367 switch(number)
Formatd 0:be945302062c 368 {
Formatd 0:be945302062c 369 case 1:
Formatd 0:be945302062c 370 if (!io1) BOOL_IO1_DIGITAL_VALUE=true; else BOOL_IO1_DIGITAL_VALUE=false;
Formatd 0:be945302062c 371 break;
Formatd 0:be945302062c 372 case 2:
Formatd 0:be945302062c 373 if (!io2) BOOL_IO2_DIGITAL_VALUE=true; else BOOL_IO2_DIGITAL_VALUE=false;
Formatd 0:be945302062c 374 break;
Formatd 0:be945302062c 375 case 3:
Formatd 0:be945302062c 376 if (!io3) BOOL_IO3_DIGITAL_VALUE=true; else BOOL_IO3_DIGITAL_VALUE=false;
Formatd 0:be945302062c 377 break;
Formatd 0:be945302062c 378 case 4:
Formatd 0:be945302062c 379 if (!io4) BOOL_IO4_DIGITAL_VALUE=true; else BOOL_IO4_DIGITAL_VALUE=false;
Formatd 0:be945302062c 380 break;
Formatd 0:be945302062c 381 case 5:
Formatd 0:be945302062c 382 if (!io5) BOOL_IO5_DIGITAL_VALUE=true; else BOOL_IO5_DIGITAL_VALUE=false;
Formatd 0:be945302062c 383 break;
Formatd 0:be945302062c 384 case 6:
Formatd 0:be945302062c 385 if (!io6) BOOL_IO6_DIGITAL_VALUE=true; else BOOL_IO6_DIGITAL_VALUE=false;
Formatd 0:be945302062c 386 break;
Formatd 0:be945302062c 387 case 7:
Formatd 0:be945302062c 388 if (!io7) BOOL_IO7_DIGITAL_VALUE=true; else BOOL_IO7_DIGITAL_VALUE=false;
Formatd 0:be945302062c 389 break;
Formatd 0:be945302062c 390 case 8:
Formatd 0:be945302062c 391 if (!io8) BOOL_IO8_DIGITAL_VALUE=true; else BOOL_IO8_DIGITAL_VALUE=false;
Formatd 0:be945302062c 392 break;
Formatd 0:be945302062c 393 case 9:
Formatd 0:be945302062c 394 if (!io9) BOOL_IO9_DIGITAL_VALUE=true; else BOOL_IO9_DIGITAL_VALUE=false;
Formatd 0:be945302062c 395 break;
Formatd 0:be945302062c 396 case 10:
Formatd 0:be945302062c 397 if (!io10) BOOL_IO10_DIGITAL_VALUE=true; else BOOL_IO10_DIGITAL_VALUE=false;
Formatd 0:be945302062c 398 break;
Formatd 0:be945302062c 399 case 11:
Formatd 0:be945302062c 400 if (!io11) BOOL_IO11_DIGITAL_VALUE=true; else BOOL_IO11_DIGITAL_VALUE=false;
Formatd 0:be945302062c 401 break;
Formatd 0:be945302062c 402 case 12:
Formatd 0:be945302062c 403 if (!io12) BOOL_IO12_DIGITAL_VALUE=true; else BOOL_IO12_DIGITAL_VALUE=false;
Formatd 0:be945302062c 404 break;
Formatd 0:be945302062c 405 }
Formatd 0:be945302062c 406 }
Formatd 0:be945302062c 407
Formatd 0:be945302062c 408
Formatd 0:be945302062c 409 void READ_IO_ANALOG_IN(char number)
Formatd 0:be945302062c 410 {
Formatd 0:be945302062c 411 switch(number)
Formatd 0:be945302062c 412 {
Formatd 0:be945302062c 413 case 1:
Formatd 0:be945302062c 414 BOOL_IO1_ANALOG_VALUE=io1;
Formatd 0:be945302062c 415 break;
Formatd 0:be945302062c 416 case 2:
Formatd 0:be945302062c 417 BOOL_IO2_ANALOG_VALUE=io2;
Formatd 0:be945302062c 418 break;
Formatd 0:be945302062c 419 case 3:
Formatd 0:be945302062c 420 BOOL_IO3_ANALOG_VALUE=io3;
Formatd 0:be945302062c 421 break;
Formatd 0:be945302062c 422 case 4:
Formatd 0:be945302062c 423 BOOL_IO4_ANALOG_VALUE=io4;
Formatd 0:be945302062c 424 break;
Formatd 0:be945302062c 425 case 5:
Formatd 0:be945302062c 426 BOOL_IO5_ANALOG_VALUE=io5;
Formatd 0:be945302062c 427 break;
Formatd 0:be945302062c 428 }
Formatd 0:be945302062c 429 }
Formatd 0:be945302062c 430
Formatd 0:be945302062c 431
Formatd 0:be945302062c 432 void READ_I2C_MLX90614(char address)
Formatd 0:be945302062c 433 {
Formatd 0:be945302062c 434 char cmd[1];
Formatd 0:be945302062c 435 char bytes_env[3];
Formatd 0:be945302062c 436 char bytes_obj[3];
Formatd 0:be945302062c 437 int i2c_Addr = address;
Formatd 0:be945302062c 438
Formatd 0:be945302062c 439 cmd[0] = 0x06;
Formatd 0:be945302062c 440 i2c.write(i2c_Addr, cmd, 1);
Formatd 0:be945302062c 441 i2c.read(i2c_Addr, bytes_env, 3);
Formatd 0:be945302062c 442
Formatd 0:be945302062c 443 cmd[0] = 0x07;
Formatd 0:be945302062c 444 i2c.write(i2c_Addr, cmd, 1);
Formatd 0:be945302062c 445 i2c.read(i2c_Addr, bytes_obj, 3);
Formatd 0:be945302062c 446
Formatd 0:be945302062c 447 switch(address)
Formatd 0:be945302062c 448 {
Formatd 0:be945302062c 449 case 0xB4:
Formatd 0:be945302062c 450 INT_I2C_MLX90614_0xB4_ENV_VALUE = (((bytes_env[0] & 0x007F) << 8) + bytes_env[1]);
Formatd 0:be945302062c 451 INT_I2C_MLX90614_0xB4_OBJ_VALUE = (((bytes_obj[0] & 0x007F) << 8) + bytes_obj[1]);
Formatd 0:be945302062c 452 break;
Formatd 0:be945302062c 453 case 0xB6:
Formatd 0:be945302062c 454 INT_I2C_MLX90614_0xB6_ENV_VALUE = (((bytes_env[0] & 0x007F) << 8) + bytes_env[1]);
Formatd 0:be945302062c 455 INT_I2C_MLX90614_0xB6_OBJ_VALUE = (((bytes_obj[0] & 0x007F) << 8) + bytes_obj[1]);
Formatd 0:be945302062c 456 INT_I2C_MLX90614_0xB6_ENV_VALUE = bytes_env[0];
Formatd 0:be945302062c 457 INT_I2C_MLX90614_0xB6_OBJ_VALUE = bytes_obj[0];
Formatd 0:be945302062c 458 break;
Formatd 0:be945302062c 459 }
Formatd 0:be945302062c 460 }
Formatd 0:be945302062c 461
Formatd 0:be945302062c 462 void READ_I2C_HMC6352()
Formatd 0:be945302062c 463 {
Formatd 0:be945302062c 464 char cmd[1];
Formatd 0:be945302062c 465 char bytes[2];
Formatd 0:be945302062c 466 int i2c_Addr = 0x42;
Formatd 0:be945302062c 467
Formatd 0:be945302062c 468 cmd[0] = 0x41;
Formatd 0:be945302062c 469 i2c.write(i2c_Addr, cmd, 1);
Formatd 0:be945302062c 470 i2c.read(i2c_Addr, bytes, 2);
Formatd 0:be945302062c 471
Formatd 0:be945302062c 472 INT_I2C_HMC6352_VALUE = (bytes[0] * 256 + bytes[1]) / 10;
Formatd 0:be945302062c 473 }
Formatd 0:be945302062c 474
Formatd 0:be945302062c 475
Formatd 0:be945302062c 476 void READ_I2C_LINE()
Formatd 0:be945302062c 477 {
Formatd 0:be945302062c 478 char cmd[1];
Formatd 0:be945302062c 479 char bytes[8];
Formatd 0:be945302062c 480 int i2c_Addr = 0xA0;
Formatd 0:be945302062c 481
Formatd 0:be945302062c 482 cmd[0] = 0x49;
Formatd 0:be945302062c 483 i2c.write(i2c_Addr, cmd, 1);
Formatd 0:be945302062c 484 i2c.read(i2c_Addr, bytes, 8);
Formatd 0:be945302062c 485
Formatd 0:be945302062c 486 CHAR_I2C_LINE_S1_VALUE = bytes[0];
Formatd 0:be945302062c 487 CHAR_I2C_LINE_S2_VALUE = bytes[1];
Formatd 0:be945302062c 488 CHAR_I2C_LINE_S3_VALUE = bytes[2];
Formatd 0:be945302062c 489 CHAR_I2C_LINE_S4_VALUE = bytes[3];
Formatd 0:be945302062c 490 CHAR_I2C_LINE_S5_VALUE = bytes[4];
Formatd 0:be945302062c 491 CHAR_I2C_LINE_S6_VALUE = bytes[5];
Formatd 0:be945302062c 492 CHAR_I2C_LINE_S7_VALUE = bytes[6];
Formatd 0:be945302062c 493 CHAR_I2C_LINE_S8_VALUE = bytes[7];
Formatd 0:be945302062c 494
Formatd 0:be945302062c 495
Formatd 0:be945302062c 496 }
Formatd 0:be945302062c 497
Formatd 0:be945302062c 498
Formatd 0:be945302062c 499 void WRITE_IO_ANALOG_OUT(char number, float value)
Formatd 0:be945302062c 500 {
Formatd 0:be945302062c 501 if (value<0.0) value = 0.0;
Formatd 0:be945302062c 502 if (value>1.0) value = 1.0;
Formatd 0:be945302062c 503 switch(number)
Formatd 0:be945302062c 504 {
Formatd 0:be945302062c 505 case 10:
Formatd 0:be945302062c 506 // io10.write(value);
Formatd 0:be945302062c 507 break;
Formatd 0:be945302062c 508 }
Formatd 0:be945302062c 509 }
Formatd 0:be945302062c 510
Formatd 0:be945302062c 511
Formatd 0:be945302062c 512 void WRITE_IO_PWM(char number, float period, float pulsewidth)
Formatd 0:be945302062c 513 {
Formatd 0:be945302062c 514 switch(number)
Formatd 0:be945302062c 515 {
Formatd 0:be945302062c 516 case 11:
Formatd 0:be945302062c 517 // io11.period = period;
Formatd 0:be945302062c 518 // io11.pulsewidth = pulsewidth;
Formatd 0:be945302062c 519 break;
Formatd 0:be945302062c 520 case 12:
Formatd 0:be945302062c 521 // io12.period = period;
Formatd 0:be945302062c 522 // io12.pulsewidth = pulsewidth;
Formatd 0:be945302062c 523 break;
Formatd 0:be945302062c 524 }
Formatd 0:be945302062c 525 }
Formatd 0:be945302062c 526
Formatd 0:be945302062c 527
Formatd 0:be945302062c 528 void WRITE_LED_ROT(bool wert)
Formatd 0:be945302062c 529 {
Formatd 0:be945302062c 530 if (wert==true) led_rot = 1; else led_rot = 0;
Formatd 0:be945302062c 531 }
Formatd 0:be945302062c 532
Formatd 0:be945302062c 533
Formatd 0:be945302062c 534 void WRITE_LED_GELB(bool wert)
Formatd 0:be945302062c 535 {
Formatd 0:be945302062c 536 if (wert==true) led_gelb = 1; else led_gelb = 0;
Formatd 0:be945302062c 537 }
Formatd 0:be945302062c 538
Formatd 0:be945302062c 539
Formatd 0:be945302062c 540 void WRITE_LED_GRUEN(bool wert)
Formatd 0:be945302062c 541 {
Formatd 0:be945302062c 542 if (wert==true) led_gruen = 1; else led_gruen = 0;
Formatd 0:be945302062c 543 }
Formatd 0:be945302062c 544
Formatd 0:be945302062c 545
Formatd 0:be945302062c 546 void WRITE_MOTOR_SPEED(int number, float speed)
Formatd 0:be945302062c 547 {
Formatd 0:be945302062c 548 if (speed>0.9) speed = 0.9;
Formatd 0:be945302062c 549 if (speed<-0.9) speed = -0.9;
Formatd 0:be945302062c 550
Formatd 0:be945302062c 551 switch(number)
Formatd 0:be945302062c 552 {
Formatd 0:be945302062c 553 case 1:
Formatd 0:be945302062c 554 bool_motor1_coast = false;
Formatd 0:be945302062c 555 motor1.speed(speed);
Formatd 0:be945302062c 556 break;
Formatd 0:be945302062c 557 case 2:
Formatd 0:be945302062c 558 bool_motor2_coast = false;
Formatd 0:be945302062c 559 motor2.speed(speed);
Formatd 0:be945302062c 560 break;
Formatd 0:be945302062c 561 case 3:
Formatd 0:be945302062c 562 bool_motor3_coast = false;
Formatd 0:be945302062c 563 motor3.speed(speed);
Formatd 0:be945302062c 564 break;
Formatd 0:be945302062c 565 case 4:
Formatd 0:be945302062c 566 bool_motor4_coast = false;
Formatd 0:be945302062c 567 motor4.speed(speed);
Formatd 0:be945302062c 568 break;
Formatd 0:be945302062c 569 }
Formatd 0:be945302062c 570 }
Formatd 0:be945302062c 571
Formatd 0:be945302062c 572
Formatd 0:be945302062c 573 void WRITE_MOTOR_STOP(int number)
Formatd 0:be945302062c 574 {
Formatd 0:be945302062c 575 switch(number)
Formatd 0:be945302062c 576 {
Formatd 0:be945302062c 577 case 1:
Formatd 0:be945302062c 578 motor1.stop(1.0);
Formatd 0:be945302062c 579 bool_motor1_coast = true;
Formatd 0:be945302062c 580 wait_motor1_coast.attach(&motor1_coast, 1);
Formatd 0:be945302062c 581 break;
Formatd 0:be945302062c 582 case 2:
Formatd 0:be945302062c 583 motor2.stop(1.0);
Formatd 0:be945302062c 584 bool_motor2_coast = true;
Formatd 0:be945302062c 585 wait_motor2_coast.attach(&motor2_coast, 1);
Formatd 0:be945302062c 586 break;
Formatd 0:be945302062c 587 case 3:
Formatd 0:be945302062c 588 motor3.stop(1.0);
Formatd 0:be945302062c 589 bool_motor3_coast = true;
Formatd 0:be945302062c 590 wait_motor3_coast.attach(&motor3_coast, 1);
Formatd 0:be945302062c 591 break;
Formatd 0:be945302062c 592 case 4:
Formatd 0:be945302062c 593 motor4.stop(1.0);
Formatd 0:be945302062c 594 bool_motor4_coast = true;
Formatd 0:be945302062c 595 wait_motor4_coast.attach(&motor4_coast, 1);
Formatd 0:be945302062c 596 break;
Formatd 0:be945302062c 597 }
Formatd 0:be945302062c 598 }
Formatd 0:be945302062c 599
Formatd 0:be945302062c 600
Formatd 0:be945302062c 601 void WRITE_DISPLAY(char zeile, char zeichen, const char *text)
Formatd 0:be945302062c 602 {
Formatd 0:be945302062c 603 if (zeile<1) zeile = 1;
Formatd 0:be945302062c 604 if (zeile>2) zeile = 2;
Formatd 0:be945302062c 605 if (zeichen<1) zeichen = 1;
Formatd 0:be945302062c 606 if (zeichen>16) zeichen = 16;
Formatd 0:be945302062c 607 zeile--;
Formatd 0:be945302062c 608 zeichen--;
Formatd 0:be945302062c 609
Formatd 0:be945302062c 610 display.locate(zeichen, zeile);
Formatd 0:be945302062c 611 display.printf(text);
Formatd 0:be945302062c 612 }
Formatd 0:be945302062c 613
Formatd 0:be945302062c 614
Formatd 0:be945302062c 615 void READ_I2C_IRSEEKER2()
Formatd 0:be945302062c 616 {
Formatd 0:be945302062c 617 char cmd[1];
Formatd 0:be945302062c 618 char bytes[6];
Formatd 0:be945302062c 619 int i2c_Addr = 0x10;
Formatd 0:be945302062c 620
Formatd 0:be945302062c 621 cmd[0] = 0x49;
Formatd 0:be945302062c 622 i2c.write(i2c_Addr, cmd, 1);
Formatd 0:be945302062c 623 i2c.read(i2c_Addr, bytes, 6);
Formatd 0:be945302062c 624
Formatd 0:be945302062c 625 CHAR_IRSEEKER2_DIRECTION = bytes[0];
Formatd 0:be945302062c 626 CHAR_IRSEEKER2_S1_VALUE = bytes[1];
Formatd 0:be945302062c 627 CHAR_IRSEEKER2_S2_VALUE = bytes[2];
Formatd 0:be945302062c 628 CHAR_IRSEEKER2_S3_VALUE = bytes[3];
Formatd 0:be945302062c 629 CHAR_IRSEEKER2_S4_VALUE = bytes[4];
Formatd 0:be945302062c 630 CHAR_IRSEEKER2_S5_VALUE = bytes[5];
Formatd 0:be945302062c 631
Formatd 0:be945302062c 632 }
Formatd 0:be945302062c 633
Formatd 0:be945302062c 634
Formatd 0:be945302062c 635 // #################### End of KeplerBRAIN V3 Library ####################
Formatd 0:be945302062c 636
Formatd 0:be945302062c 637
Formatd 0:be945302062c 638
Formatd 0:be945302062c 639 void fahren(float spd1, float spd2)
Formatd 0:be945302062c 640 {
Formatd 0:be945302062c 641 spd2+=0.05;
Formatd 0:be945302062c 642 WRITE_MOTOR_SPEED(1, spd1);
Formatd 0:be945302062c 643 WRITE_MOTOR_SPEED(2, spd2);
Formatd 0:be945302062c 644 }
Formatd 0:be945302062c 645
Formatd 0:be945302062c 646 void gerade(float time)
Formatd 0:be945302062c 647 {
Formatd 0:be945302062c 648 fahren(0.6, 0.65);
Formatd 0:be945302062c 649 wait(time);
Formatd 0:be945302062c 650 WRITE_MOTOR_STOP(1);
Formatd 0:be945302062c 651 WRITE_MOTOR_STOP(2);
Formatd 0:be945302062c 652 }
Formatd 0:be945302062c 653
Formatd 0:be945302062c 654 void drehen(int grad) // bool für richtung noch einfügen
Formatd 0:be945302062c 655 {
Formatd 0:be945302062c 656 bool a=true;
Formatd 0:be945302062c 657 int pos=INT_I2C_HMC6352_VALUE;
Formatd 0:be945302062c 658 char textzeile1[16];
Formatd 0:be945302062c 659 grad+=pos;
Formatd 0:be945302062c 660 if(grad>=360)
Formatd 0:be945302062c 661 {
Formatd 0:be945302062c 662 grad-=360;
Formatd 0:be945302062c 663 }
Formatd 0:be945302062c 664 while(a)
Formatd 0:be945302062c 665 {
Formatd 0:be945302062c 666 fahren(0.25,-0.3);
Formatd 0:be945302062c 667 READ_I2C_HMC6352();
Formatd 0:be945302062c 668 pos=INT_I2C_HMC6352_VALUE;
Formatd 0:be945302062c 669 pos-=360;
Formatd 0:be945302062c 670 if (pos>=grad)
Formatd 0:be945302062c 671 {
Formatd 0:be945302062c 672 WRITE_MOTOR_STOP(1);
Formatd 0:be945302062c 673 WRITE_MOTOR_STOP(2);
Formatd 0:be945302062c 674 a=false;
Formatd 0:be945302062c 675 }
Formatd 0:be945302062c 676 sprintf(textzeile1,"Grad: %3u ",INT_I2C_HMC6352_VALUE);
Formatd 0:be945302062c 677 WRITE_DISPLAY(1,1,textzeile1);
Formatd 0:be945302062c 678 WRITE_MOTOR_STOP(1);
Formatd 0:be945302062c 679 WRITE_MOTOR_STOP(2);
Formatd 0:be945302062c 680 wait(0.01);
Formatd 0:be945302062c 681 }
Formatd 0:be945302062c 682 WRITE_MOTOR_STOP(1);
Formatd 0:be945302062c 683 WRITE_MOTOR_STOP(2);
Formatd 0:be945302062c 684
Formatd 0:be945302062c 685 }
Formatd 0:be945302062c 686
Formatd 0:be945302062c 687 Timer timer;
Formatd 0:be945302062c 688
Formatd 0:be945302062c 689 void ausgleichen()
Formatd 0:be945302062c 690 {
Formatd 0:be945302062c 691 int pos=INT_I2C_HMC6352_VALUE;
Formatd 0:be945302062c 692 float error=0;
Formatd 0:be945302062c 693 int Lasterror=0;
Formatd 0:be945302062c 694 int beginn=0,end=0;
Formatd 0:be945302062c 695 float P=0;
Formatd 0:be945302062c 696 float I=0;
Formatd 0:be945302062c 697 float D=0;
Formatd 0:be945302062c 698 float Kp=0.015;
Formatd 0:be945302062c 699 float Ki=0.00002/30;
Formatd 0:be945302062c 700 float Kd=1;
Formatd 0:be945302062c 701 float spd1=0;
Formatd 0:be945302062c 702 float spd2=0;
Formatd 0:be945302062c 703 float minspeed=0.28;
Formatd 0:be945302062c 704 float speed=0.6;
Formatd 0:be945302062c 705
Formatd 0:be945302062c 706 char textzeile1[16];
Formatd 0:be945302062c 707 char textzeile2[16];
Formatd 0:be945302062c 708 bool speederror=false;
Formatd 0:be945302062c 709 int kompass=0;
Formatd 0:be945302062c 710 int kompassNull = INT_I2C_HMC6352_VALUE;
Formatd 0:be945302062c 711
Formatd 0:be945302062c 712 while(1)
Formatd 0:be945302062c 713 {
Formatd 0:be945302062c 714 READ_TASTER();
Formatd 0:be945302062c 715 timer.start();
Formatd 0:be945302062c 716
Formatd 0:be945302062c 717 beginn=timer.read_us()/100;
Formatd 0:be945302062c 718
Formatd 0:be945302062c 719 READ_I2C_HMC6352();
Formatd 0:be945302062c 720
Formatd 0:be945302062c 721 end=timer.read_us()/100;
Formatd 0:be945302062c 722
Formatd 0:be945302062c 723 kompass = INT_I2C_HMC6352_VALUE + 180 - kompassNull;
Formatd 0:be945302062c 724 if(kompass<0) kompass+=360;
Formatd 0:be945302062c 725 if(kompass>359) kompass+=-360;
Formatd 0:be945302062c 726
Formatd 0:be945302062c 727 error=180-kompass;//INT_I2C_HMC6352_VALUE;//180-kompass;
Formatd 0:be945302062c 728
Formatd 0:be945302062c 729 P=Kp*error;
Formatd 0:be945302062c 730 I=I+((end-beginn)*error)*Ki;
Formatd 0:be945302062c 731 D=Kd*((error-Lasterror)/(end-beginn));
Formatd 0:be945302062c 732
Formatd 0:be945302062c 733 Lasterror=error;
Formatd 0:be945302062c 734 if(error==0) I=0;
Formatd 0:be945302062c 735
Formatd 0:be945302062c 736 /* if((spd1<minspeed)&&(spd1>0)) spd1=minspeed;
Formatd 0:be945302062c 737 if((spd1>-minspeed)&&(spd1<0)) spd1=-minspeed;
Formatd 0:be945302062c 738 if((spd2<minspeed)&&(spd2>0)) spd2=minspeed;
Formatd 0:be945302062c 739 if((spd2>-minspeed)&&(spd2<0)) spd2=-minspeed;*/
Formatd 0:be945302062c 740
Formatd 0:be945302062c 741 spd1=speed+(P+I+D);
Formatd 0:be945302062c 742 spd2=speed-(P+I+D);
Formatd 0:be945302062c 743
Formatd 0:be945302062c 744 /*
Formatd 0:be945302062c 745 if((spd1>0)&&(spd1<0.25)) spd1=0.28;
Formatd 0:be945302062c 746 if((spd1<0)&&(spd1>-0.25)) spd1=-0.28;
Formatd 0:be945302062c 747
Formatd 0:be945302062c 748 if((spd2>0)&&(spd2<0.25)) spd2=0.28;
Formatd 0:be945302062c 749 if((spd2<0)&&(spd2>-0.25)) spd2=-0.28;*/
Formatd 0:be945302062c 750
Formatd 0:be945302062c 751
Formatd 0:be945302062c 752 WRITE_MOTOR_SPEED(1, spd1);
Formatd 0:be945302062c 753 WRITE_MOTOR_SPEED(2, spd2);
Formatd 0:be945302062c 754 sprintf(textzeile1,"%.3f %3u ",error,kompass);
Formatd 0:be945302062c 755 sprintf(textzeile2,"%.3f %.3f ",spd1,spd2);
Formatd 0:be945302062c 756
Formatd 0:be945302062c 757 WRITE_DISPLAY(1,1,textzeile1);
Formatd 0:be945302062c 758 WRITE_DISPLAY(2,1,textzeile2);
Formatd 0:be945302062c 759
Formatd 0:be945302062c 760 }
Formatd 0:be945302062c 761 timer.stop();
Formatd 0:be945302062c 762 }
Formatd 0:be945302062c 763
Formatd 0:be945302062c 764 void ballsensor()
Formatd 0:be945302062c 765 {
Formatd 0:be945302062c 766 float error=0;
Formatd 0:be945302062c 767 int Lasterror=0;
Formatd 0:be945302062c 768 int beginn=0,end=0;
Formatd 0:be945302062c 769 float P=0;
Formatd 0:be945302062c 770 float I=0;
Formatd 0:be945302062c 771 float D=0;
Formatd 0:be945302062c 772 float Kp=0.25;
Formatd 0:be945302062c 773 float Ki=0.00002/15;
Formatd 0:be945302062c 774 float Kd=1;
Formatd 0:be945302062c 775 float spd1=0;
Formatd 0:be945302062c 776 float spd2=0;
Formatd 0:be945302062c 777 float minspeed=0.28;
Formatd 0:be945302062c 778 float speed=0;
Formatd 0:be945302062c 779
Formatd 0:be945302062c 780 char textzeile1[16];
Formatd 0:be945302062c 781 char textzeile2[16];
Formatd 0:be945302062c 782 bool speederror=false;
Formatd 0:be945302062c 783 int ballsensor=0;
Formatd 0:be945302062c 784
Formatd 0:be945302062c 785 while(1)
Formatd 0:be945302062c 786 {
Formatd 0:be945302062c 787 READ_TASTER();
Formatd 0:be945302062c 788 timer.start();
Formatd 0:be945302062c 789
Formatd 0:be945302062c 790 beginn=timer.read_us()/100;
Formatd 0:be945302062c 791
Formatd 0:be945302062c 792 READ_I2C_HMC6352();
Formatd 0:be945302062c 793 READ_I2C_IRSEEKER2();
Formatd 0:be945302062c 794
Formatd 0:be945302062c 795 end=timer.read_us()/100;
Formatd 0:be945302062c 796
Formatd 0:be945302062c 797 ballsensor = CHAR_IRSEEKER2_DIRECTION;//ballsensor = INT_I2C_HMC6352_VALUE + 180 - kompassNull;
Formatd 0:be945302062c 798
Formatd 0:be945302062c 799
Formatd 0:be945302062c 800 error=(ballsensor-4);//INT_I2C_HMC6352_VALUE;//180-kompass;
Formatd 0:be945302062c 801
Formatd 0:be945302062c 802 P=Kp*error;
Formatd 0:be945302062c 803 I=I+((end-beginn)*error)*Ki;
Formatd 0:be945302062c 804 D=Kd*((error-Lasterror)/(end-beginn));
Formatd 0:be945302062c 805
Formatd 0:be945302062c 806 Lasterror=error;
Formatd 0:be945302062c 807 if(error==0) I=0;
Formatd 0:be945302062c 808
Formatd 0:be945302062c 809 /* if((spd1<minspeed)&&(spd1>0)) spd1=minspeed;
Formatd 0:be945302062c 810 if((spd1>-minspeed)&&(spd1<0)) spd1=-minspeed;
Formatd 0:be945302062c 811 if((spd2<minspeed)&&(spd2>0)) spd2=minspeed;
Formatd 0:be945302062c 812 if((spd2>-minspeed)&&(spd2<0)) spd2=-minspeed;*/
Formatd 0:be945302062c 813
Formatd 0:be945302062c 814 spd1=speed+(P+I+D);
Formatd 0:be945302062c 815 spd2=speed-(P+I+D);
Formatd 0:be945302062c 816
Formatd 0:be945302062c 817 /*
Formatd 0:be945302062c 818 if((spd1>0)&&(spd1<0.25)) spd1=0.28;
Formatd 0:be945302062c 819 if((spd1<0)&&(spd1>-0.25)) spd1=-0.28;
Formatd 0:be945302062c 820
Formatd 0:be945302062c 821 if((spd2>0)&&(spd2<0.25)) spd2=0.28;
Formatd 0:be945302062c 822 if((spd2<0)&&(spd2>-0.25)) spd2=-0.28;*/
Formatd 0:be945302062c 823
Formatd 0:be945302062c 824
Formatd 0:be945302062c 825 WRITE_MOTOR_SPEED(1, spd1);
Formatd 0:be945302062c 826 WRITE_MOTOR_SPEED(2, spd2);
Formatd 0:be945302062c 827 sprintf(textzeile1,"%.0f %.3f ",error,P);
Formatd 0:be945302062c 828 sprintf(textzeile2,"%.3f %.3f ",I,D);
Formatd 0:be945302062c 829 //sprintf(textzeile2,"%.3f %.3f ",spd1,spd2);
Formatd 0:be945302062c 830
Formatd 0:be945302062c 831 WRITE_DISPLAY(1,1,textzeile1);
Formatd 0:be945302062c 832 WRITE_DISPLAY(2,1,textzeile2);
Formatd 0:be945302062c 833
Formatd 0:be945302062c 834 }
Formatd 0:be945302062c 835 timer.stop();
Formatd 0:be945302062c 836 }
Formatd 0:be945302062c 837
Formatd 0:be945302062c 838 int main()
Formatd 0:be945302062c 839 {
Formatd 0:be945302062c 840 KEPLERBRAIN_INIT();
Formatd 0:be945302062c 841
Formatd 0:be945302062c 842 // Variablendefinition globale Variablen
Formatd 0:be945302062c 843
Formatd 0:be945302062c 844 char status = 1;
Formatd 0:be945302062c 845 char textzeile1[16];
Formatd 0:be945302062c 846 char textzeile2[16];
Formatd 0:be945302062c 847 float speed1=0;
Formatd 0:be945302062c 848 float speed2=0;
Formatd 0:be945302062c 849 float error=0;
Formatd 0:be945302062c 850 bool schleife1=true,ausgleichen2=false;
Formatd 0:be945302062c 851
Formatd 0:be945302062c 852 while(schleife1==true)
Formatd 0:be945302062c 853 {
Formatd 0:be945302062c 854 // Sensoren einlesen
Formatd 0:be945302062c 855 READ_TASTER();
Formatd 0:be945302062c 856 READ_I2C_HMC6352();
Formatd 0:be945302062c 857
Formatd 0:be945302062c 858 // Ereignisbehandlung
Formatd 0:be945302062c 859 if((BOOL_TASTER_RECHTS_PRESSED==true))
Formatd 0:be945302062c 860 {
Formatd 0:be945302062c 861 wait(2);
Formatd 0:be945302062c 862 ausgleichen2=true;
Formatd 0:be945302062c 863 ballsensor();
Formatd 0:be945302062c 864 }
Formatd 0:be945302062c 865
Formatd 0:be945302062c 866 if(!ausgleichen2)
Formatd 0:be945302062c 867 {
Formatd 0:be945302062c 868 WRITE_MOTOR_STOP(1);
Formatd 0:be945302062c 869 WRITE_MOTOR_STOP(2);
Formatd 0:be945302062c 870 }
Formatd 0:be945302062c 871 if((BOOL_TASTER_LINKS_PRESSED==true)&&(speed2<=0.9)) drehen(90);
Formatd 0:be945302062c 872
Formatd 0:be945302062c 873 //sprintf(textzeile1,"Grad: %3u ",INT_I2C_HMC6352_VALUE);
Formatd 0:be945302062c 874 //sprintf(textzeile1,"Grad: %3u ",INT_I2C_HMC6352_VALUE);
Formatd 0:be945302062c 875 //sprintf(textzeile2,"ballsens: %3u ",);
Formatd 0:be945302062c 876 //drehen(90);
Formatd 0:be945302062c 877 //gerade(0.7);
Formatd 0:be945302062c 878 //schleife=false;
Formatd 0:be945302062c 879 // Ansteuerung Aktoren
Formatd 0:be945302062c 880 WRITE_DISPLAY(1,1,textzeile1);
Formatd 0:be945302062c 881 WRITE_DISPLAY(2,1,textzeile2);
Formatd 0:be945302062c 882 }
Formatd 0:be945302062c 883 }