148 lines
3.4 KiB
C
Executable file
148 lines
3.4 KiB
C
Executable file
#include "ball_tracker.h"
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Ball_Tracker::Update()
|
|
{
|
|
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
|
|
|
uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
|
|
uint16 intensity[sensorCount];
|
|
uint8 greatestIntensity = 0;
|
|
for(uint8 i = 0; i < sensorCount; i++)
|
|
{
|
|
IR_Sensor* currentSensor = parent->GetModule<IR_Sensor>(i + IO_SENSOR_IR_0_DEG);
|
|
|
|
intensity[i] = 1023 - currentSensor->GetIRIntensity();
|
|
|
|
if(intensity[i] < 24)
|
|
{
|
|
intensity[i] = 0;
|
|
}
|
|
|
|
if(intensity[i] > intensity[greatestIntensity])
|
|
{
|
|
greatestIntensity = i;
|
|
}
|
|
|
|
if(i == 0)
|
|
{
|
|
if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran
|
|
{
|
|
if(ballHeldCounter < 10) ballHeldCounter++;
|
|
}
|
|
else if(ballHeldCounter > 0)
|
|
{
|
|
ballHeldCounter--;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(intensity[greatestIntensity])
|
|
{
|
|
uint8 secondIntensity = 0xFF;
|
|
uint8 leftSensor = (greatestIntensity + 1) % sensorCount;
|
|
uint8 rightSensor = (greatestIntensity + sensorCount - 1) % sensorCount;
|
|
|
|
if(intensity[leftSensor])
|
|
{
|
|
secondIntensity = leftSensor;
|
|
}
|
|
|
|
if(intensity[rightSensor] > intensity[leftSensor])
|
|
{
|
|
secondIntensity = rightSensor;
|
|
}
|
|
|
|
float mainDirection;
|
|
|
|
switch(greatestIntensity + IO_SENSOR_IR_0_DEG)
|
|
{
|
|
case IO_SENSOR_IR_0_DEG:
|
|
mainDirection = 0;
|
|
break;
|
|
case IO_SENSOR_IR_30_DEG:
|
|
mainDirection = 1.0f * PI / 6.0f;
|
|
break;
|
|
case IO_SENSOR_IR_60_DEG:
|
|
mainDirection = 1.0f * PI / 3.0f;
|
|
break;
|
|
case IO_SENSOR_IR_100_DEG:
|
|
mainDirection = 5.0f * PI / 9.0f;
|
|
break;
|
|
case IO_SENSOR_IR_180_DEG:
|
|
mainDirection = PI;
|
|
break;
|
|
case IO_SENSOR_IR_260_DEG:
|
|
mainDirection = 13.0f * PI / 9.0f;
|
|
break;
|
|
case IO_SENSOR_IR_300_DEG:
|
|
mainDirection = 15.0f * PI / 9.0f;
|
|
break;
|
|
case IO_SENSOR_IR_330_DEG:
|
|
mainDirection = 33.0f * PI / 18.0f;
|
|
break;
|
|
default:
|
|
mainDirection = EMPTY_FLOAT;
|
|
return;
|
|
break;
|
|
}
|
|
|
|
if(secondIntensity != 0xFF)
|
|
{
|
|
float secondDirection;
|
|
|
|
switch(secondIntensity + IO_SENSOR_IR_0_DEG)
|
|
{
|
|
case IO_SENSOR_IR_0_DEG:
|
|
secondDirection = 0;
|
|
break;
|
|
case IO_SENSOR_IR_30_DEG:
|
|
secondDirection = 1.0f * PI / 6.0f;
|
|
break;
|
|
case IO_SENSOR_IR_60_DEG:
|
|
secondDirection = 1.0f * PI / 3.0f;
|
|
break;
|
|
case IO_SENSOR_IR_100_DEG:
|
|
secondDirection = 5.0f * PI / 9.0f;
|
|
break;
|
|
case IO_SENSOR_IR_180_DEG:
|
|
secondDirection = PI;
|
|
break;
|
|
case IO_SENSOR_IR_260_DEG:
|
|
secondDirection = 13.0f * PI / 9.0f;
|
|
break;
|
|
case IO_SENSOR_IR_300_DEG:
|
|
secondDirection = 15.0f * PI / 9.0f;
|
|
break;
|
|
case IO_SENSOR_IR_330_DEG:
|
|
secondDirection = 33.0f * PI / 18.0f;
|
|
break;
|
|
default:
|
|
secondDirection = EMPTY_FLOAT;
|
|
return;
|
|
break;
|
|
}
|
|
|
|
if(fabs(mainDirection - secondDirection) > PI)
|
|
{
|
|
min(mainDirection, secondDirection) += 2.0f * PI;
|
|
}
|
|
|
|
direction = (intensity[greatestIntensity] * mainDirection +
|
|
intensity[secondIntensity] * secondDirection) /
|
|
(intensity[greatestIntensity] + intensity[secondIntensity]);
|
|
}
|
|
else
|
|
{
|
|
direction = mainDirection;
|
|
}
|
|
|
|
direction += ourPositionTracker->GetOrientation();
|
|
|
|
direction = easyAngle(direction);
|
|
}
|
|
else
|
|
{
|
|
direction = EMPTY_FLOAT;
|
|
}
|
|
}
|