JaciBrunning / Pathfinder

Cross-Platform, Multi-Use Motion Profiling and Trajectory Generation
MIT License
255 stars 78 forks source link

"A fatal error has been detected by the Java Runtime Environment" #40

Open BoxWizard000000 opened 6 years ago

BoxWizard000000 commented 6 years ago

** Robot program starting ** NT: server: client CONNECTED: 10.1.35.17 port 60883 Opening USB serial port to communicate with navX-MXP/Micro navX-MXP/Micro Connected via USB. navX-MXP/Micro Configuration Response Received. HI! 1 12.857513427734375 0.0 HI! A fatal error has been detected by the Java Runtime Environment:

SIGSEGV (0xb) at pc=0xa9c0d398, pid=17983, tid=0xb52ab470

JRE version: OpenJDK Runtime Environment (8.0_131-b57) (build 1.8.0_131-b57) Java VM: OpenJDK Client VM (25.131-b57 mixed mode, Evaluation linux-aarch32 ) Problematic frame: C [libpathfinderjava.so+0x6398] pf_trajectory_fromSecondOrderFilter+0x2c8

Core dump written. Default location: //core or core.17983 (max size 2048 kB). To ensure a full core dump, try "ulimit -c unlimited" before starting Java again

An error report file with more information is saved as: /tmp/hs_err_pid17983.log

I'm really new to motion profiling and thought I'd use these libraries but upon startup I get this error. What do I need to provide to help alleviate this problem?

virtuald commented 6 years ago

The library isn't particularly resilient to incorrect initialization. Perhaps if you posted your code we could point out what you missed?

BoxWizard000000 commented 6 years ago
public interface RobotMap 
{
    public interface PROFILING
    {
        public static final Waypoint[] 

                MID_TO_RIGHT_SWITCH =
                {
                    new Waypoint(0, 0, 0),
                    new Waypoint(0.288, 11.167, Pathfinder.d2r(357.04)),
                    new Waypoint(1.163, 22.333, Pathfinder.d2r(353.98)),
                    new Waypoint(2.660, 33.500, Pathfinder.d2r(350.71)),
                    new Waypoint(4.847, 44.667, Pathfinder.d2r(347.07)),
                    new Waypoint(7.839, 55.833, Pathfinder.d2r(342.82)),
                    new Waypoint(11.845, 67.000, Pathfinder.d2r(337.51)),
                    new Waypoint(17.279, 78.167, Pathfinder.d2r(330.20)),
                    new Waypoint(25.206, 89.333, Pathfinder.d2r(318.07)),
                    new Waypoint(46.516, 100.5, Pathfinder.d2r(270.00))
                },

                MID_TO_LEFT_SWITCH =
                {
                    new Waypoint(0, 0, 0),
                    new Waypoint(-1.125, 11.167, Pathfinder.d2r(11.51)),
                    new Waypoint(4.648, 22.333, Pathfinder.d2r(23.51)),
                    new Waypoint(-10.681, 33.500, Pathfinder.d2r(30)),
                    new Waypoint(-17.128, 44.667, Pathfinder.d2r(30)),
                    new Waypoint(-23.575, 55.833, Pathfinder.d2r(30)),
                    new Waypoint(-30.022, 67.000, Pathfinder.d2r(30)),
                    new Waypoint(-36.469, 78.167, Pathfinder.d2r(30)),
                    new Waypoint(-42.196, 89.333, Pathfinder.d2r(30)),
                    new Waypoint(-61.508, 100.5, Pathfinder.d2r(90))
                };
    }

I declare my waypoints in RobotMap like so. It is used inside of a constructor in the command "DriveAlongProfile."

DriveAlongProfile:

package org.usfirst.frc.team135.robot.commands.auton.singles;

import org.usfirst.frc.team135.robot.Robot;
import org.usfirst.frc.team135.robot.RobotMap;
import org.usfirst.frc.team135.robot.RobotMap.COMPETITION.DRIVETRAIN;
import org.usfirst.frc.team135.robot.util.PIDIn;
import org.usfirst.frc.team135.robot.util.PIDOut;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import jaci.pathfinder.*;
import jaci.pathfinder.followers.EncoderFollower;
import jaci.pathfinder.modifiers.TankModifier;
/**
 *
 */
public class DriveAlongProfile extends Command implements RobotMap {

    private Waypoint[] _points;
    private Trajectory _leftTrajectory, _rightTrajectory;
    private EncoderFollower _leftEncoderFollower, _rightEncoderFollower;

    private static final Trajectory.FitMethod _FIT_METHOD = Trajectory.FitMethod.HERMITE_QUINTIC;

    private static final int _SAMPLES = Trajectory.Config.SAMPLES_HIGH;

    private static final double
        _TIMESTEP = 0.05, //In seconds
        _MAX_VELOCITY = DRIVETRAIN.MAX_VELOCITY_TICKS * CONVERSIONS.TICKS2METERS,
        _MAX_ACCELERATION = DRIVETRAIN.MAX_ACCELERATION_TICKS * CONVERSIONS.TICKS2METERS,
        _MAX_JERK = DRIVETRAIN.MAX_JERK_TICKS * CONVERSIONS.TICKS2METERS;

