h
Dependencies: Motordriver TextLCD mbed
Diff: main.cpp
- Revision:
- 0:be945302062c
diff -r 000000000000 -r be945302062c main.cpp --- /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); + } +}