// Task for reactivision output processing
void vServoTask( void)
{
u16_t ripaddr[2];
portBASE_TYPE xStatus;
xTuioQueuePacket xValueRead;
xTuioQueuePacket xDistanceValue[6];
// Tracking Variables
short sPosX = 0;
short sPosY = 0;
short sDegreesX = servoMIN_DEGREES;
short sDegreesY = (servoMIN_DEGREES+servoMAX_DEGREES)/4;
vServo_ConfigurePwm(sDegreesX, sDegreesY);
sX = 0;
sY = 0;
sZ = 0;
// Uip connect
vTaskDelay(1000);
uip_ipaddr(ripaddr, 192,168,0,1);
uip_connect(ripaddr, HTONS(3000));
xTuioQueue = xQueueCreate(20, sizeof(xTuioQueuePacket));
vSemaphoreCreateBinary(xControlServo);
short sFlightPlanStage = servotaskFLIGHT_PLAN_1;
short sGoalPoint;
short sGoalCounter;
// Servo control loop
for (;;) {
//Read from TUIO queue.
xStatus = xQueueReceive(xTuioQueue, &xValueRead, 10); // Block task for 10ms when waiting for the Queue
if (xStatus == pdPASS) { // Process received value
// values are between 0 and 1
sPosX = (short) (xValueRead.position_x*100.0f);
sPosY = (short) (xValueRead.position_y*100.0f);
short sId = xValueRead.class_id-servoFIDUCIAL_SET;
// If the middle fiducial marker, track it with the camera
if (sId >= 0 && sId <= 5) {
// Remember, position is taken from TOP LEFT
if (sPosX < servoBOUNDING_BOX_MIN && sDegreesX < servoMAX_DEGREES) {
sDegreesX++;
} else if (sPosX > servoBOUNDING_BOX_MAX && sDegreesX > servoMIN_DEGREES) {
sDegreesX--;
}
if (sPosY < servoBOUNDING_BOX_MIN && sDegreesY < servoMAX_DEGREES) {
sDegreesY++;
} else if (sPosY > servoBOUNDING_BOX_MAX && sDegreesY > servoMIN_DEGREES) {
sDegreesY--;
}
// Set the fiducial to being used, and the value to the current packet
sDistanceUsed[sId] = 1;
xDistanceValue[sId] = xValueRead;
// If there is an ID to compare to, calculate distance
if (sGetId(sId) != -1 && sTask5) {
short sNextId = sGetId(sId);
// Print markers used for distancing
//debug_printf("markers: %d %d\r\n", xValueRead.class_id, xDistanceValue[sNextId].class_id);
// Compute the distance to the fiducial
double dD = sApproxSqrt(sPow(sPosX-(short) (xDistanceValue[sNextId].position_x*100.0f),2) +
sPow(sPosY-(short) (xDistanceValue[sNextId].position_y*100.0f),2));
dD = (33379*sPow((short) dD,2) - 2288800*dD + 44475000)/10000;
//debug_printf(">> Distance: %d\r\n", (short) dD);
// Calculate the XYZ coordinates.
double dDegX = sDegreesX - servoMIN_DEGREES - (servoMIN_DEGREES+servoMAX_DEGREES)/2;
double dDegY = sDegreesY - servoMIN_DEGREES;
sX = (short) (dD*(sin((double) (dDegX/180.0f*3.14f))));
sY = (short) (dD*(sin((double) (dDegY/180.0f*3.14f))));
sZ = (short) (dD*(cos((double) (dDegX/180.0f*3.14f))));
//debug_printf(">> Angles: %d %d\r\n", (short) dDegX, (short) dDegY);
//debug_printf(">> Point: %d %d %d\r\n", sX, sY, sZ);
// On detecting the blimp, set the goal to 1.5m in X and flight plan to 2
if (sId < 3 && sFlightPlanStage == servotaskFLIGHT_PLAN_1) {
sGoalPoint = sX + 1500;
sGoalCounter = 0;
sFlightPlanStage = servotaskFLIGHT_PLAN_2;
debug_printf("Starting stage 2\t\t\ Goal: %d\r\n", sGoalPoint);
// If in stage 2, check if it has reached it's goal point. if this is confirmed
// using a counter, move to stage 3
} else if (sFlightPlanStage == servotaskFLIGHT_PLAN_2) {
sSetSpeed(0xFF);
if (sX > sGoalPoint) {
sGoalCounter++;
}
if (sGoalCounter > 10) {
sFlightPlanStage = servotaskFLIGHT_PLAN_3;
debug_printf("Starting stage 3\r\n");
}
// Set the goal point 1.5m back in x and move to stage 4
} else if (sFlightPlanStage == servotaskFLIGHT_PLAN_3) {
sRequestTurnLeft();
if (sId > 2) {
sGoalPoint = sX - 1500;
sGoalCounter = 0;
sFlightPlanStage = servotaskFLIGHT_PLAN_4;
debug_printf("Starting stage 4\t\t\ Goal: %d\r\n", sGoalPoint);
}
// Task for reactivision output processing
void vReactivision_Task( void)
{
u16_t ripaddr[2];
portBASE_TYPE xStatus;
xTuioPacket xValueRead;
xTuioPacket xDistanceRead;
// Tracking Variables
short sPosX = 0;
short sPosY = 0;
short sDegreesX = (servoMIN_DEGREES+servoMAX_DEGREES)/2;
short sDegreesY = servoMAX_DEGREES;
// Load up the log file and get this sessions log number
short sFileNo;
sFileNo = sSdCard_GetLogNo();
debug_printf("Fileno %d\r\n", sFileNo);
vSdCard_CreateLogFile(sFileNo);
//Make TCP connection
uip_ipaddr(ripaddr, 192,168,0,1);
uip_connect(ripaddr, HTONS(3000));
//Make TCP connection for remote logger
uip_ipaddr(ripaddr, 192,168,0,1);
uip_connect(ripaddr, HTONS(3010));
//Create queue
xTuioQueue = xQueueCreate(5, sizeof(xTuioPacket));
xLogQueue = xQueueCreate(20, sizeof(xLogStruct));
vSemaphoreCreateBinary(xControlServo);
// Servo control loop
for (;;) {
if (xSemaphoreTake(xControlServo, 10) == pdFALSE) {
continue;
}
//Read from TUIO queue.
xStatus = xQueueReceive(xTuioQueue, &xValueRead, 10); // Block task for 10ms when waiting for the Queue
if (xStatus == pdPASS) { // Process received value
// values are between 0 and 1
sPosX = (short) (xValueRead.position_x*100.0f);
sPosY = (short) (xValueRead.position_y*100.0f);
// Calculate distance
if (sFiducialDistance) {
// Loop until two values are set for the fiducial marker
while (sFiducialDistance && !(xQueueReceive(xTuioQueue, &xDistanceRead, 10) == pdPASS
&& xValueRead.class_id != xDistanceRead.class_id));
if (sFiducialDistance) {
// Compute the distance
short sX = sApproxSqrt(sPow(sPosX-(short) (xDistanceRead.position_x*100.0f),2) +
sPow(sPosY-(short) (xDistanceRead.position_y*100.0f),2));
sprintf(ucRemoteBuffer, ">> Z-dist: %d\r\n", (1849*sPow(sX,2) - 297690*sX + 14309000)/10000);
remote_printf(ucRemoteBuffer);
}
// Track servo
} else {
// Remember, position is taken from TOP LEFT
if (sPosX < mainBOUNDING_BOX_MIN && sDegreesX < servoMAX_DEGREES) {
sDegreesX++;
} else if (sPosX > mainBOUNDING_BOX_MAX && sDegreesX > servoMIN_DEGREES) {
sDegreesX--;
}
if (sPosY < mainBOUNDING_BOX_MIN && sDegreesY > servoMIN_DEGREES) {
sDegreesY--;
} else if (sPosY > mainBOUNDING_BOX_MAX && sDegreesY < servoMAX_DEGREES) {
sDegreesY++;
}
vSdCard_LogData(sFileNo,
(unsigned long) xTaskGetTickCount(),
sPosX,
sPosY,
sDegreesX,
sDegreesY);
}
}
// Set the servos and give the semaphore
vServo_SetPan(sDegreesX);
vServo_SetTilt(sDegreesY);
xSemaphoreGive(xControlServo);
vTaskDelay(10);
}
}
void s3DWindow::QuakeCam()
{
sInt time = sGetTime();
{
GearSpeed = sPow(2,GearShift*0.5f);
sF32 ss = SideSpeed*GearSpeed;
sF32 sf = ForeSpeed*GearSpeed;
if(sGetKeyQualifier()&sKEYQ_SHIFT)
{
ss *= 10;
sf *= 10;
}
else if(sGetKeyQualifier()&sKEYQ_CTRL)
{
ss *= 0.125f;
sf *= 0.125f;
}
sVector30 force,speed;
force.Init(0,0,0);
if(sGetKeyQualifier()&sKEYQ_ALT)
{
if(QuakeMask & 1) RotY += ss*1000.0f;
if(QuakeMask & 2) RotY -= ss*1000.0f;
if (QuakeMask & 3)
{
sF32 dist = -(Pos.l - Focus).Length();
Pos.EulerXYZ(RotX,RotY,RotZ);
Pos.l = Focus + (Pos.k * dist);
}
}
else
{
if(QuakeMask & 1) force -= Pos.i * ss;
if(QuakeMask & 2) force += Pos.i * ss;
}
if(QuakeMask & 4) force -= Pos.k * sf;
if(QuakeMask & 8) force += Pos.k * sf;
if(QuakeMask & 16) force -= Pos.j * ss;
if(QuakeMask & 32) force += Pos.j * ss;
if(SpeedDamping>0)
{
speed = QuakeSpeed;
for(sInt i=QuakeTime;i<time;i++)
{
speed += force;
// speed = force*500.0f;
speed = speed*SpeedDamping;
Pos.l += speed;
}
QuakeSpeed = speed;
} else
{
for(sInt i=QuakeTime;i<time;i++)
{
QuakeSpeed += force;
// QuakeSpeed = force*500.0f;
Pos.l += QuakeSpeed;
}
QuakeSpeed.Init(0,0,0);// = 0.f;
}
}
if(QuakeMode)
{
Pos.EulerXYZ(RotX,RotY,RotZ);
}
QuakeTime = time;
}