    private static final double _VELOCITY_RATIO = 1 / _MAX_VELOCITY;

    private static final double 
        _drivekP = 1.0,
        _drivekI = 0.01,
        _drivekD = 10.0,
        _drivekA = 0; //Acceleration gain. Tweak this if you want to go to accelerate faster.

    private PIDController _angleController;
    private PIDOut _buffer;
    private PIDIn _pidSource;

    private static final double 
        _turnkP = .267,
        _turnkI = 0.00267,
        _turnkD = 2.67,
        _turnkF = 0; //There might be some error causing real world process we can take out with kF

    private static final double _TRACK_WIDTH = DRIVETRAIN.TRACK_WIDTH * CONVERSIONS.INCHES2METERS; //Jaci calls this wheelbase width

    private Timer _timer;
    private double _timeout = 5.0;

    private boolean _done = false;

    public DriveAlongProfile(Waypoint[] points, double timeout) {
        requires(Robot.drivetrain);

        this._timer = new Timer();
        this._timeout = timeout;

        this._points = points.clone(); //Don't want a reference of points, I want a copy of it

        this._buffer = new PIDOut();
        this._pidSource = new PIDIn(() -> Robot.navx.getFusedAngle(), PIDSourceType.kDisplacement);

        this._angleController = new PIDController(DriveAlongProfile._turnkP, 
                                                    DriveAlongProfile._turnkI,
                                                    DriveAlongProfile._turnkD, 
                                                    DriveAlongProfile._turnkF, 
                                                    this._pidSource, 
                                                    this._buffer);

        Trajectory.Config baseTrajectoryConfig = new Trajectory.Config(DriveAlongProfile._FIT_METHOD,
                                                                        DriveAlongProfile._SAMPLES, 
                                                                        DriveAlongProfile._TIMESTEP, 
                                                                        DriveAlongProfile._MAX_VELOCITY,
                                                                        DriveAlongProfile._MAX_ACCELERATION, 
                                                                        DriveAlongProfile._MAX_JERK);

        Trajectory baseTrajectory = Pathfinder.generate(this._points, baseTrajectoryConfig);

        TankModifier modifier = new TankModifier(baseTrajectory);

        modifier.modify(DriveAlongProfile._TRACK_WIDTH);

        this._leftTrajectory = modifier.getLeftTrajectory();
        this._rightTrajectory = modifier.getRightTrajectory();

        this._leftEncoderFollower = new EncoderFollower(this._leftTrajectory);
        this._rightEncoderFollower = new EncoderFollower(this._rightTrajectory);

    }

