Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)
-   -   Motion Profiling won't work (http://www.chiefdelphi.com/forums/showthread.php?t=154090)

Glitch159 22-01-2017 14:33

Motion Profiling won't work
 
Hello! So, we are testing motion profiling on a robot, and we are able to feed the points to it, but it doesn't seem to want to actually utilize these points for anything (as in, it is getting the points, but not moving at all). We've made sure that the robot is physically fully functional, so it isn't a hardware problem. Any help would be appreciated. Thanks :cool:

TimTheGreat 22-01-2017 15:20

Re: Motion Profiling won't work
 
Quote:

Originally Posted by Glitch159 (Post 1634807)
Hello! So, we are testing motion profiling on a robot, and we are able to feed the points to it, but it doesn't seem to want to actually utilize these points for anything (as in, it is getting the points, but not moving at all). We've made sure that the robot is physically fully functional, so it isn't a hardware problem. Any help would be appreciated. Thanks :cool:

Something like this is really hard to diagnose without some code. Could you share it?

Ether 22-01-2017 17:02

Re: Motion Profiling won't work
 
Quote:

Originally Posted by Glitch159 (Post 1634807)
we are testing motion profiling on a robot, and we are able to feed the points to it, but it doesn't seem to want to actually utilize these points for anything

Have you carefully followed the detailed steps provided in Section 6 of this document?



Glitch159 23-01-2017 19:24

Re: Motion Profiling won't work
 
First of all, that entire document has been read in it's entirety, and we made sure that we added in everything that was specified that we would need. Second of all, here is the code:

Code:

package org.usfirst.frc.team839.robot.commands;

import org.usfirst.frc.team839.robot.instrumentation;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.FeedbackDevice;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.command.Command;

public class GoodMotionProfileCommand extends Command
{

        CANTalon _talon = new CANTalon(5);
        CANTalon _talonSlave = new CANTalon(4);
       
        //MotionProfileExample _example = new MotionProfileExample(_talon);
        private CANTalon.MotionProfileStatus _status = new CANTalon.MotionProfileStatus();

        boolean isFinished = false;
        double[][] profile;
        public GoodMotionProfileCommand(double[][] points)
        {
                System.out.println("GoodMotionProfileCommand constructor");
                profile = points;
        }

        @Override
        protected void initialize()
        {

                System.out.println("GoodMotionProfileCommand initialize");

                _talon.clearMotionProfileTrajectories();
                _talon.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
                _talon.reverseSensor(false);
               
                _talon.changeControlMode(TalonControlMode.MotionProfile);
//                _talonSlave.changeControlMode(TalonControlMode.Follower);
//                _talonSlave.set(5);
               
                _talon.set(CANTalon.SetValueMotionProfile.Disable.value);
                _talon.changeMotionControlFramePeriod(10);
               
                //feed Trajectory Points
                fill(profile,profile.length);
                _talon.set(CANTalon.SetValueMotionProfile.Enable.value);
//                _talon.processMotionProfileBuffer();
        }

        private void fill(double[][] profile, int totalCnt) {

                System.out.println("GoodMotionProfileCommand fill");
                /* create an empty point */
                CANTalon.TrajectoryPoint point = new CANTalon.TrajectoryPoint();

                for (int i = 0; i < totalCnt; ++i) {
                        /* for each point, fill our structure and pass it to API */
                        point.position = profile[i][0];
                        point.velocity = profile[i][1];
                        point.timeDurMs = (int) profile[i][2];
                        point.profileSlotSelect = 0; /* which set of gains would you like to use? */
                        point.velocityOnly = true; /* set true to not do any position
                                                                                * servo, just velocity feedforward
                                                                                */
                        point.zeroPos = false;
                        if (i == 0)
                                point.zeroPos = true; /* set this to true on the first point */

                        point.isLastPoint = false;
                        if ((i + 1) == totalCnt)
                                point.isLastPoint = true; /* set this to true on the last point  */

                        _talon.pushMotionProfileTrajectory(point);
                }
        }
       

       
        @Override
        protected void execute()
        {
                _talon.getMotionProfileStatus(_status);

                if(_status.hasUnderrun)
                {
                        System.out.println("GoodMotionProfileCommand _status.hasUnderrun");

                        //load points
                        _talon.clearMotionProfileHasUnderrun();;
                        _talon.processMotionProfileBuffer();
                }
               
                if (_status.activePointValid && _status.activePoint.isLastPoint)
                {
                        System.out.println("GoodMotionProfileCommand  _status.activePoint.isLastPoint");
                        _talon.set(CANTalon.SetValueMotionProfile.Hold.value);
                        isFinished = true;

                }
                else
                {
                        instrumentation.process(_status);
                        _talon.set(CANTalon.SetValueMotionProfile.Enable.value);
                        _talon.processMotionProfileBuffer();
                }

        }
        @Override
        protected boolean isFinished()
        {
                return isFinished;
        }

        @Override
        protected void end()
        {
                System.out.println("GoodMotionProfileCommand end");
                _talon.set(CANTalon.SetValueMotionProfile.Hold.value);
                _talon.clearMotionProfileTrajectories();
                _talon.set(CANTalon.SetValueMotionProfile.Disable.value);
                _talon.changeControlMode(TalonControlMode.Voltage);
        }

        @Override
        protected void interrupted()
        {
               
        }
}


ozrien 23-01-2017 23:03

Re: Motion Profiling won't work
 
Did you set the gains?

Screenshot the selftest in the web based config.

What firmware version?

Glitch159 25-01-2017 19:07

Re: Motion Profiling won't work
 
Quote:

Originally Posted by ozrien (Post 1635404)
Did you set the gains?

Screenshot the selftest in the web based config.

What firmware version?

Firmware is up to date, and we added the Feed-forward gains as well as P, I, and D gains. We finally have the motors turning now :) However, there is a new problem: we've had to ramp up the acceleration value to something that sounds unreasonable.


All times are GMT -5. The time now is 21:49.

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