Go to Post Go fish, dude. - Bill Gold [more]
Home
Go Back   Chief Delphi > Technical > Programming
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Reply
Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 06-13-2018, 07:14 PM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 10
Tahsin Ahmed is an unknown quantity at this point
Pathfinder returning infinity?

I was using the Pathfinder library to generate and run a profile, and I decided to print out the left and right values that Pathfinder calculates. The robot looks good, but on the console, it prints that the left and right power are infinity? I isolated that the problem does not come from the turning calculations, but instead with the encoderFollower's calculate method. Then I checked my encoder to see if it was the thing that was acting up. It was a little off probably because it wasn't tuned, but not enough such that it would have such a dramatic effect. I also tried to use the DistanceFollower and I got the same results. The profile runs ok, but the values are infinity.
Here is the code in question:
Code:
    public DrivetrainMotionProfileJaciEncoderCommand(PathGenerator path) {
    	leftTraj = path.modifier.getLeftTrajectory(); 
    	rightTraj = path.modifier.getRightTrajectory();
    	maxVelocity = path.maxVelocity;
    }

    protected void initialize() {
    	Robot.drivetrain.resetEncoders(); 
    	Robot.drivetrain.resetGyro();
    	leftFollower = new EncoderFollower(leftTraj);
    	rightFollower = new EncoderFollower(rightTraj);
    	leftFollower.reset();
    	rightFollower.reset();
    	//Wheel diameter in feet
    leftFollower.configureEncoder(Robot.drivetrain.leftBottomMotor.getSelectedSensorPosition(0), RobotMap.DRIVETRAIN_ENCODER_TICKS_PER_REVOLUTION, RobotMap.DRIVETRAIN_WHEEL_DIAMETER / 12);
    	rightFollower.configureEncoder(Robot.drivetrain.rightBottomMotor.getSelectedSensorPosition(0), RobotMap.DRIVETRAIN_ENCODER_TICKS_PER_REVOLUTION, RobotMap.DRIVETRAIN_WHEEL_DIAMETER / 12);
    	leftFollower.configurePIDVA(SmartDashboard.getNumber("Motion Profile P", 0.006), SmartDashboard.getNumber("Motion Profile I", 0), SmartDashboard.getNumber("Motion Profile D", 0.03), 1 / maxVelocity, 0);
    	rightFollower.configurePIDVA(SmartDashboard.getNumber("Motion Profile P", 0.006), SmartDashboard.getNumber("Motion Profile I", 0), SmartDashboard.getNumber("Motion Profile D", 0.03), 1 / maxVelocity, 0);
    }

    protected void execute() {
    	double leftOutput = leftFollower.calculate(Robot.drivetrain.leftBottomMotor.getSelectedSensorPosition(0));
    	double rightOutput = rightFollower.calculate(Robot.drivetrain.rightBottomMotor.getSelectedSensorPosition(0));
    	double gyroHeading = Robot.drivetrain.getGyroAngle();
    	double desiredHeading = Pathfinder.r2d(leftFollower.getHeading());
    	double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);
    	double turn = 0.8 * (-1.0 / 80.0) * angleDifference;
    	Robot.drivetrain.tankDrive(leftOutput + turn, rightOutput - turn);
    	System.out.println("Left Power: " + (leftOutput + turn) + "Right Power: " + (rightOutput - turn));
    }

    protected boolean isFinished() {
    	if(leftFollower.isFinished() && rightFollower.isFinished()){
    		System.out.println("Path has finished");
    		return true; 
    	}else {
    		return false; 
    	}
    }
PathGenerator is a class I made that create the path usingPathfinder when its constructed.
Waypoints:
Code:
Waypoint[] points = new Waypoint[] {
			new Waypoint(0, 3.25, 0),
			new Waypoint(10, 3.25, 0),
			new Waypoint(24, 3.25, 0),
			new Waypoint(27, 6, Pathfinder.d2r(90))
	};
Plus my maxVelocity is 7.813 ft/sec and my max acceleration is 6.104 ft/sec^2.
What causes the infinity? And is it something I should be worrying about?
Reply With Quote
  #2   Spotlight this post!  
