Example #1
0
// 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);
                        }
Example #2
0
// 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);
    }
}
Example #3
0
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;
}