Format
h
Dependencies: Motordriver TextLCD mbed
Diff: main.cpp
- Revision:
- 0:be945302062c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jan 11 13:07:18 2014 +0000
@@ -0,0 +1,883 @@
+// ######################### KeplerBRAIN V3 - 08.11.2013 #########################
+// #
+// # Funktionen und deren Nutzung der KeplerBRAIN Library
+// #
+// #
+// # MOTORAUSGÄNGE
+// #
+// # Zum Ansteuern der Motoren dienen die beiden angeführten Funktionen.
+// # Die erste steuert die Geschwindigkeit zwischen 0% und 90 %, die
+// # Drehrichtung wird durch das Vorzeichen des übergebenen Dezimalwerts
+// # festgelegt. Die zweite Funktion dient zum apprupten Anhalten eines
+// # Motors ohne dass dieser nachläuft.
+// #
+// # Funktion: void WRITE_MOTOR_SPEED(int number, float speed)
+// # Parameter number: int 1, 2, 3, 4
+// # Parameter speed: float -0.9 bis 0.9 (Vorzeichen: Drehrichtung)
+// #
+// # Funktion: void WRITE_MOTOR_STOP(int number)
+// # Parameter number: ganze Zahl 1, 2, 3, 4
+// #
+// #
+// # DIGITALE SENSOREN AN DEN IO-BUCHSEN
+// #
+// # Werden an einen der 12 IO-Buchsen digitale Sensoren, also Sensoren
+// # die beim Auslösen eine Verbindung zwischen dem IO-Pin des Mikrokontrollers
+// # und GND herstellen (Taster, Schalter, ...) angeschlossen, so muss der
+// # entsprechende # IO-Pin als digitaler Eingang konfiguriert werden. Dies
+// # geschieht am Anfang der Bibliothek im Block "Declatation of IO-Ports".
+// #
+// # Festlegen eines IO-Ports 1 als digitaler Eingang: DigitalIn io1(PTB0);
+// #
+// # Das Einlesen des Sensor-Status erfolgt über die folgende Funktion, der
+// # Status des IO-Eingangs wird in einer Variable gespeichert.
+// #
+// # Funktion: void READ_IO_DIGITAL_IN(char number)
+// # Parameter number: int 1, 2, 3, ..., 12
+// # Variable: bool BOOL_IO1_DIGITAL_VALUE true oder false
+// #
+// #
+// # ANALOGE SENSOREN AN DEN IO-BUCHSEN
+// #
+// # Werden an einen der ersten 5 IO-Buchsen Sensoren, die Spannungswerte
+// # zwischen 0 V und 3.3 V liefern, angeschlossen, so muss der entsprechende
+// # IO-Pin als analoger Eingang konfiguriert werden. Dies geschieht am Anfang
+// # der Bibliothek im Block "Declatation of IO-Ports".
+// #
+// # Festlegen eines IO-Ports 1 als digitaler Eingang: AnalogIn io1(PTB0);
+// #
+// # Das Einlesen des Sensor-Status erfolgt über die folgende Funktion, der
+// # entsprechende Spannungswert wird in einer Variable abgelegt.
+// #
+// # Funktion: void READ_IO_ANALOG_IN(char number)
+// # Parameter number: int 1, 2, 3, 4, 5
+// # Variable: float FLOAT_IO1_ANALOG_VALUE 0.0 ... 1.0
+// #
+// #
+// # LEUCHTDIODEN
+// #
+// # Zum Aus- und Einschalten der Leuchtdioden werden die folgenden
+// # drei Funtkionen verwendet:
+// #
+// # Funktion: void WRITE_LED_ROT(bool wert)
+// # Funktion: void WRITE_LED_GELB(bool wert)
+// # Funktion: void WRITE_LED_GRUEN(bool wert)
+// # Parameter number: bool true oder false
+// #
+// #
+// # TASTER
+// #
+// # Zum Auswerten der beiden Taster muss zuerst eine Funktion aufgerufen
+// # werden. Im Anschluss daran kann auf das Drücken eines Tasters durch
+// # Auslesen des Werts der beiden Tastervariablen reagiert werden.
+// #
+// # Funktion: void READ_TASTER()
+// #
+// # Variablen: BOOL_TASTER_LINKS_PRESSED, BOOL_TASTER_RECHTS_PRESSED
+// # Werte: bool true oder false
+// #
+// #
+// # DISPLAY
+// #
+// # Die Ausgabe von Text auf dem Display erfolgt über die folgende Funktion,
+// # der die Position der Ausgabe und ein String übergeben werden. Der
+// # String muss in einem eigens dafür definierten char-Array vorliegen.
+// #
+// # Funktion: void WRITE_DISPLAY(char zeile, char zeichen, const char *text)
+// # Parameter zeile: char 1, 2
+// # Parameter zeichen: char 1, 2, ..., 16
+// # Parameter *text: char[]
+// #
+// # Beispiel:
+// #
+// # char MeinText[16];
+// # MeinText = "Text Anzeige 123";
+// # WRITE_DISPLAY(1,1,MeinText);
+// #
+// #
+// # I2C PYROSENSOR MLX90614
+// #
+// # Dieser Sensor wird an den I2C Bus angeschlossen und kann die
+// # Objekttemperatur aufgrund dessen Infrarotstrahlung und die Umgebungs-
+// # temperatur messen. Den beiden Funktionen muss die Adresse übergeben
+// # werden und liefern die Temperatur in Grad Celsius. Diese werden in
+// # zwei Variablen abgelegt.
+// #
+// # Funktion: char READ_I2C_MLX90614_OBJ(char address)
+// # Parameter zeile: char 0xB4, 0xB6, 0xB8, 0xBA
+// # Variable: int INT_I2C_MLX90614_0xB4_OBJ_VALUE
+// #
+// # Funktion: char READ_I2C_MLX90614_ENV(char address)
+// # Parameter zeile: char 0xB4, 0xB6, 0xB8, 0xBA
+// # Variable: int INT_I2C_MLX90614_0xB4_ENV_VALUE
+// #
+// #
+// # I2C KOMPASSSENSOR HMC6352
+// #
+// # Dieser Sensor wird an den I2C Bus angeschlossen und liefert durch
+// # den Aufruf der folgenden Funktion die Richtung bezogen auf den
+// # magnetischen Nordpol in Grad.
+// #
+// # Funktion: void READ_I2C_HMC6352()
+// # Variable: int INT_I2C_HMC6352_VALUE 0, ..., 359
+
+
+// #################### Beginn Import externe Librarys ####################
+
+#include "mbed.h"
+#include "TextLCD.h"
+#include "motordriver.h"
+
+// #################### End Import externe Librarys ####################
+
+
+// #################### Beginn Declaration of IO-Ports ####################
+
+DigitalIn io1(PTB0); // IO 1 DigitalIn, DigitalOut, AnalogIn
+AnalogIn io2(PTB1); // IO 2 DigitalIn, DigitalOut, AnalogIn
+DigitalIn io3(PTB2); // IO 3 DigitalIn, DigitalOut, AnalogIn
+DigitalIn io4(PTB3); // IO 4 DigitalIn, DigitalOut, AnalogIn
+DigitalIn io5(PTC2); // IO 5 DigitalIn, DigitalOut, AnalogIn
+DigitalIn io6(PTC10); // IO 6 DigitalIn, DigitalOut
+DigitalIn io7(PTE5); // IO 7 DigitalIn, DigitalOut
+DigitalIn io8(PTE20); // IO 8 DigitalIn, DigitalOut
+DigitalIn io9(PTE21); // IO 9 DigitalIn, DigitalOut
+DigitalIn io10(PTE30); // IO 10 DigitalIn, DigitalOut, AnalogOut // if AnalogOut REMOVE // in WRITE_IO_ANALOG_OUT
+DigitalIn io11(PTA5); // IO 11 DigitalIn, DigitalOut, PwmOut // if AnalogOut REMOVE // in WRITE_IO_PWM
+DigitalIn io12(PTA4); // IO 12 DigitalIn, DigitalOut, PwmOut // if AnalogOut REMOVE // in WRITE_IO_PWM
+
+// #################### End Declaration of IO-Ports ####################
+
+
+// #################### Begin of KeplerBRAIN V3 Library ####################
+
+// ********** KL25Z PIN-Belegungen **********
+
+// PTA20/RST Taster Reset S1
+// GND0 GND
+// VIN_5_9V 5V
+// PTB8 Taster S4 links
+// PTB9 Taster S3 rechts
+
+// PTE22 Led2 rot
+// PTE23 Led3 gelb
+// PTE29 Led1 grün
+
+// PTD7 Display RS
+// PTD6 Display E
+// PTE31 Display D4
+// PTA17 Display D5
+// PTA16 Display D6
+// PTC17 Display D7
+
+// PTD5 Motor1 IN1
+// PTA13 Motor1 IN2
+// PTC9 Motor1 EN
+
+// PTC6 Motor2 IN1
+// PTC5 Motor2 IN2
+// PTC8 Motor2 EN
+
+// PTC4 Motor3 IN1
+// PTC3 Motor3 IN2
+// PTA12 Motor3 EN
+
+// PTC7 Motor4 IN1
+// PTC0 Motor4 IN2
+// PTD4 Motor4 EN
+
+// PTB0 IO1/AIN
+// PTB1 IO2/AIN
+// PTB2 IO3/AIN
+// PTB3 IO4/AIN
+// PTC2 IO5/AIN
+// PTC10 IO6
+
+// PTE5 IO7
+// PTE20 IO8
+// PTE21 IO9
+// PTE30 IO10/AOUT
+// PTA5 IO11/PWM
+// PTA4 IO12/PWM
+
+// PTE0 I2C SCL
+// PTE1 I2C SDA
+
+// PTD1 SPI SCK
+// PTD3 SPI MISO
+// PTD2 SPI MOSI
+// PTD0 SPI SEL
+
+// PTA2 SERIAL TX
+// PTA1 SERIAL RX
+
+
+// ********** Deklaration Objekte Variablen intern **********
+
+I2C i2c(PTE0, PTE1); // I2C Objekt
+
+TextLCD display(PTD7, PTD6, PTE31, PTA17, PTA16, PTC17, TextLCD::LCD16x2); // Display RS, EN, D4, D5, D6, D7, Type
+
+Motor motor1(PTC9, PTD5, PTA13, 1); // Motor 1 pwm, fwd, rev, can brake
+Motor motor2(PTC8, PTC6, PTC5, 1); // Motor 2 pwm, fwd, rev, can brake
+Motor motor3(PTA12, PTC4, PTC3, 1); // Motor 3 pwm, fwd, rev, can brake
+Motor motor4(PTD4, PTC7, PTC0, 1); // Motor 4 pwm, fwd, rev, can brake
+
+bool bool_motor1_coast, bool_motor2_coast, bool_motor3_coast, bool_motor4_coast; // Motor wartet nach stop auf coast
+
+Timeout wait_motor1_coast; // Timeout zwischen Motor stop und Motor coast
+Timeout wait_motor2_coast; // Timeout zwischen Motor stop und Motor coast
+Timeout wait_motor3_coast; // Timeout zwischen Motor stop und Motor coast
+Timeout wait_motor4_coast; // Timeout zwischen Motor stop und Motor coast
+
+DigitalOut led_rot(PTE22); // Led rot
+DigitalOut led_gelb(PTE23); // Led gelb
+DigitalOut led_gruen(PTE29); // Led grün
+
+DigitalIn taster_links(PTB8); // Taster links
+DigitalIn taster_rechts(PTB9); // Taster links
+
+bool bool_taster_links_down;
+bool bool_taster_rechts_down;
+
+// ********** Deklaration Funktionen intern **********
+
+void motor1_coast()
+{
+ if (bool_motor1_coast) motor1.coast();
+}
+
+void motor2_coast()
+{
+ if (bool_motor2_coast) motor2.coast();
+}
+
+void motor3_coast()
+{
+ if (bool_motor3_coast) motor3.coast();
+}
+
+void motor4_coast()
+{
+ if (bool_motor4_coast) motor4.coast();
+}
+
+
+// ********** Deklaration Variablen Library **********
+
+bool BOOL_TASTER_LINKS_PRESSED, BOOL_TASTER_RECHTS_PRESSED;
+bool BOOL_IO1_DIGITAL_VALUE, BOOL_IO2_DIGITAL_VALUE, BOOL_IO3_DIGITAL_VALUE, BOOL_IO4_DIGITAL_VALUE,
+ BOOL_IO5_DIGITAL_VALUE, BOOL_IO6_DIGITAL_VALUE, BOOL_IO7_DIGITAL_VALUE, BOOL_IO8_DIGITAL_VALUE,
+ BOOL_IO9_DIGITAL_VALUE, BOOL_IO10_DIGITAL_VALUE, BOOL_IO11_DIGITAL_VALUE, BOOL_IO12_DIGITAL_VALUE;
+float BOOL_IO1_ANALOG_VALUE, BOOL_IO2_ANALOG_VALUE, BOOL_IO3_ANALOG_VALUE, BOOL_IO4_ANALOG_VALUE, BOOL_IO5_ANALOG_VALUE;
+int INT_I2C_MLX90614_0xB4_OBJ_VALUE, INT_I2C_MLX90614_0xB4_ENV_VALUE;
+int INT_I2C_MLX90614_0xB6_OBJ_VALUE, INT_I2C_MLX90614_0xB6_ENV_VALUE;
+int INT_I2C_HMC6352_VALUE;
+char CHAR_I2C_LINE_S1_VALUE,CHAR_I2C_LINE_S2_VALUE,CHAR_I2C_LINE_S3_VALUE,CHAR_I2C_LINE_S4_VALUE,
+ CHAR_I2C_LINE_S5_VALUE,CHAR_I2C_LINE_S6_VALUE,CHAR_I2C_LINE_S7_VALUE,CHAR_I2C_LINE_S8_VALUE;
+char CHAR_IRSEEKER2_DIRECTION, CHAR_IRSEEKER2_S1_VALUE, CHAR_IRSEEKER2_S2_VALUE, CHAR_IRSEEKER2_S3_VALUE,
+ CHAR_IRSEEKER2_S4_VALUE, CHAR_IRSEEKER2_S5_VALUE;
+
+// ********** Deklaration Funktionen Library **********
+
+void KEPLERBRAIN_INIT()
+{
+ bool_taster_links_down = false;
+ bool_taster_rechts_down = false;
+
+ BOOL_TASTER_LINKS_PRESSED = false;
+ BOOL_TASTER_RECHTS_PRESSED = false;
+
+ BOOL_IO1_DIGITAL_VALUE = false;
+ BOOL_IO2_DIGITAL_VALUE = false;
+ BOOL_IO3_DIGITAL_VALUE = false;
+ BOOL_IO4_DIGITAL_VALUE = false;
+ BOOL_IO5_DIGITAL_VALUE = false;
+ BOOL_IO6_DIGITAL_VALUE = false;
+ BOOL_IO7_DIGITAL_VALUE = false;
+ BOOL_IO8_DIGITAL_VALUE = false;
+ BOOL_IO9_DIGITAL_VALUE = false;
+ BOOL_IO10_DIGITAL_VALUE = false;
+ BOOL_IO11_DIGITAL_VALUE = false;
+ BOOL_IO12_DIGITAL_VALUE = false;
+
+ BOOL_IO1_ANALOG_VALUE = 0.0;
+ BOOL_IO2_ANALOG_VALUE = 0.0;
+ BOOL_IO3_ANALOG_VALUE = 0.0;
+ BOOL_IO4_ANALOG_VALUE = 0.0;
+ BOOL_IO5_ANALOG_VALUE = 0.0;
+
+ CHAR_IRSEEKER2_DIRECTION = 0;
+ CHAR_IRSEEKER2_S1_VALUE = 0;
+ CHAR_IRSEEKER2_S2_VALUE = 0;
+ CHAR_IRSEEKER2_S3_VALUE = 0;
+ CHAR_IRSEEKER2_S4_VALUE = 0;
+ CHAR_IRSEEKER2_S5_VALUE = 0;
+
+ INT_I2C_MLX90614_0xB4_OBJ_VALUE = 0;
+ INT_I2C_MLX90614_0xB4_ENV_VALUE = 0;
+ INT_I2C_MLX90614_0xB6_OBJ_VALUE = 0;
+ INT_I2C_MLX90614_0xB6_ENV_VALUE = 0;
+
+ INT_I2C_HMC6352_VALUE = 0;
+
+ // Initialisierung Motoren
+
+ bool_motor1_coast = false;
+ bool_motor2_coast = false;
+ bool_motor3_coast = false;
+ bool_motor4_coast = false;
+}
+
+
+void READ_TASTER()
+{
+ BOOL_TASTER_LINKS_PRESSED = false;
+ if (!taster_links)
+ {
+ if (bool_taster_links_down==false)
+ {
+ BOOL_TASTER_LINKS_PRESSED = true;
+ }
+ bool_taster_links_down = true;
+ }
+ else
+ {
+ bool_taster_links_down = false;
+ }
+
+ BOOL_TASTER_RECHTS_PRESSED = false;
+ if (!taster_rechts)
+ {
+ if (bool_taster_rechts_down==false)
+ {
+ BOOL_TASTER_RECHTS_PRESSED = true;
+ }
+ bool_taster_rechts_down = true;
+ }
+ else
+ {
+ bool_taster_rechts_down = false;
+ }
+}
+
+
+void READ_IO_DIGITAL_IN(char number)
+{
+ switch(number)
+ {
+ case 1:
+ if (!io1) BOOL_IO1_DIGITAL_VALUE=true; else BOOL_IO1_DIGITAL_VALUE=false;
+ break;
+ case 2:
+ if (!io2) BOOL_IO2_DIGITAL_VALUE=true; else BOOL_IO2_DIGITAL_VALUE=false;
+ break;
+ case 3:
+ if (!io3) BOOL_IO3_DIGITAL_VALUE=true; else BOOL_IO3_DIGITAL_VALUE=false;
+ break;
+ case 4:
+ if (!io4) BOOL_IO4_DIGITAL_VALUE=true; else BOOL_IO4_DIGITAL_VALUE=false;
+ break;
+ case 5:
+ if (!io5) BOOL_IO5_DIGITAL_VALUE=true; else BOOL_IO5_DIGITAL_VALUE=false;
+ break;
+ case 6:
+ if (!io6) BOOL_IO6_DIGITAL_VALUE=true; else BOOL_IO6_DIGITAL_VALUE=false;
+ break;
+ case 7:
+ if (!io7) BOOL_IO7_DIGITAL_VALUE=true; else BOOL_IO7_DIGITAL_VALUE=false;
+ break;
+ case 8:
+ if (!io8) BOOL_IO8_DIGITAL_VALUE=true; else BOOL_IO8_DIGITAL_VALUE=false;
+ break;
+ case 9:
+ if (!io9) BOOL_IO9_DIGITAL_VALUE=true; else BOOL_IO9_DIGITAL_VALUE=false;
+ break;
+ case 10:
+ if (!io10) BOOL_IO10_DIGITAL_VALUE=true; else BOOL_IO10_DIGITAL_VALUE=false;
+ break;
+ case 11:
+ if (!io11) BOOL_IO11_DIGITAL_VALUE=true; else BOOL_IO11_DIGITAL_VALUE=false;
+ break;
+ case 12:
+ if (!io12) BOOL_IO12_DIGITAL_VALUE=true; else BOOL_IO12_DIGITAL_VALUE=false;
+ break;
+ }
+}
+
+
+void READ_IO_ANALOG_IN(char number)
+{
+ switch(number)
+ {
+ case 1:
+ BOOL_IO1_ANALOG_VALUE=io1;
+ break;
+ case 2:
+ BOOL_IO2_ANALOG_VALUE=io2;
+ break;
+ case 3:
+ BOOL_IO3_ANALOG_VALUE=io3;
+ break;
+ case 4:
+ BOOL_IO4_ANALOG_VALUE=io4;
+ break;
+ case 5:
+ BOOL_IO5_ANALOG_VALUE=io5;
+ break;
+ }
+}
+
+
+void READ_I2C_MLX90614(char address)
+{
+ char cmd[1];
+ char bytes_env[3];
+ char bytes_obj[3];
+ int i2c_Addr = address;
+
+ cmd[0] = 0x06;
+ i2c.write(i2c_Addr, cmd, 1);
+ i2c.read(i2c_Addr, bytes_env, 3);
+
+ cmd[0] = 0x07;
+ i2c.write(i2c_Addr, cmd, 1);
+ i2c.read(i2c_Addr, bytes_obj, 3);
+
+ switch(address)
+ {
+ case 0xB4:
+ INT_I2C_MLX90614_0xB4_ENV_VALUE = (((bytes_env[0] & 0x007F) << 8) + bytes_env[1]);
+ INT_I2C_MLX90614_0xB4_OBJ_VALUE = (((bytes_obj[0] & 0x007F) << 8) + bytes_obj[1]);
+ break;
+ case 0xB6:
+ INT_I2C_MLX90614_0xB6_ENV_VALUE = (((bytes_env[0] & 0x007F) << 8) + bytes_env[1]);
+ INT_I2C_MLX90614_0xB6_OBJ_VALUE = (((bytes_obj[0] & 0x007F) << 8) + bytes_obj[1]);
+ INT_I2C_MLX90614_0xB6_ENV_VALUE = bytes_env[0];
+ INT_I2C_MLX90614_0xB6_OBJ_VALUE = bytes_obj[0];
+ break;
+ }
+}
+
+void READ_I2C_HMC6352()
+{
+ char cmd[1];
+ char bytes[2];
+ int i2c_Addr = 0x42;
+
+ cmd[0] = 0x41;
+ i2c.write(i2c_Addr, cmd, 1);
+ i2c.read(i2c_Addr, bytes, 2);
+
+ INT_I2C_HMC6352_VALUE = (bytes[0] * 256 + bytes[1]) / 10;
+}
+
+
+void READ_I2C_LINE()
+{
+ char cmd[1];
+ char bytes[8];
+ int i2c_Addr = 0xA0;
+
+ cmd[0] = 0x49;
+ i2c.write(i2c_Addr, cmd, 1);
+ i2c.read(i2c_Addr, bytes, 8);
+
+ CHAR_I2C_LINE_S1_VALUE = bytes[0];
+ CHAR_I2C_LINE_S2_VALUE = bytes[1];
+ CHAR_I2C_LINE_S3_VALUE = bytes[2];
+ CHAR_I2C_LINE_S4_VALUE = bytes[3];
+ CHAR_I2C_LINE_S5_VALUE = bytes[4];
+ CHAR_I2C_LINE_S6_VALUE = bytes[5];
+ CHAR_I2C_LINE_S7_VALUE = bytes[6];
+ CHAR_I2C_LINE_S8_VALUE = bytes[7];
+
+
+}
+
+
+void WRITE_IO_ANALOG_OUT(char number, float value)
+{
+ if (value<0.0) value = 0.0;
+ if (value>1.0) value = 1.0;
+ switch(number)
+ {
+ case 10:
+ // io10.write(value);
+ break;
+ }
+}
+
+
+void WRITE_IO_PWM(char number, float period, float pulsewidth)
+{
+ switch(number)
+ {
+ case 11:
+ // io11.period = period;
+ // io11.pulsewidth = pulsewidth;
+ break;
+ case 12:
+ // io12.period = period;
+ // io12.pulsewidth = pulsewidth;
+ break;
+ }
+}
+
+
+void WRITE_LED_ROT(bool wert)
+{
+ if (wert==true) led_rot = 1; else led_rot = 0;
+}
+
+
+void WRITE_LED_GELB(bool wert)
+{
+ if (wert==true) led_gelb = 1; else led_gelb = 0;
+}
+
+
+void WRITE_LED_GRUEN(bool wert)
+{
+ if (wert==true) led_gruen = 1; else led_gruen = 0;
+}
+
+
+void WRITE_MOTOR_SPEED(int number, float speed)
+{
+ if (speed>0.9) speed = 0.9;
+ if (speed<-0.9) speed = -0.9;
+
+ switch(number)
+ {
+ case 1:
+ bool_motor1_coast = false;
+ motor1.speed(speed);
+ break;
+ case 2:
+ bool_motor2_coast = false;
+ motor2.speed(speed);
+ break;
+ case 3:
+ bool_motor3_coast = false;
+ motor3.speed(speed);
+ break;
+ case 4:
+ bool_motor4_coast = false;
+ motor4.speed(speed);
+ break;
+ }
+}
+
+
+void WRITE_MOTOR_STOP(int number)
+{
+ switch(number)
+ {
+ case 1:
+ motor1.stop(1.0);
+ bool_motor1_coast = true;
+ wait_motor1_coast.attach(&motor1_coast, 1);
+ break;
+ case 2:
+ motor2.stop(1.0);
+ bool_motor2_coast = true;
+ wait_motor2_coast.attach(&motor2_coast, 1);
+ break;
+ case 3:
+ motor3.stop(1.0);
+ bool_motor3_coast = true;
+ wait_motor3_coast.attach(&motor3_coast, 1);
+ break;
+ case 4:
+ motor4.stop(1.0);
+ bool_motor4_coast = true;
+ wait_motor4_coast.attach(&motor4_coast, 1);
+ break;
+ }
+}
+
+
+void WRITE_DISPLAY(char zeile, char zeichen, const char *text)
+{
+ if (zeile<1) zeile = 1;
+ if (zeile>2) zeile = 2;
+ if (zeichen<1) zeichen = 1;
+ if (zeichen>16) zeichen = 16;
+ zeile--;
+ zeichen--;
+
+ display.locate(zeichen, zeile);
+ display.printf(text);
+}
+
+
+void READ_I2C_IRSEEKER2()
+{
+ char cmd[1];
+ char bytes[6];
+ int i2c_Addr = 0x10;
+
+ cmd[0] = 0x49;
+ i2c.write(i2c_Addr, cmd, 1);
+ i2c.read(i2c_Addr, bytes, 6);
+
+ CHAR_IRSEEKER2_DIRECTION = bytes[0];
+ CHAR_IRSEEKER2_S1_VALUE = bytes[1];
+ CHAR_IRSEEKER2_S2_VALUE = bytes[2];
+ CHAR_IRSEEKER2_S3_VALUE = bytes[3];
+ CHAR_IRSEEKER2_S4_VALUE = bytes[4];
+ CHAR_IRSEEKER2_S5_VALUE = bytes[5];
+
+}
+
+
+// #################### End of KeplerBRAIN V3 Library ####################
+
+
+
+void fahren(float spd1, float spd2)
+{
+ spd2+=0.05;
+ WRITE_MOTOR_SPEED(1, spd1);
+ WRITE_MOTOR_SPEED(2, spd2);
+}
+
+void gerade(float time)
+{
+ fahren(0.6, 0.65);
+ wait(time);
+ WRITE_MOTOR_STOP(1);
+ WRITE_MOTOR_STOP(2);
+}
+
+void drehen(int grad) // bool für richtung noch einfügen
+{
+ bool a=true;
+ int pos=INT_I2C_HMC6352_VALUE;
+ char textzeile1[16];
+ grad+=pos;
+ if(grad>=360)
+ {
+ grad-=360;
+ }
+ while(a)
+ {
+ fahren(0.25,-0.3);
+ READ_I2C_HMC6352();
+ pos=INT_I2C_HMC6352_VALUE;
+ pos-=360;
+ if (pos>=grad)
+ {
+ WRITE_MOTOR_STOP(1);
+ WRITE_MOTOR_STOP(2);
+ a=false;
+ }
+ sprintf(textzeile1,"Grad: %3u ",INT_I2C_HMC6352_VALUE);
+ WRITE_DISPLAY(1,1,textzeile1);
+ WRITE_MOTOR_STOP(1);
+ WRITE_MOTOR_STOP(2);
+ wait(0.01);
+ }
+ WRITE_MOTOR_STOP(1);
+ WRITE_MOTOR_STOP(2);
+
+}
+
+Timer timer;
+
+void ausgleichen()
+{
+ int pos=INT_I2C_HMC6352_VALUE;
+ float error=0;
+ int Lasterror=0;
+ int beginn=0,end=0;
+ float P=0;
+ float I=0;
+ float D=0;
+ float Kp=0.015;
+ float Ki=0.00002/30;
+ float Kd=1;
+ float spd1=0;
+ float spd2=0;
+ float minspeed=0.28;
+ float speed=0.6;
+
+ char textzeile1[16];
+ char textzeile2[16];
+ bool speederror=false;
+ int kompass=0;
+ int kompassNull = INT_I2C_HMC6352_VALUE;
+
+ while(1)
+ {
+ READ_TASTER();
+ timer.start();
+
+ beginn=timer.read_us()/100;
+
+ READ_I2C_HMC6352();
+
+ end=timer.read_us()/100;
+
+ kompass = INT_I2C_HMC6352_VALUE + 180 - kompassNull;
+ if(kompass<0) kompass+=360;
+ if(kompass>359) kompass+=-360;
+
+ error=180-kompass;//INT_I2C_HMC6352_VALUE;//180-kompass;
+
+ P=Kp*error;
+ I=I+((end-beginn)*error)*Ki;
+ D=Kd*((error-Lasterror)/(end-beginn));
+
+ Lasterror=error;
+ if(error==0) I=0;
+
+ /* if((spd1<minspeed)&&(spd1>0)) spd1=minspeed;
+ if((spd1>-minspeed)&&(spd1<0)) spd1=-minspeed;
+ if((spd2<minspeed)&&(spd2>0)) spd2=minspeed;
+ if((spd2>-minspeed)&&(spd2<0)) spd2=-minspeed;*/
+
+ spd1=speed+(P+I+D);
+ spd2=speed-(P+I+D);
+
+ /*
+ if((spd1>0)&&(spd1<0.25)) spd1=0.28;
+ if((spd1<0)&&(spd1>-0.25)) spd1=-0.28;
+
+ if((spd2>0)&&(spd2<0.25)) spd2=0.28;
+ if((spd2<0)&&(spd2>-0.25)) spd2=-0.28;*/
+
+
+ WRITE_MOTOR_SPEED(1, spd1);
+ WRITE_MOTOR_SPEED(2, spd2);
+ sprintf(textzeile1,"%.3f %3u ",error,kompass);
+ sprintf(textzeile2,"%.3f %.3f ",spd1,spd2);
+
+ WRITE_DISPLAY(1,1,textzeile1);
+ WRITE_DISPLAY(2,1,textzeile2);
+
+ }
+ timer.stop();
+}
+
+void ballsensor()
+{
+ float error=0;
+ int Lasterror=0;
+ int beginn=0,end=0;
+ float P=0;
+ float I=0;
+ float D=0;
+ float Kp=0.25;
+ float Ki=0.00002/15;
+ float Kd=1;
+ float spd1=0;
+ float spd2=0;
+ float minspeed=0.28;
+ float speed=0;
+
+ char textzeile1[16];
+ char textzeile2[16];
+ bool speederror=false;
+ int ballsensor=0;
+
+ while(1)
+ {
+ READ_TASTER();
+ timer.start();
+
+ beginn=timer.read_us()/100;
+
+ READ_I2C_HMC6352();
+ READ_I2C_IRSEEKER2();
+
+ end=timer.read_us()/100;
+
+ ballsensor = CHAR_IRSEEKER2_DIRECTION;//ballsensor = INT_I2C_HMC6352_VALUE + 180 - kompassNull;
+
+
+ error=(ballsensor-4);//INT_I2C_HMC6352_VALUE;//180-kompass;
+
+ P=Kp*error;
+ I=I+((end-beginn)*error)*Ki;
+ D=Kd*((error-Lasterror)/(end-beginn));
+
+ Lasterror=error;
+ if(error==0) I=0;
+
+ /* if((spd1<minspeed)&&(spd1>0)) spd1=minspeed;
+ if((spd1>-minspeed)&&(spd1<0)) spd1=-minspeed;
+ if((spd2<minspeed)&&(spd2>0)) spd2=minspeed;
+ if((spd2>-minspeed)&&(spd2<0)) spd2=-minspeed;*/
+
+ spd1=speed+(P+I+D);
+ spd2=speed-(P+I+D);
+
+ /*
+ if((spd1>0)&&(spd1<0.25)) spd1=0.28;
+ if((spd1<0)&&(spd1>-0.25)) spd1=-0.28;
+
+ if((spd2>0)&&(spd2<0.25)) spd2=0.28;
+ if((spd2<0)&&(spd2>-0.25)) spd2=-0.28;*/
+
+
+ WRITE_MOTOR_SPEED(1, spd1);
+ WRITE_MOTOR_SPEED(2, spd2);
+ sprintf(textzeile1,"%.0f %.3f ",error,P);
+ sprintf(textzeile2,"%.3f %.3f ",I,D);
+ //sprintf(textzeile2,"%.3f %.3f ",spd1,spd2);
+
+ WRITE_DISPLAY(1,1,textzeile1);
+ WRITE_DISPLAY(2,1,textzeile2);
+
+ }
+ timer.stop();
+}
+
+int main()
+{
+ KEPLERBRAIN_INIT();
+
+ // Variablendefinition globale Variablen
+
+ char status = 1;
+ char textzeile1[16];
+ char textzeile2[16];
+ float speed1=0;
+ float speed2=0;
+ float error=0;
+ bool schleife1=true,ausgleichen2=false;
+
+ while(schleife1==true)
+ {
+ // Sensoren einlesen
+ READ_TASTER();
+ READ_I2C_HMC6352();
+
+ // Ereignisbehandlung
+ if((BOOL_TASTER_RECHTS_PRESSED==true))
+ {
+ wait(2);
+ ausgleichen2=true;
+ ballsensor();
+ }
+
+ if(!ausgleichen2)
+ {
+ WRITE_MOTOR_STOP(1);
+ WRITE_MOTOR_STOP(2);
+ }
+ if((BOOL_TASTER_LINKS_PRESSED==true)&&(speed2<=0.9)) drehen(90);
+
+ //sprintf(textzeile1,"Grad: %3u ",INT_I2C_HMC6352_VALUE);
+ //sprintf(textzeile1,"Grad: %3u ",INT_I2C_HMC6352_VALUE);
+ //sprintf(textzeile2,"ballsens: %3u ",);
+ //drehen(90);
+ //gerade(0.7);
+ //schleife=false;
+ // Ansteuerung Aktoren
+ WRITE_DISPLAY(1,1,textzeile1);
+ WRITE_DISPLAY(2,1,textzeile2);
+ }
+}