Unread 06-13-2018, 09:17 PM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 10
Tahsin Ahmed is an unknown quantity at this point
Re: Pathfinder returning infinity?

Also, this might be important too. The PID values are P-0.006 I-0.0 D-0.03.
Reply With Quote
  #3   Spotlight this post!  
Unread 06-13-2018, 09:31 PM
jdao jdao is offline
Registered User
AKA: Jonathan Dao
FRC #4201 (Vitruvian Bots)
Team Role: Mentor
 
Join Date: Jan 2015
Rookie Year: 2012
Location: United States
Posts: 17
jdao will become famous soon enough
Re: Pathfinder returning infinity?

Try this: https://www.chiefdelphi.com/forums/s...7&postcount=60

The reason why you are probably seeing infinity when you turn is that if you're calculating the angle difference incorrectly, it will apply the turn power opposite of what you want, which will in turn, induce more error and Pathfinder will calculate a larger turn from the resultant encoder reading which ends up spiraling out of control (Your robot will just turn in circles when you start turning).
Reply With Quote
  #4   Spotlight this post!  
Unread 06-14-2018, 03:02 PM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 10
Tahsin Ahmed is an unknown quantity at this point
I tried to add the heading to the desired heading but I still got infinity. There is definitely something wrong with the calculate method of the encoder follower.

Sent from my SM-J327T1 using Tapatalk
Reply With Quote
  #5   Spotlight this post!  
Unread 06-14-2018, 11:06 PM
icemannie's Avatar
icemannie icemannie is offline
Registered User
AKA: John Boyle
FRC #2194 (Fondy Fire)
Team Role: Mentor
 
Join Date: Jul 2016
Rookie Year: 2011
Location: Fond du Lac, WI
Posts: 20
icemannie is an unknown quantity at this point
Re: Pathfinder returning infinity?

These are the distance follower calculations. If you zero Kp, Kd and Ka you will be running on feed forward (Kv) only and the result should always be <1


public double calculate(double distance_covered) {
if (segment < trajectory.length()) {
Trajectory.Segment seg = trajectory.get(segment);
double error = seg.position - distance_covered;
double calculated_value = kp * error + // Proportional
kd * ((error - last_error) / seg.dt) + // Derivative
(kv * seg.velocity + ka * seg.acceleration); // V and A Terms
last_error = error;
heading = seg.heading;
segment++;

return calculated_value;
} else
return 0;
}
Reply With Quote
  #6   Spotlight this post!  
Unread Yesterday, 01:41 PM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 10
Tahsin Ahmed is an unknown quantity at this point
I found out the problem. It was that I used the values that I got in Vannaka's MP generator to generate a profile in Pathfinder. I figured out how to copy the ccvs onto the roboRIO and run it from there. Although it worked the time it took for it to finish was way longer that I expected. I used the velocity view on the generator. Does anyone know how accurate this is. What should I do to make the profile complete faster?


Sent from my SM-J327T1 using Tapatalk
Reply With Quote
  #7   Spotlight this post!  
Unread Yesterday, 06:35 PM
icemannie's Avatar
icemannie icemannie is offline
Registered User
AKA: John Boyle
FRC #2194 (Fondy Fire)
Team Role: Mentor
 
Join Date: Jul 2016
Rookie Year: 2011
Location: Fond du Lac, WI
Posts: 20
icemannie is an unknown quantity at this point
Re: Pathfinder returning infinity?

Look at the csv files for left and right wheels using Excel. The number of rows multiplied by the time increment you have chosen should be the target time to run the profile. You can adjust that using the velocity, accel and jerk settings to get your (realistic) target time.

Then you need to tune the profile using the p, a, d and turn gains.

Try logging command and actual left and right wheel distances, angles and speeds to a csv file and plot the results using Excel to see how well you are tuned and check the results of changes.
Reply With Quote
Reply


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump


All times are GMT -5. The time now is 08:51 AM.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2018, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi