Format
h
Dependencies: Motordriver TextLCD mbed
main.cpp
- Committer:
- Formatd
- Date:
- 2014-01-11
- Revision:
- 0:be945302062c
File content as of revision 0:be945302062c:
// ######################### 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);
}
}