0,0 → 1,427 |
/*############################################################################ |
############################################################################*/ |
|
#include "main.h" |
#include "kafi.h" |
|
unsigned char twi_state = 0; |
unsigned char motor = 0; |
unsigned char motorread = 0; |
unsigned char motor_rx[8]; |
|
unsigned int TriggerMagneticHeading = 0; |
unsigned int TriggerSonicReading = 0; |
|
|
//############################################################################ |
//Initzialisieren der I2C (TWI) Schnittstelle |
void i2c_init(void) |
//############################################################################ |
{ |
TWSR = 0; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
} |
|
//############################################################################ |
//Start I2C |
char i2c_start(void) |
//############################################################################ |
{ |
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
return(0); |
} |
|
//############################################################################ |
//Start I2C |
void i2c_stop(void) |
//############################################################################ |
{ |
TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT); |
} |
|
void i2c_reset(void) |
//############################################################################ |
{ |
i2c_stop(); |
twi_state = 0; |
motor = TWDR; |
motor = 0; |
TWCR = 0x80; |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
i2c_init(); |
i2c_start(); |
i2c_write_byte(0); |
} |
|
//############################################################################ |
//Start I2C |
char i2c_write_byte(char byte) |
//############################################################################ |
{ |
TWSR = 0x00; |
TWDR = byte; |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
|
return(0); |
} |
|
//############################################################################ |
//Start I2C |
SIGNAL (TWI_vect) |
//############################################################################ |
{ |
#if 0 |
int motorwert = 0; |
int scale_p = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 4.F) / 6; |
int scale_d = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 4.F) / 6; |
#endif |
|
switch (twi_state++) |
{ |
case 0: |
i2c_write_byte(0x52+(motor*2)); |
break; |
case 1: |
switch(motor++) |
{ |
case 0: |
#if 0 |
pd_ergebnis = (int) (scale_p* DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10; |
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
} |
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil)); |
} |
/* M o t o r V o r n */ |
motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; |
if ((motorwert < 0)) |
{ |
motorwert = 0; |
} |
else if(motorwert > MAX_GAS) |
{ |
motorwert = MAX_GAS; |
} |
if (motorwert < MIN_GAS) |
{ |
motorwert = MIN_GAS; |
} |
Motor_Vorne = motorwert; |
if(!MotorenEin) |
{ |
Motor_Vorne = 0; |
if(MotorTest[0]) Motor_Vorne = MotorTest[0]; |
} |
#endif |
i2c_write_byte(Motor_Vorne); |
break; |
case 1: |
#if 0 |
pd_ergebnis = (scale_p * DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10; |
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
} |
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil)); |
} |
/* M o t o r H e c k */ |
motorwert = GasMischanteil - pd_ergebnis + GierMischanteil; |
if ((motorwert < 0)) |
{ |
motorwert = 0; |
} |
else if(motorwert > MAX_GAS) |
{ |
motorwert = MAX_GAS; |
} |
if (motorwert < MIN_GAS) |
{ |
motorwert = MIN_GAS; |
} |
Motor_Hinten = motorwert; |
|
if(!MotorenEin) |
{ |
Motor_Hinten = 0; |
if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
} |
#endif |
i2c_write_byte(Motor_Hinten); |
break; |
case 2: |
#if 0 |
pd_ergebnis = (scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10; |
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
} |
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil)); |
} |
/* M o t o r R e c h t s */ |
motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; |
if (motorwert < 0) |
{ |
motorwert = 0; |
} |
else if(motorwert > MAX_GAS) |
{ |
motorwert = MAX_GAS; |
} |
if (motorwert < MIN_GAS) |
{ |
motorwert = MIN_GAS; |
} |
Motor_Rechts = motorwert; |
if(!MotorenEin) |
{ |
Motor_Rechts = 0; |
if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
} |
#endif |
i2c_write_byte(Motor_Rechts); |
break; |
case 3: |
#if 0 |
pd_ergebnis = (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10; |
if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
} |
if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil))) |
{ |
pd_ergebnis = -(GasMischanteil + abs(GierMischanteil)); |
} |
/* M o t o r L i n k s */ |
motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
if (motorwert < 0) |
{ |
motorwert = 0; |
} |
else if(motorwert > MAX_GAS) |
{ |
motorwert = MAX_GAS; |
} |
if (motorwert < MIN_GAS) |
{ |
motorwert = MIN_GAS; |
} |
Motor_Links = motorwert; |
if(!MotorenEin) |
{ |
Motor_Links = 0; |
if(MotorTest[2]) Motor_Links = MotorTest[2]; |
} |
#endif |
i2c_write_byte(Motor_Links); |
break; |
} |
break; |
case 2: |
i2c_stop(); |
if (motor<4) twi_state = 0; |
else motor = 0; |
i2c_start(); |
break; |
|
//Liest Daten von Motor |
case 3: |
i2c_write_byte(0x53+(motorread*2)); |
break; |
case 4: |
switch(motorread) |
{ |
case 0: |
i2c_write_byte(Motor_Vorne); |
break; |
case 1: |
i2c_write_byte(Motor_Hinten); |
break; |
case 2: |
i2c_write_byte(Motor_Rechts); |
break; |
case 3: |
i2c_write_byte(Motor_Links); |
break; |
} |
break; |
case 5: //1 Byte vom Motor lesen |
motor_rx[motorread] = TWDR; |
|
case 6: |
switch(motorread) |
{ |
case 0: |
i2c_write_byte(Motor_Vorne); |
break; |
case 1: |
i2c_write_byte(Motor_Hinten); |
break; |
case 2: |
i2c_write_byte(Motor_Rechts); |
break; |
case 3: |
i2c_write_byte(Motor_Links); |
break; |
} |
break; |
case 7: //2 Byte vom Motor lesen |
motor_rx[motorread+4] = TWDR; |
motorread++; |
if (motorread>3) motorread=0; |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
} |
#if 0 |
break; |
TriggerMagneticHeading++; |
TriggerMagneticHeading %= 50; |
TriggerSonicReading++; |
TriggerSonicReading %= 200; |
|
if (TriggerMagneticHeading == 1) |
{ |
// Get Magnetic Heading |
i2c_start(); |
twi_state = 8; |
break; |
} |
|
if (TriggerSonicReading == 1) |
{ |
// Get Ultrasonic Reading |
i2c_start(); |
twi_state = 20; |
break; |
} |
else if (TriggerSonicReading == 190) |
{ |
// Get Ultrasonic Reading |
i2c_start(); |
twi_state = 30; |
break; |
} |
else |
{ |
I2CTimeout = 10; |
twi_state = 0; |
break; |
} |
|
case 8: |
i2c_write_byte(0xC0); |
break; |
case 9: |
i2c_write_byte(0x02); |
break; |
case 10: |
i2c_start(); |
break; |
case 11: |
i2c_write_byte(0xC1); |
break; |
case 12: |
i2c_write_byte(MagneticHeading); |
break; |
case 13: |
MagneticHeading = (0x0F & TWDR) << 8; |
i2c_stop(); |
i2c_start(); |
break; |
case 14: |
i2c_write_byte(0xC0); |
break; |
case 15: |
i2c_write_byte(0x03); |
break; |
case 16: |
i2c_start(); |
break; |
case 17: |
i2c_write_byte(0xC1); |
break; |
case 18: |
i2c_write_byte(MagneticHeading); |
break; |
case 19: |
MagneticHeading += TWDR; |
i2c_stop(); |
I2CTimeout = 22; |
twi_state = 0; |
break; |
|
/* Trigger Ultrasonic Ping */ |
case 20: /* Ping */ |
i2c_write_byte(0xE0); /* Send I2C Adress */ |
break; |
case 21: |
i2c_write_byte(0x00); /* Send I2C Register */ |
break; |
case 22: |
i2c_write_byte(82); /* Send I2C Value */ |
break; |
case 24: |
VersionID = TWDR; |
i2c_stop(); |
I2CTimeout = 22; |
twi_state = 0; |
UltrasonicPingCnt++; |
break; |
case 30: |
i2c_write_byte(0xE0);/* Send I2C Adress */ |
break; |
case 31: |
i2c_write_byte(0x02); /* Send I2C Register */ |
break; |
case 32: |
i2c_start(); |
break; |
case 33: |
i2c_write_byte(0xE1); |
break; |
case 34: |
i2c_write_byte(255); |
break; |
case 35: |
UltrasonicRange = TWDR << 8;/* Read I2C Value */ |
UltrasonicRangeHigh = TWDR; |
i2c_stop(); |
i2c_start(); |
break; |
case 36: |
i2c_write_byte(0xE0); |
break; |
case 37: |
i2c_write_byte(0x03); |
break; |
case 38: |
i2c_start(); |
break; |
case 39: |
i2c_write_byte(0xE1); |
break; |
case 40: |
i2c_write_byte(UltrasonicRangeLow); |
break; |
case 41: |
UltrasonicRange += TWDR; |
UltrasonicRangeLow = TWDR; |
i2c_stop(); |
I2CTimeout = 22; |
twi_state = 0; |
break; |
} |
#endif |
TWCR |= 0x80; |
} |