Autonomous Code not Working (note we are using a limelight)

We are using a limelight to guide the robot during both autonomous and some parts of teleop. We are using a Limelight.

The main issue is with seeking and ranging. We want one part of autonomous to help guide the robot in the correct distance between the target. And then another one positions the robot in the right angle.

Our code:

DriveSubsystem:


// All the imports:
package frc.robot.subsystems;

import static frc.robot.OI.*;
import static frc.robot.RobotMap.*;

import com.ctre.phoenix.motorcontrol.Faults;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;

import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.Robot;
import frc.robot.commands.DriveCommand;
import edu.wpi.first.wpilibj.command.Subsystem;

import edu.wpi.first.wpilibj.smartdashboard.*;
import edu.wpi.first.wpilibj.shuffleboard.*;

import edu.wpi.first.wpilibj.Sendable;

// Class declaration:
public class DriveSubsystem extends Subsystem {
    // Creating variables/objects:
    WPI_TalonSRX _rFront;
    WPI_TalonSRX _rBack;
    WPI_TalonSRX _lFront;
    WPI_TalonSRX _lBack;

    DifferentialDrive _diffDrive;

    Faults _lFaults;
    Faults _rFaults;

    ADXRS450_Gyro gyro = new ADXRS450_Gyro();

    // This instantiates, or sets up all of the variables. For example, it sets the right front wheel to the 2nd talon.
    public DriveSubsystem() {

        _rFront = new WPI_TalonSRX(1);
        _rBack = new WPI_TalonSRX(2);
        _lFront = new WPI_TalonSRX(3);
        _lBack = new WPI_TalonSRX(4);

        _diffDrive = new DifferentialDrive(_lFront, _rFront);
        _lFaults = new Faults();
        _rFaults = new Faults();

        SmartDashboard.putData(_diffDrive);

        SmartDashboard.putData(gyro);

    }

    // This is the driver method, that is run constantly in the DriveCommand. This is what takes raw data from the joysticks and pushes power to the motors.
    public void Driver() {

        SmartDashboard.putNumber("Joystick X value",leftJoystick.getX());
        SmartDashboard.putNumber("Joystick Y value",leftJoystick.getY());

        String work = "";

        // Constantly update the forw and turn variables with joystick data:

        double forw = -1 *leftJoystick.getRawAxis(1); /* pos = forward */
        double turn = +1 *leftJoystick.getRawAxis(2); /* pos = right */
        boolean btn1 =leftJoystick.getRawButton(1); /* if button is down, print joystick values */

        // Margin of error for joystick sensitivity = 10%
        if (Math.abs(forw) < 0.10) {
            forw = 0;
        }
        if (Math.abs(turn) < 0.10) {
            turn = 0;
        }

        // DRIVE THE ROBOT:
        _diffDrive.arcadeDrive(forw/1.5, turn);

        // Data printed to make sure joystick forward is positive and joystick turn is positive for right
        work += " JF:" + forw + " JT:" + turn;

        // Get sensor values:
        double leftVelUnitsPer100ms = _lFront.getSelectedSensorVelocity(0);
        double rghtVelUnitsPer100ms = _rFront.getSelectedSensorVelocity(0);
        // double leftPos = _leftFront.GetSelectedSensorPosition(0);
        // double rghtPos = _rghtFront.GetSelectedSensorPosition(0);

        work += " L:" + leftVelUnitsPer100ms + " R:" + rghtVelUnitsPer100ms;

        // Recognize any faults in the sensor:
        _lFront.getFaults(_lFaults);
        _rFront.getFaults(_rFaults);

        if (_lFaults.SensorOutOfPhase) {
            work += " L sensor is out of phase";
        }
        if (_rFaults.SensorOutOfPhase) {
            work += " R sensor is out of phase";
        }

        // Print to drive station when button 1 is pressed:
        if (btn1) {
            System.out.println(work);
        }
    }

    public void autonomous(){

        _diffDrive.arcadeDrive(5, 0); //Drive forward at speed 5 and 0 rotation
        Timer.delay(2.0); //Continue for 2 seconds
        _diffDrive.arcadeDrive(0, 0); //Stop driving

        if (Robot.limelight.getTV() != 1) { //Check to see if target is detected
            autonomousSeeking();
        }

    }