    // Called just before this Command runs the first time
    protected void initialize() {       

        this._leftEncoderFollower.configureEncoder((int)Robot.drivetrain.getEncoderCounts(Robot.drivetrain.frontLeftTalon), 
                                                    (int)CONVERSIONS.REVS2TICKS, 
                                                    DRIVETRAIN.WHEEL_DIAMETER * CONVERSIONS.INCHES2METERS);  

        this._rightEncoderFollower.configureEncoder((int)Robot.drivetrain.getEncoderCounts(Robot.drivetrain.frontRightTalon), 
                                                    (int)CONVERSIONS.REVS2TICKS, 
                                                    DRIVETRAIN.WHEEL_DIAMETER * CONVERSIONS.INCHES2METERS);

        this._leftEncoderFollower.configurePIDVA(DriveAlongProfile._drivekP, 
                                                    DriveAlongProfile._drivekI, 
                                                    DriveAlongProfile._drivekD, 
                                                    DriveAlongProfile._VELOCITY_RATIO, 
                                                    DriveAlongProfile._drivekA);

        this._rightEncoderFollower.configurePIDVA(DriveAlongProfile._drivekP, 
                                                    DriveAlongProfile._drivekI, 
                                                    DriveAlongProfile._drivekD, 
                                                    DriveAlongProfile._VELOCITY_RATIO, 
                                                    DriveAlongProfile._drivekA);

        this._angleController.setInputRange(0, 360);
        this._angleController.setContinuous(true);
        this._angleController.setOutputRange(-.5, .5);
        this._angleController.setAbsoluteTolerance(.2);

        this._angleController.enable();
        this._timer.start();
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() 
    {
        double left = this._leftEncoderFollower.calculate((int) Robot.drivetrain.getEncoderCounts(Robot.drivetrain.frontLeftTalon));
        double right = this._rightEncoderFollower.calculate((int) Robot.drivetrain.getEncoderCounts(Robot.drivetrain.frontRightTalon));
        double heading = (this._leftEncoderFollower.getHeading() + this._rightEncoderFollower.getHeading()) / 2;

        this._angleController.setSetpoint(Pathfinder.r2d(heading) % 360);

        if (this._timer.get() >= this._timeout)
        {
            this._done = true;
            return;
        }   
        else if (left == 0 && right == 0 && this._buffer.output == 0)
        {
            this._done = true;
            return;
        }

        left += this._buffer.output;
        right -= this._buffer.output;

        Robot.drivetrain.driveTank(left, right);
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return this._done;
    }

    // Called once after isFinished returns true
    protected void end() 
    {
        Robot.drivetrain.stopMotors();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() 
    {
        this.end();
    }
}

This command is called within a command group called "MidToSwitch"

MidToSwitch:

package org.usfirst.frc.team135.robot.commands.auton.groups;

import org.usfirst.frc.team135.robot.RobotMap;
import org.usfirst.frc.team135.robot.commands.auton.singles.DriveAlongProfile;
import org.usfirst.frc.team135.robot.commands.auton.singles.SetLiftPosition;
import org.usfirst.frc.team135.robot.commands.teleop.ExtendMandibles;
import org.usfirst.frc.team135.robot.commands.teleop.GrabMandibles;

import edu.wpi.first.wpilibj.command.CommandGroup;

/**
 *
 */
public class MidToSwitch extends CommandGroup implements RobotMap{

    private static final double
        SPEED = 1.0,
        TIMEOUT = 5.0;

    public MidToSwitch(boolean switchIsRight) {

        addSequential(new ExtendMandibles());
        addSequential(new SetLiftPosition(COMPETITION.LIFT.SWITCH_POSITION));

        if(switchIsRight)
        {
            addSequential(new DriveAlongProfile(PROFILING.MID_TO_RIGHT_SWITCH, MidToSwitch.TIMEOUT));
        }
        else
        {
            addSequential(new DriveAlongProfile(PROFILING.MID_TO_LEFT_SWITCH, MidToSwitch.TIMEOUT));
        }

        addSequential(new GrabMandibles());

    }
}

This is grouped within another command group that makes the decision of which middle autonomous routine to run.

package org.usfirst.frc.team135.robot.commands.auton.entrypoints;

import org.usfirst.frc.team135.robot.Robot;
import org.usfirst.frc.team135.robot.commands.auton.groups.MidToSwitch;
import org.usfirst.frc.team135.robot.commands.auton.singles.InitializeAngle;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class MiddlePosition extends CommandGroup {

    private static final int
        RIGHT = 0,
        LEFT = 1,
        INVALID = -1;

    public MiddlePosition() {
        int switchPosition = getSwitchPosition(DriverStation.getInstance().getGameSpecificMessage());

        addSequential(new InitializeAngle(270));

        System.out.println(switchPosition);

        if (SmartDashboard.getBoolean("Try to go for Switch?", true) && !SmartDashboard.getBoolean("Try to go for Scale?", false))
        {
            System.out.println(Robot.navx.getFusedAngle());
            if (switchPosition == RIGHT)
            {

                addSequential(new MidToSwitch(true));
            }
            else if (switchPosition == LEFT)
            {
                System.out.println(Robot.navx.initAngle);
                addSequential(new MidToSwitch(false));
            }
            else
            {

            }
        }
        else
        {

        }
    }

    private int getSwitchPosition(String msg)
    {
        if (msg.toUpperCase().charAt(0)  == 'L') //Switch is straight up from us
        {
            return LEFT;
        }
        else if (msg.toUpperCase().charAt(0)  == 'R') //Switch is far away from us
        {
            return RIGHT;
        }
        else
        {
            System.out.println("Unable to get a valid game specific message. Only running autoline.");
            return INVALID;
        }
    }
}

Finally they are added as choosers to the SmartDashboard.

public void robotInit() {
        //Order does matter.

        navx = NavX.getInstance();
        canifier = Canifier.getInstance();
        ultrasonic = UltrasonicSensor.getInstance();
        drivetrain = DriveTrain.getInstance();
        hang = Hang.getInstance();
        intake = Intake.GetInstance();
        lift = Lift.getInstance();
        oi = OI.getInstance();

        //CameraServer.getInstance().startAutomaticCapture();

        m_chooser.addObject("Left Position", new LeftPosition());
        m_chooser.addObject("Middle Position", new MiddlePosition());
        m_chooser.addObject("Right Position", new RightPosition());
        SmartDashboard.putData("Auto mode", m_chooser);

        SmartDashboard.setPersistent("Try to go for Scale?");
        SmartDashboard.setPersistent("Try to go for Switch?");
        SmartDashboard.setPersistent("Prefer Switch or Scale?");

    }
JaciBrunning commented 6 years ago

In the future please post code in a gist, it's very hefty to read in an issue report.

Usually this issue appears if your path is invalid, change your waypoints to make sure they fit within your drivetrain constraints.

Interverse commented 6 years ago

We have the same error and our waypoint is just moving to a waypoint of 5,0,0 from 0,0,0

IshanArora21 commented 5 years ago

I'm having the same error.

cjstepan commented 5 years ago

@JacisNonsense I'm having an issue with this as well. When the robot tries to load the trajectory file, It crashes. Here is my program I am running: https://github.com/FRCTeam3206/2019Subsystems-Java/tree/master/HobbesPathweaver Here is the RioLog Error I am receiving: RioLog Error Pathweaver.txt

Any help is very much appreciated. Thank you!