/**
* Dreht das Auto auf einen vorgegeben Winkel bezogen auf das WKs
*/
void turn_to_angle(uint16_t aimangle, uint16_t ownangle){
int8_t phase, position, turndirection;
int16_t help1;
//Bestimmen in welchen Abschnitt das Auto schaut Intervallbreite = 8191,
phase = orientation_inWK(ownangle);
position = orientation_inWK(aimangle);
if (position > phase) turndirection = 1;
else turndirection = 0;
help1 = position-phase;
if (abs8(help1) > 4){ //Für die Fälle wenn "Überlauf" im Kreis
position = 4 - abs8(help1) + 4;
if (help1 > 0) help1 = position*(-1);
else help1 = position;
}
if (help1 >= 0 && help1 <=4 ) position = -1;//drehe links rum
else position = 1;//drehe rechts rum
if (abs16(help1) > 1){ //wenn differenz kleiner ist nur eine drehung (oder keine) notwendig
help1 = abs16(help1);
while (help1 >= 0){ //drehe so lange bis im richtigen Abschnitt
turn12(1,position,1);
help1--;
}
}
}
/**
* Dreht das Auto so lange bis es in einem Zug (ohne zu rangieren) zum Ziel fahren kann
* Input: x,y-Koordinaten des Ziels, x,y-Koordinaten des Autos, Orientierung des Autos
*/
void look_in_direction(int16_t zielx, int16_t ziely, int16_t ownx, int16_t owny, uint16_t orientierung){
//Einteilung des Einheitskreises in 8 Abschnitte -> in welchem Abschnitt befindet sich das Ziel?
int16_t absolutx, absoluty, abschnitt, help1;
int8_t position;
absolutx = zielx-ownx;
absoluty = ziely-owny;
// Seg_Hex(0x0F);
if (abs32(absolutx) > abs32(absoluty)) help1 = 1;
else help1 = 0;
// Bestimmen in welchem Abschnitt sich der Zielpunkt befindet. 8 Abschnitte
if (absolutx > 0)
if (absoluty > 0)
if (help1)position = 1;
else position = 2;
else
if (help1) position = 8;
else position = 7;
else
if (absoluty > 0)
if (help1) position = 4;
else position = 3;
else
if (help1) position = 5;
else position = 6;
//Bestimmen in welchen Abschnitt das Auto schaut Intervallbreite = 8191,
if (orientierung >= 0 && orientierung <= 8191) abschnitt = 3;
else if (orientierung <= 16382) abschnitt = 4;
else if (orientierung <= 24573) abschnitt = 5;
else if (orientierung <= 32764) abschnitt = 6;
else if (orientierung <= 40955) abschnitt = 7;
else if (orientierung <= 49146) abschnitt = 8;
else if (orientierung <= 57337) abschnitt = 1;
else if (orientierung <= 65528) abschnitt = 2;
if (abschnitt == 0) return;
help1 = position-abschnitt;
if (abs8(help1) > 4){ //Für die Fälle wenn "Überlauf" im Kreis
position = 4 - abs8(help1) + 4;
if (help1 > 0) help1 = position*(-1);
else help1 = position;
}
if (help1 >= 0 && help1 <=4 ) position = -1;//drehe links rum
else position = 1;//drehe rechts rum
if (abs16(help1) > 1){ //wenn differenz kleiner ist nur eine drehung (oder keine) notwendig
help1 = abs16(help1);
while (help1 >= 0){ //drehe so lange bis im richtigen Abschnitt
turn12(1,position,1);
help1--;
}
}
}
int AmpFollower::peakEnvelope(int inL, int inR)
{
int amp = abs16(inL);
if(amp < 40)
{
return 0;
}
return peakDetector1.process(amp);
}
void indicateClipping(int16_t in, int16_t out) {
static int clipCounter = 0;
if (clipCounter != 0) {
if (--clipCounter == 0)
LEDState = LED_G;
}
else if (in == INT16_MAX || out == INT16_MAX ||
in == INT16_MIN || out == INT16_MIN)
{
LEDState = LED_R;
clipCounter = 50;
} else {
if (abs16(in) > 8192 || abs16(out) > 8192) {
LEDState = LED_Y;
clipCounter = 10;
}
}
}
void TPCalculateRelativePosition2(void) {
STP.TPModule.RelativePosition.Raw.Fields.u8Byte1.bits.b1Left = STP.TPModule.AbsolutePosition.Raw.Fields.u8Byte1.bits.b1Gesture;
if (STP.TPModule.AbsolutePosition.Raw.Fields.u8ZPressure < KEY_PRESED_THRESHOLD)
{
u8PacketInvalid = 5;
//STP.TPModule.RelativePosition.Raw.Fields.u8Byte1.u8Byte = 0;
}
if (u8PacketInvalid != 0)
{
u8PacketInvalid --;
STP.TPModule.AbsolutePosition.u16LastX = STP.TPModule.AbsolutePosition.u16X;
STP.TPModule.AbsolutePosition.u16LastY = STP.TPModule.AbsolutePosition.u16Y;
STP.TPModule.RelativePosition.Raw.Fields.u8XDelta = 0;
STP.TPModule.RelativePosition.Raw.Fields.u8YDelta = 0;
return;
}
STP.TPModule.u16Sensivity = 5;
STP.TPModule.RelativePosition.Raw.Fields.u8XDelta = (abs16(STP.TPModule.AbsolutePosition.u16X - STP.TPModule.AbsolutePosition.u16LastX)) / STP.TPModule.u16Sensivity;
STP.TPModule.RelativePosition.Raw.Fields.u8YDelta = (abs16(STP.TPModule.AbsolutePosition.u16Y - STP.TPModule.AbsolutePosition.u16LastY)) / STP.TPModule.u16Sensivity;
// if (abs8(STP.TPModule.RelativePosition.Raw.Fields.u8YDelta) > TP_MAX_DELTA) STP.TPModule.RelativePosition.Raw.Fields.u8YDelta = 0;
// if (abs8(STP.TPModule.RelativePosition.Raw.Fields.u8XDelta) > TP_MAX_DELTA) STP.TPModule.RelativePosition.Raw.Fields.u8XDelta = 0;
if (STP.TPModule.AbsolutePosition.u16LastX > STP.TPModule.AbsolutePosition.u16X)
STP.TPModule.RelativePosition.Raw.Fields.u8XDelta = -(signed char)STP.TPModule.RelativePosition.Raw.Fields.u8XDelta;
if (STP.TPModule.AbsolutePosition.u16LastY > STP.TPModule.AbsolutePosition.u16Y)
STP.TPModule.RelativePosition.Raw.Fields.u8YDelta = -(signed char)STP.TPModule.RelativePosition.Raw.Fields.u8YDelta;
STP.TPModule.AbsolutePosition.u16LastX = STP.TPModule.AbsolutePosition.u16X;
STP.TPModule.AbsolutePosition.u16LastY = STP.TPModule.AbsolutePosition.u16Y;
}
int AmpFollower::transient(int *inL, int *inR, int buffSize)
{
//peak squared method uses output of one level detector as input to another
for(int i = 0; i < buffSize; i++)
{
int amp = abs16(inL[i]);
lvlEst1 = peakDetector1.process(amp);
lvlEst2 = peakDetector2.process(lvlEst1);
//the amplitude is then averaged over a buffer
transientAvg += lvlEst2;
}
//divide transient average by buffer size, this is equal to multiplying by 1/256 which is 128
transientAvg = mult16(transientAvg, 128);
//examine slope of averages, if it is greater than the previous slope, a transient has started
transientSlope = transientAvg - prevTransientAvg;
//if the slope is greater than the previous slope a transient will have started
if(transientSlope > prevTransientSlope)
{
if(transientLock == 0)
{
transientLock = 1; //set transient lock
return 1;
}
}
//when the slope is less than the previous slope we've ended our transient and can start searching for new ones
if(transientSlope < prevTransientSlope)
transientLock = 0;
//update storage of previous values
prevTransientAvg = transientAvg;
prevTransientSlope = transientSlope;
return 0;
}
int AmpFollower::rmsEnvelope(int inL, int inR)
{
int amp = max(abs16(inL), abs16(inR));
rmsLevel += mult16(rmsb0, (mult16(amp, amp) - rmsLevel));
return sqrt(rmsLevel); //todo: double check this sqrt function and make sure it properly operates on fixed point
}
/**
* SensorRange = einschränkung der Reichweite. Verhindert unnötig lange Berechnung
* Return: Distance
*/
uint16_t driven_before(int8_t type, uint16_t SensorRange) {
double diffR;
#ifdef CARTOGRAPHY_SCENARIO
gps_reducedData_t *own;
uint16_t angle, diffL;
int16_t xCollision = 0, yCollision = 0, ownX, ownY;
int32_t newX, newY;
//Aktuelle Orientierung beachten!!! Orientierung in Abhängigkeit der Fahrzeugorientierung und dem aktuell betrachteten Sensor
own = get_ownCoords();
switch(type) {
case RIGHT_SENSOR:
angle = AngleMinus(own->angle, 16383);
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2; //siehe Anforderungen in wallFollow für align = 2
break;
case LEFT_SENSOR:
angle = AnglePlus(own->angle, 16383);
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;//siehe Anforderungen in wallFollow für align = 2
break;
case FRONT_SENSOR:
angle = own->angle;
if (SensorRange > RANGESIDE*2) SensorRange = RANGESIDE*2;
break;
default: angle = own->angle; break;
}
SensorRange = SensorRange / 5;
ownX = CoordinatesToMap(own->x);
ownY = CoordinatesToMap(own->y);
if (Sema != NULL) {
while ( xSemaphoreTake( Sema, ( portTickType ) 10 ) != pdTRUE ) {
os_wait(10);
}
newX = (SensorRange*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX;
ownX = (2*cos_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownX; //Ein stück aus dem Urpsrung rausgehen
newY = (SensorRange*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;
ownY = (2*sin_Taylor(uint16DegreeRelativeToX(angle)))/100 + ownY;
//gibt es ein bereits befahrenes Gebiet/Hindernis das zwischen Sensorwert und Auto liegt?
diffL = calc_reachability(&xCollision, &yCollision, ownX, ownY, (int16_t)newX, (int16_t)newY);
xSemaphoreGive(Sema);
} else return 600; //600 = max. Sensorreichweite
if (diffL == 1) {
diffR = sqrt(abs16(ownX - xCollision)*abs16(ownX - xCollision) + abs16(ownY - yCollision)*abs16(ownY - yCollision));
diffR = diffR*5;
if (diffR < 100) {
Seg_Hex(0x00);
}
else Seg_Hex(0x01);
}
else {
diffR = 600; // 600 für nichts gefunden
}
if (diffR > 600) diffR = 600;
#endif
return (uint16_t) diffR;
}