    public void autonomousSeeking() { //Attempts to find target
        if (!leftJoystick.getRawButtonPressed(EMG_Stp)) { //Checks autonomous kill button
            if (Robot.limelight.getTV() != 1) {
                _diffDrive.arcadeDrive(0.1, 0.1);
            }
        }
    }

    public void ranging() {
        if (Robot.limelight.calcXDist() > 125) {
            _diffDrive.arcadeDrive(0, -0.1);
        }
        if (Robot.limelight.calcXDist() < 115) {
            _diffDrive.arcadeDrive(0, 0.1);
        }
    }

    public void goto180() {
        if (gyro.isConnected()) {
            while ((!(gyro.getAngle() > 179 && gyro.getAngle() < 181)) && !leftJoystick.getRawButtonPressed(2)) {
                _diffDrive.arcadeDrive(0.1, -0.1);
            }
        } else {
            System.out.println("Gyro not connected!");
        }
    }

    // This initializes everything in the subsystem, sets everything to "default":
    public void initDefaultCommand() {

        // Set all the values to factory default
        _rFront.configFactoryDefault();
        _rBack.configFactoryDefault();
        _lFront.configFactoryDefault();
        _lBack.configFactoryDefault();

        // Set up the followers
        _rBack.follow(_rFront);
        _lBack.follow(_lFront);

        // Flip values so robot moves forward when LEDs are green
        _rFront.setInverted(true); // !< Update this
        _lFront.setInverted(false); // !< Update this

        // Set values that are backward, or inverted to whatever is opposite
        _rBack.setInverted(InvertType.FollowMaster);
        _lBack.setInverted(InvertType.FollowMaster);

        // Adjust sensor phase so sensor moves positive when Talon's LEDs are green
        _rFront.setSensorPhase(true);
        _lFront.setSensorPhase(true);

        // Make sure left and right are opposite.
        _diffDrive.setRightSideInverted(false);

        // Help this subsystem recognize that DriveCommand.java is its main command file
        setDefaultCommand(new DriveCommand());

    }

}

Is your robot oscillating at all or is it just not responding to the command?

It seems more like a logic problem. It does move but it does not do what we intended. It does not position itself within the distance. Note that I currently don’t have access to the robot so I cannot test things but any help with some of the logic involved would be helpful

so your autonomousSeeking() method seems to be only moving the robot if limelight does not see a target. however TV is going to be true if limelight sees a target anywhere on the screen, not just the center. unless of course your .getTV() method only returns true if it is within certain constraints. I would also put your TV onto smart dashboard and see what your limelight is seeing when your seeking method runs.

also how does your distance formula work? have you tried measuring to make sure it is getting the right values. Those can be pretty finicky. Another thing i noticed was that your ranging method is applying power to the turn parameter rather than the forward parameter of arcadeDrive. Is it supposed to line you up at the correct angle or distance?

The distance formula works by using the change in angle from (ty) and uses trig to solve the rest. The height of the camera on the robot and the height of the target are all constants, as far as we know. Also, we just noticed the mistake with the arcadedrive, we will get to fixing that. The target is set to certain constraints with the limelight’s dashboard but that’s true, we should add more constraints before the autonomous code just works on its own. Thank you

1 Like

We used regression instead of trigonometry, takes a bit more time but should be much more reliable.

Could you explain more what that is? We would love some help with really fine tuning our distance calculation methods. We’re still pretty new to using the limelight and any new methods would be very helpful.

Let’s say you have an amount of dots on a graph, and you want to find the line that passes the closest to as many dots as possible. Regression is essentially the math of finding the function (i.e. f(x) or y = x) of that line. The math itself is tricky, but Excel / Google Sheets can do it for you.
All you need to do is position the robot a certain distance from the target and record the ty value that you get from the limelight. Repeat this as many times as you can from varying distances (differences of half a meter worked for us).
After this, you should have a table of ty/distance pairs. Use Excel/Sheets to generate a graph from the table (you want ty as the x-axis), and get the expression of the graph. It has a very good chance of being an expression you wouldn’t want to meet in a math test, but I think you can limit the type of expression in the Excel options, linear works fine. Convert the expression into code (in java terms, a static method is great for this), and read the distance using something like: double distance = Utils.calculateDistance(limelight.getTy());. Preferably put the calculation in your Limelight class so you can use limelight.getDistance(); instead of the above pseudocode, but that is your decision. There is some explanation of how to do this in the limelight docs that uses ta (which might be better, I’m not sure).

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