V5 Vision Sensor Nonsense

I have been starting and fiddling with the same section of code for four hours. I am having completely ridiculous issues using the V5 Vision sensor.

To summarize our robot design and goals: we have two vision sensors for skills autonomous. One is pointed low to detect balls, the other is facing forwards to detect flags. In the calibration software, both detect the objects fine. I have code that centers the robot facing whatever object I specify, and it works great for the balls. It fails for the flags inexplicably.

Here is the code I am using for this aligning:

void turnToObject(double targetRotation, int expectedX, int expectedY, vex::vision::signature objSig, int sleep)
{
  Vision.setLedColor(255,255,255);
  VisionHigh.setLedColor(255,255,255);

  vex::vision curVis = (objSig.uMean == GOAL.uMean) ? VisionHigh : Vision;
  Controller.Screen.clearScreen();
  Controller.Screen.setCursor(1, 1);
  Controller.Screen.print("%d", GOAL.uMean);
  Controller.Screen.setCursor(2, 1);
  Controller.Screen.print("%d", objSig.uMean);
  Controller.Screen.setCursor(3, 1);
  Controller.Screen.print("%d", objSig.uMean == GOAL.uMean);

  vex::task::sleep(4000);

  //Make turning happen quicker so camera can be main factor in turning
  range = 4;
  baseSpeed = 22;
  maxSpeed = 40;
  maxBoost = maxSpeed - baseSpeed;
  turnTo(targetRotation);

  int successes = 0;

  //Camera-based turning happens here
  while(true)
  {
    curVis.takeSnapshot(objSig);

    if(curVis.objectCount > 0)
    {
      curVis.setLedColor(0,255,0);
      int objIndex = getObjClosestToPoint(expectedX, expectedY, objSig, curVis);
      Controller.Screen.clearScreen();
      if(objIndex == -1)
      {
        Controller.Screen.setCursor(1, 1);
        Controller.Screen.print("Error! -1 Index");
        break;
      }

      expectedX = curVis.objects[objIndex].centerX;
      expectedY = curVis.objects[objIndex].centerY;

      Controller.Screen.setCursor(1, 1);
      Controller.Screen.print("X:%d, Y:%d", expectedX, expectedY);
      Controller.Screen.setCursor(2, 1);
      Controller.Screen.print("%d", objIndex);

      LFront.spin(vex::directionType::fwd, sign(expectedX - centerX) * 3 + (expectedX - centerX) * .1, vex::velocityUnits::pct);
      LBack.spin(vex::directionType::fwd, sign(expectedX - centerX) * 3 + (expectedX - centerX) * .1, vex::velocityUnits::pct);
      RFront.spin(vex::directionType::fwd, sign(expectedX - centerX) * -3 - (expectedX - centerX) * .1, vex::velocityUnits::pct);
      RBack.spin(vex::directionType::fwd, sign(expectedX - centerX) * -3 - (expectedX - centerX) * .1, vex::velocityUnits::pct);

      if(expectedX < centerX + 3 && expectedX > centerX - 3)
      {
        successes += 1;
        if(expectedX < centerX + 1 && expectedX > centerX - 1)
        {
          LFront.stop();
          LBack.stop();
          RFront.stop();
          RBack.stop();
        }

        if(successes > 7)
        {
          //End rotation
          curVis.setLedColor(0, 0, 255);
          break;
        }
      }
      else
      {
        successes = 0;
      }
    }
    else
    {
      curVis.setLedColor(255,0,0);
      range = 4;
      baseSpeed = 8;
      maxSpeed = 18;
      maxBoost = maxSpeed - baseSpeed;
      
      turnTo(targetRotation);
      break;
    }
    vex::task::sleep(25);
  }

  //Reset variables
  range = 4;
  baseSpeed = 8;
  maxSpeed = 18;
  maxBoost = maxSpeed - baseSpeed;

  vex::task::sleep(sleep);
}

The screen prints are for debugging.

When I run this for the goal markers (the green flags), it does successfully detect the flags, however it reports its position as (0, 0), which causes the robot to turn to the left constantly.

This is where I start getting very confused. If I replace the variable “curVis” in “curVis.takeSnapshot()” with a hardcoded “VisionHigh.takeSnapshot()”, it works. If I have a separate loop running somewhere else entirely and it runs “VisionHigh.takeSnapshot(GOAL)”, it works. Neither of these are valid solutions for me for various reasons. Why does this happen?

I’m willing to share any other code needed for this, it is extremely infuriating.

This creates a new instance of a vision sensor, I doubt that will work as it will have its own internal variables etc. and I’m not even sure what port it would use (pretty sure we didn’t implement a copy constructor), perhaps use a reference instead.

3 Likes

Really? This creates a copy of the object? If so, that could certainly explain it, but I thought that objects were not copied like that but instead used a reference. From what I recall, this is how Java does it and I had thought C++ was the same.

I’ll try using references when I get back tomorrow. I tried references for several other things in my code but not that.

I think that did it. Thank you very much. I spent far too long trying to fix something so simple.

2 Likes