OCCRA
Go to Post I can't think of a match where we didnt do the ritual. We always called it... "Deploying the code" - Briansmithtown [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: 14
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: 14
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: 23
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: 14
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: 24
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 06-18-2018, 01:41 PM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 14
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 06-18-2018, 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: 24
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
  #8   Spotlight this post!  
Unread 06-20-2018, 08:09 PM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 14
Tahsin Ahmed is an unknown quantity at this point
Re: Pathfinder returning infinity?

Well, I've run into a few more problems. First of all, when tunning the PIDs I found that using 0 works the best. I know that PID values should be low since this is motion profiling, but still is PID values of 0 normal. Anything higher offshoots the path. Another thing is with the velocity ratio. When I use 1 / max velocity like it says on the Pathfinder guide the turns in my paths happen much later than expected. However, if I give it another value in place of max velocity in that expression, it works much better. In fact, I was able to sort of tune it to a point where the path looks almost like the one I expect. Is this normal? What am I doing wrong?
Reply With Quote
  #9   Spotlight this post!  
Unread 06-20-2018, 11:44 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: 23
jdao will become famous soon enough
Re: Pathfinder returning infinity?

What value are you using for your period? Typically, the most common value I've found is 0.05, but there have been some examples that use 0.005. When originally implementing Pathfinder for my own team, I used 0.005, which caused us to use heavily over-inflated values before Pathfinder would behave 'properly'. For example we stuck 400 yards/second as our velocity before our robot would follow the path at close to full power (Using 'real' values had the robot follow the path, but at < 50% power). Switching to a period of 0.05 seconds solved this problem.

I think this problem occurs due to the period being far much faster than WPILib's scheduler. The scheduler runs at loop once every ~20ms (Note that this is not a guarantee rate at can fluctuate slightly, but most people can ignore this), so being too fast throws it off somehow. If you really want to, you can use a Notifier guarantee that the path will be read at the specific period you set it to (You can see our implementation here). Even if you don't decide to run at a faster rate, using a notifier helps the robot run a path far more consistently than if you didn't since the notifier guarantees that the read and execution of the path occurs at your set interval rather than being beholden to random variance.
Reply With Quote
  #10   Spotlight this post!  
Unread 06-21-2018, 04:39 AM
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: 24
icemannie is an unknown quantity at this point
Re: Pathfinder returning infinity?

You should be able to get a path running reasonably in the Command you are using with a .02 period using Timed Robot.

What kind of drive is on the robot? What wheels and wheelbase width and how did you arrive at it? What top speed? Are you being consistent in your units? (Distance Follower makes that part easier.)
Your code uses gyroHeading. Is that 0 - 360 or +/-180

With 0 for PID values you are running on feedforward only.

Robot Casserole has a Simple CSV Logger on GitHub.

https://github.com/RobotCasserole173...CsvLogger.java
Reply With Quote
  #11   Spotlight this post!  
Unread 06-21-2018, 06:08 AM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 14
Tahsin Ahmed is an unknown quantity at this point
I've been using 0.01 as by period. Besides that I'm pretty sure I've been consistent with by values. I had a max velocity of 12 ft/sec and a max acceleration of of 7 ft/sec/sec. I'll try reduce the period then implement the notifier.

Sent from my SM-J327T1 using Tapatalk
Reply With Quote
  #12   Spotlight this post!  
Unread 06-21-2018, 10:53 AM
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: 24
icemannie is an unknown quantity at this point
Re: Pathfinder returning infinity?

The period must be the same as the frequency with which you process the trajectory. So if you are running commands at 20 ms, your period must be .02.
Reply With Quote
  #13   Spotlight this post!  
Unread 06-21-2018, 12:18 PM
Tahsin Ahmed Tahsin Ahmed is offline
Registered User
FRC #0694
 
Join Date: Jan 2018
Location: New York
Posts: 14
Tahsin Ahmed is an unknown quantity at this point
So basically I could run the trajectory at 10 ms so long as I set the notifier to run at 0.01 sec. What is the benefit of having a shorter period. I would assume that everything is more accurate because there are more points, but more points mean more calculations, so wouldn't that make it slow too?

Sent from my SM-J327T1 using Tapatalk
Reply With Quote
  #14   Spotlight this post!  
Unread 06-21-2018, 02:44 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: 23
jdao will become famous soon enough
Re: Pathfinder returning infinity?

Smaller periods allow you to be more accurate, but doesn't necessarily make it slower to calculate. All of the heavy calculation is done when you generate the path and store it to a .csv file. The calculate portion of reading a trajectory takes the ideal values of where it should be/how fast it should be going and then adjusts the output based on your encoder as feedback to take into account variables from the real world.

The upper limit of how fast you could go would probably end up being how fast your encoder can update/how fast you can read that value.

The only other downside I can see of being 'too accurate' would be smaller variations in following your path from the real world would cause your robot to react more aggressively to maintain that path.
Reply With Quote
  #15   Spotlight this post!  
Unread 06-22-2018, 10:57 AM
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: 23
jdao will become famous soon enough
Re: Pathfinder returning infinity?

Another note about making paths with smaller periods: Although it may not take a lot of time to process on the robot, it will take a lot of time processing off the robot. Most people wont really feel this effect, but if you really want to be competitive, decreasing the time it takes to make the paths does make a difference at a competition for tuning your paths rapidly. For example, at Champs you get 10 minutes on the practice field before you have to requeue, which can take anywhere from 30 minutes to over an hour depending on how many teams are at the practice fields. Being able to make effective use of that time means you need to minimize time spend doing code redeploys and waiting around for code changes is wasteful if you don't need to. This is something I realized this year in using pathfinder, where our team was able to rapidly iterate our autos by running them up to 15 times in 10 minutes, whereas most other teams were only doing a handful in that same time period. It's also something I'm really excited about improving for my team in 2019 by moving more things outside of the base robot code to minimize redeploys.
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 09:00 PM.

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