126 lines
2.9 KiB
C
126 lines
2.9 KiB
C
|
#include "ball_tracker.h"
|
||
|
|
|
||
|
|
//-----------------------------------------------------------------------------
|
||
|
|
void Ball_Tracker::Update()
|
||
|
|
{
|
||
|
|
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(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 = -1.0f;
|
||
|
|
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 = -1.0f;
|
||
|
|
return;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
direction = (intensity[greatestIntensity] * mainDirection +
|
||
|
|
intensity[secondIntensity] * secondDirection) /
|
||
|
|
(intensity[greatestIntensity] + intensity[secondIntensity]);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
direction = mainDirection;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
direction = -1.0f;
|
||
|
|
}
|
||
|
|
}
|