Robot not moving forward and turning to desired position

FIRST General Forum
, , , ,
1

I am trying to make a robot move and turn to a ball target. Whenever the ball is to the left of the robot, the right wheels will move faster than then the left wheel so the robots turns left. And vice versa for the right side. For some reason while it is in this stage, of either turning left or right, it will move forward and turn in the correct direction but stop prematurely. I get the distance to the target using PhotonLib and use encoders to track the robot’s progression toward the target. What could be going wrong? Is my math possibly wrong. And if so, how can I fix it.

Sidenote: leftModularEncoder returns negative values when the left wheels rotate foward and in order to drive the robot forward you must write theTank.drive(-1, 1);

Math:
currentTurningDistanceFromTarget = (degreeToTarget*DEGREESTOMETERS) - (-leftModularEncoder.getDistance() - rightModularEncoder.getDistance());

Concerned Code
//If ball is right of the robot

      if( (degreeToTarget*DEGREESTOMETERS) - 0.04 > 0)

      {

        hasTurned = true;

        //Rotate information

        currentTurningDistanceFromTarget = (degreeToTarget*DEGREESTOMETERS) - (-leftModularEncoder.getDistance() - rightModularEncoder.getDistance());

        motorPowerToTurn = MathUtil.clamp(rotationPIDController.calculate(currentTurningDistanceFromTarget, 0), -1.0, 1.0);

      

        //Foward information: May be faulty when it comes to math

        currentDistanceAwayFromTarget = (distanceToTarget*DISTANCETOTARGETFACTOR) - rightModularEncoder.getDistance();

        motorPower = MathUtil.clamp(rotatingdistancePIDController.calculate(currentDistanceAwayFromTarget, 0), -1.0, 1.0);

        //Moving to Target

        theTank.drive(MathUtil.clamp(-motorPowerToTurn-motorPower, -1.0, 1.0), motorPower);

        System.out.println("To Left excess | Left: "+ MathUtil.clamp(-motorPowerToTurn-motorPower, -1.0, 1.0) + " Right: " + motorPower + " Current distance to target: " + distanceToTarget*DISTANCETOTARGETFACTOR);

        //System.out.println("Moving left Current Distance To Target: " + currentDistanceAwayFromTarget + " Current Angle Away from Target: " + currentTurningDistanceFromTarget);

      }

      //If ball is left of the camera

      else if(((-degreeToTarget*DEGREESTOMETERS) - 0.04 > 0))

      {

        hasTurned = true;

        //Rotate information

        currentTurningDistanceFromTarget = (-degreeToTarget*DEGREESTOMETERS) - (rightModularEncoder.getDistance() + leftModularEncoder.getDistance());

        motorPowerToTurn = MathUtil.clamp(rotationPIDController.calculate(currentTurningDistanceFromTarget, 0), -1.0, 1.0);

        //Foward information

        currentDistanceAwayFromTarget = (distanceToTarget*DISTANCETOTARGETFACTOR) - rightModularEncoder.getDistance();

        motorPower = MathUtil.clamp(rotatingdistancePIDController.calculate(currentDistanceAwayFromTarget, 0), -1.0, 1.0);

        //Moving to Target

        theTank.drive(-motorPower, MathUtil.clamp(motorPowerToTurn+motorPower, -1.0, 1.0));

        System.out.println("To Right excess | Left: " + -motorPower + " Right: " + MathUtil.clamp(motorPowerToTurn+motorPower, -1.0, 1.0)  + " Current distance to target: " + distanceToTarget*DISTANCETOTARGETFACTOR);

        //System.out.println("Moving right Current Distance To Target: " + currentDistanceAwayFromTarget + " Current Angle Away from Target: " + currentTurningDistanceFromTarget);

      }

      else

      {

        if(hasTurned){

          leftModularEncoder.reset();

          rightModularEncoder.reset();

          hasTurned = false;

        }

        //Foward information

        currentDistanceAwayFromTarget = (distanceToTarget*DISTANCETOTARGETFACTOR) - rightModularEncoder.getDistance();

        motorPower = MathUtil.clamp(distancePIDController.calculate(currentDistanceAwayFromTarget, 0), -1.0, 1.0);

        //Moving to target

        theTank.drive(-motorPower, motorPower);

        System.out.println("Straight with motor power " + motorPower + " Distance from target: " + distanceToTarget*DISTANCETOTARGETFACTOR + " Encoder Distance Covered: " + -leftModularEncoder.getDistance());

        //System.out.println("Moving Straight Current Distance To Target: " + currentDistanceAwayFromTarget + " Current Angle Away from Target: " + currentTurningDistanceFromTarget);

      }

      

      try {

        info = ballTracker.getTargetGoal();

      } catch (NullPointerException e) {

        System.out.println("Lost sight of a target");

        theTank.drive(0, 0);

      }

        

        distanceToTarget = info.get("Distance");

        degreeToTarget = info.get("Yaw");

  }

Robot.java (17.6 KB)

2

Printout/log the power values you are sending into the motors… Are they zero? Or are they so small that they don’t make the robot move?

3

They’re small and sometimes even becoming values that make the robot go backwards

4

Is it possible that the power to the motors is so small that the robot can’t overcome friction to keep turning?

5

Yes, that is likely the case and I have tried increasing the kP value on rotationPIDController, which is responsible for controlling the excess value that gets added to one side, to counteract this, but it did nothing.

6

I think you need to either specify a “feed forward “ amount or a minimum power.

Simplest to start with if the power is less than say 15% make it at least 15%. You’ll have to correct for sign….

This may cause the robot to oscillate …

7

So you think the math is not the problem. Because the only reason it should be negative is if the math that I do is wrong. Right? Math being in how I believe the difference between the encoder distance values is directly related to the degree in which the robot turns (without multiplying , dividing, adding, or subtracting anything to that original difference). Secondly, following your solution, what do you think the problem is? Why am I getting values that indicate the target is literally behind the robot.

8

I’m inclined to agree with you; if you’re seeing negative values in situations where that doesn’t make sense then the first order of business is to track down why that’s happening.

9

I just have Misunderstood what the issue is…. Sounded to me like like your PID wasn’t tuned to overcome friction.

10

I would drop the PID and just use “bang bang” control to debug all of the sensors and their orientations… Or log all the values out….

11

You could use a simple non-PID position controller to move towards a target.

First – calculate error = (desired angle - current angle)
Then calculate drive “spin” speed using a position control algorithm. A description of the algorithm can be found here. https://github.com/jsimpso81/FRC_Secret_Book_Of_FRC_LabVIEW_2/tree/main/ControlTheoryTraining/module11_PositionControl

This isn’t as elegant as PID, or other modeling approaches, but it is easier to implement and tune. Also it is generally fairly robust – as long as the inputs are good!

One other consideration – regardless of what you use – expect different results when running the robot on different types of floors.

12

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.