Revision 0:be945302062c, committed 2014-01-11

Comitter:

Formatd

Date:

Sat Jan 11 13:07:18 2014 +0000

Commit message:

h

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motordriver.lib	Sat Jan 11 13:07:18 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/littlexc/code/Motordriver/#3110b9209d3c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Sat Jan 11 13:07:18 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/TextLCD/#44f34c09bd37

--- /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);           
+  }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.lib	Sat Jan 11 13:07:18 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/#673126e12c73