Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)
-   -   Pd loop autonomous (http://www.chiefdelphi.com/forums/showthread.php?t=134359)

2386programming 11-02-2015 14:36
Pd loop autonomous
 
Hello, my team is working on the autonomous for our robot. We have an autonomous of driving into the auto zone, and picking up a tote and driving into the auto zone, but we are trying to find a way to do different movements with the drive train in autonomous using the pd loop. We have a mecanum drive and would like to do something like pick up a tote, move back, and then strafe. If any assistance can be given in structure for the code that would be fantastic.

Thank you!

Team 2386

cstelter 11-02-2015 15:18
Re: Pd loop autonomous
 
Are you using command based robot? Simple robot? Iterative Robot? Answers will vary depending on your situation.

Our team has not gotten to actual autonomous code because the build team is behind yet. So far we have only a practice chassis with mecanum. Using Commands we created one command for DriveForTime that takes for its constructor magnitude, direction, rotation, whether or not to use field-centric motion, and a timeout. We have another command for RotateToAngle. With just those commands they can be sequenced to drive straight for 1s, rotate 90 degrees, over and over 4 times such that it is ending up within an inch of its starting position.

We're hoping once we have a bit more of a robot that we can continue to add commands for raising/lowering forks, etc to collect containers or totes.

2386programming 11-02-2015 16:02
Re: Pd loop autonomous
 
We are using an iterative robot.

cstelter 11-02-2015 18:47
Re: Pd loop autonomous
 
Quote:
Originally Posted by 2386programming (Post 1441993)
We are using an iterative robot.
So, with IterativeRobot you are putting your code in

Code:
autonomsousInit()
{
  //stuff to set intial state
}

autonomousPeriodic()
{
    //sequencing
}
I'm probably not the best one to be giving advice as I've never helped a team program in Iterative Robot directly-- only command based.

But my advice would be to set up some sort of rudimentary state machine to keep things sane. You could break each step into a separate function (or better yet as I show below, a different object from a class with a function that makes it run) and then call the correct function based on the phase you are in.

Note, I just typed this code into the browser-- it no doubt has mistakes and is only given as a rough guideline. Hopefully the next 12 posts won't be folks pointing out errors, but that could be entirely possible. It's not intended to be dropped in and run, but just an example to organize a raw Iterative Robot without commands and the scheduler.

Code:

class xxxx extends IterativeRobot {
  int m_state;

  DriveForTime driveTwoSeconds;
  DriveForTime turnLeftOneSecond;
  DriveForTime backupFiveSeconds;
  MoveForksForTime liftTwoSeconds;

autonomsousInit()
{
  //stuff to set intial state
  DriveForTime driveTwoSeconds=new DriveForTime(2.0,0.5,0.5);
  DriveForTime turnLeftOneSecond=new DriveForTime(1.0,-0.5,0.5);
  DriveForTime backupFiveSeconds= new DriveForTIme(5.0,-0.8,-0.8);
  MoveForksForTime liftTwoSeconds= new MoveForksForTime(2.0,0.5);
  m_state=0;
}

class DriveForTime {
  Timer m_timer;
  boolean m_firstTimeThroughRun=true;
  double m_time;
  double m_leftSpeed;
  double m_rightSpeed;

  //constructor-- called when you call new on DriveForTime
  DriveForTime(double time, double leftSpeed, double rightSpeed) {
      m_time=time;
      m_leftSeed=leftSpeed;
      m_rightSpeed=rightSpeed;
  }

  //This will get called once each time thorugh autonomousPeriodic
  public void runPeriodic() {
      driveTrain.tankDrive(m_leftSpeed,m_rightSpeed);
      if(m_firstTimeThroughRun) {
          //start the timer and reset m_firstTimeThroughRun flag
          m_timer.reset();
          m_timer.start();
          m_firstTimeThroughRun=false;
      }
 
      if(m_timer.hasPeriodPassed(m_time)) {
            //we're done, advance state counter
            driveTrain.tankDrive(0,0);
            m_state++;
      }
}

class MoveForksForTime {
  Timer m_timer; 
  boolean m_firstTimeThroughRun=true;  //Allow starting timer first time run() is called
  double m_time;    //how long to run forks
  double m_speed;  //power to run forks

  //constructor-- this will be called when you new a MoveForksForTime
  MoveForksForTime(double time, double speed) {
      m_time=time;
      m_speed=speed;
  }
  public void runPeriodic() {
      forksMotor.set(speed);

      if(m_firstTimeThroughRun) {
          //start the timer and reset m_firstTimeThroughRun flag
          m_timer.reset();
          m_timer.start();
          m_firstTimeThroughRun=false;
      }
 
      if(m_timer.hasPeriodPassed(m_time)) {
            //we're done, advance state
            forksMotor.set(0);
            m_state++;
      }
}



autonomousPeriodic()
{
    //sequencing to drive forward at half power for 2s, turn left for one second and then backup at 80% power for 5s, and lift the forks at half power for 2s.

  if(m_state==0) {
        driveTwoSeconds.runPeriodic();
  } else if (m_state==1) {
        turnLeftOneSecond.runPeriodic();
  } else if (m_state==2) {
        backupFiveSeconds.runPeriodic();
  } else if (m_state==3) {
        liftTwoSeconds.runPeriodic();
  }
}
By using object oriented techniques, one can hide all the timer code that exists and is duplicated in both MoveForksForTime and DriveForTime in a base class requiring you to only write the motor driving mentod.

But this is just a *very* cheap/crude version of what the Scheduler class and the Command class can do for you. Going down the path above is simply reinventing a very substantial wheel that already exists for us. But if you just can't make that jump, and you can understand the above, at least it helps lend order to what otherwise can end up being a nightmare of nested if/then/else code.

I would strongly recommend you consider going to Commands and Subsystems. The above approach can get you by in a pinch and might make more intuitive sense because you're writing all the code to sequence your commands by maintaining the m_state variable.

The Scheduler lets you create unique commands like I did simply above that require certain parts of your robot called subsystems (maybe the drivetrain or a motor and an encoder). The command has predefined phases that the scheduler calls (initialize(), execute(), end(), isFinished(), etc.), and once you fill out those items, you just have to start your command in the scheduler and will take over exclusive rights to the subsystem and operate until it declares it is finished or until another command also needing the subsystem is scheduled.

The Scheduler keeps track of all scheduled commands and keeps them firing each time through teleopPeriodic() or autonomousPeriodic()-- both of those functions become a one line Scheduler.getInstance().run() call.

cstelter 11-02-2015 19:00
Re: Pd loop autonomous
 
Rereading the original post, I realize my example using tankDrive is a bit off the mark, but one can call mecanumDrive and keep track of magnitude, Direction, and rotation instead of leftSpeed and rightSpeed and call mecanum_Polar instead of tankDrive on the RobotDrive class.

Also, I am not sure what the term 'Pd loop' means in your subject. Do you mean PID loop? If so that's a whole different discussion

cad321 11-02-2015 19:41
Re: Pd loop autonomous
 
To clarify the original post, we have been using a custom class for PID at the moment however were only using P and D. When we named it we just called it a PD Loop to avoid confusion amongst our team. At the moment we can make our robot drive a certain distance and stop, but that's it. We can make one clear movement such as drive forward 90inches however are struggling on how to write the logic to allow us to drive 90inches and when we have driven 90 inches, lower an arm (without saying wait X amount of time, then do such and such).

The reason we're using a custom class for our PID is because we have had issues in the past with the included libraries and have used our class without trouble before.

We can upload our code tomorrow once we have access to our robot/equipment again.

cstelter 11-02-2015 20:42
Re: Pd loop autonomous
 
So someting like this?

Code:
class PDLoop  {
  PIDController m_controller;

  private PIDSource source = new PIDSource() {
      public double pidGet() {
            return m_encoder.getDistance();
  }

  private PIDOutput output = new PIDOutput() {
        public void pidWrite(double output) {
            driveTrain.mecanumDrive_Polar(output,0,0);
        }
 
  public PDLoop() {
      m_controller=new PIDController(1.0,0,0.01,source,output);
  }

  public void driveForDistance(double distance)
  {
        m_controller.setSetpoint(distance);
        m_controller.enable();
  }
}
Does your class look something like this?

If so, to go an extra distance you could rewrite the last method as

Code:
  public void driveForDistance(double distance)
  {
        m_controller.setSetpoint(source.pidGet() + distance);
        m_controller.enable();
  }
This way you can call again, and the setpoint will get reset to its current encoder distance plus the distance requested and it will begin moving again.

Allowing strafing to distance would require some extra logic or another class all together. The above class could probably accomodate strafing with minimal changes.

Code:
class PDLoop {
  PIDController m_controller;
  double m_direction=0;

  private PIDSource source = new PIDSource() {
      public double pidGet() {
            //probably need encoder from left front wheel for strafe right signs
            //to work.
            return m_encoder.getDistance();
  }

  private PIDOutput output = new PIDOutput() {
        public void pidWrite(double output) {
            driveTrain.mecanumDrive_Polar(output,m_direction,0);
        }
 
  public PDLoop() {
      m_controller=new PIDController(1.0,0,0.01,source,output);
  }

  private void setDistanceAndEnable(double distance)
  {
        m_controller.setSetpoint(source.pidGet()+distance);
        m_controller.enable();
  }

  public void driveForDistance(double distance)
  {
        //negative distance is same as m_direction=180
        m_direction=0;
        setDistanceAndEnable(distance);
  }

  public void strafeRightForDistance(double distance) {
        //pass in negative distance to strafe left
        m_direction=90;
        setDistanceAndEnable(distance);
  }
}
I'm not at all certain though if the encoder reads horizontal distance the same as forward distance or if it reads either correctly. Still on our to-do list to figure out how encoders behave distance wise with mecanum.

I'm not really sure the above can work-- it's just an approach I'd try given the situation you seem to be in. I think the left front goes forward for either driving forward or strafing right so using that encoder for distance makes sense to me for a strafeRight command.

cstelter 11-02-2015 21:07
Re: Pd loop autonomous
 
OK-- so my first reply talks to how to sequence commands.

The second reply talks about how to set it up to strafe as the original post also asked.

But I just reread the most recent post and this is asking how to sequence after a pid movement.

In your pidClass you can add a method called onTarget() which returns true when the PID has reached its target.

Code:
class PDLoop {
...
public boolean isOnTarget() {
    if(Math.abs(source.pidGet() - m_controller.getSetpoint()) < 1) {
        //within one inch, be done
        return true;
    }
}
You could also disable when you return true too.

Also you may wish to see isOnTarget()==true for say 5 periods or longer to be sure you don't sample as it is overshooting.

Or you could wait until you are both onTarget and the encoder's rate is 0;

Above I provided a way to sequence commands through a simple state counter. I used time for the classe DriveForTime(). DriveForDistance might look something like

Code:
class xxxx extends IterativeRobot {
  ...

  private class DriveForDistance {

      boolean m_firstTimeThrougRun=true;
      double m_distance;

      public DriveForDistance (double distance)
      {
            m_distance=distance;
      }
 
      public void runPeriodic()
      {
            if(m_firstTimeThroughRun) {
                PDLoop.driveForDistance(m_distance);
            }
            if(PDLoop.isOnTarget() && Math.abs(drivetrain.encoder.getRate()) < 0.1) {
                m_state++;             
            }
        }
  }

  DriveForDistance driveNinetyInches;

  public void autonomousInit() {
      driveNinetyInches=new DriveForDistance(90);
      ...
  }

autonomousPeriodic()
{
    //sequencing to drive forward at half power for 2s, turn left for one second and then backup at 80% power for 5s, and lift the forks at half power for 2s.

  if(m_state==0) {
        driveNinetyInches.runPeriodic();
  } else if (m_state==1) {
        turnLeftOneSecond.runPeriodic();
  } else if (m_state==2) {
        backupFiveSeconds.runPeriodic();
  } else if (m_state==3) {
        liftTwoSeconds.runPeriodic();
  }
}

Poseidon5817 12-02-2015 11:45
Re: Pd loop autonomous
 
How we currently do our autonomous is something like this:

Code:
public class Auton {

public int curState = 1;
public Timer timer = new Timer();

public void runAuton() {

    switch(curState) {

        case 1:
            timer.start()
            curState = 2
            break;

      case 2:
          //do something
          if(timer.get() >= sometime) {
              curState = 3;
              timer.stop();
              timer.reset();
              timer.start();
          }
          break;

      case 3:
          //So on for how many states you want
          break;

    }



}

}
Then in your main class you have:

Code:
public class Robot {

    public void autonomousPeriodic() {

        instanceOfAuton.runAuton();

    }

}
This is how we do our state machine, but using an enumeration of AutonState instead of a number.

To do the PD loop you can wire the encoder into a Talon SRX and run it in CAN instead of PWM. From there you can call some methods:

Code:
talon.setFeedbackDevice(FeedbackDevice.QuadEncoder);
talon.changeControlMode(ControlMode.Position) //or speed
talon.setPID(1.0, 0.0 0.01) //PD loop because I is 0
talon.enableControl();
talon.set(somePosition) //Or speed
This way, the talon runs the PID loop internally and you don't have to worry about having a separate encoder object. This is how we are doing the PI loop on our elevator and it is working well so far. Depending on which control mode you are in, the argument passed in the set() method can be a position, speed, voltage, current, or percentage. Percentage is the default -1.0 to 1.0 value.

cstelter 12-02-2015 12:21
Re: Pd loop autonomous
 
Just to be clear-- I've posted a lot of code about an idea about how to manage a state machine in Autonomous, but I don't advocate that. I don't discourage anyone from inventing their own state machine in any way, it can be fun and useful learning experience. But if your goal is to get up and running as quickly as possible with a robust way to manage autonomous, my vote is with RobotBuilder -> Java/C++ using Commands and subsystems.

Someone can correct me if I am wrong-- I don't *think* you even need to organize your motors into subsystems if you don't want to-- you can probably use commands without any calls to requires(mySubsytem). Of course it is then up to you to insure you don't schedule two commands using the same motors or unpredictable results may occur. But there is great benefit in being able to schedule a new command when an event happens only to interrupt a previously scheduled command that needs the same subsystem. Still, I'm pretty sure you can get the benefit of command sequencing without absolutely having to use subsystems. You just may shoot yourself in the foot more easily than with them.

Benefits of Commands:
  • You can sequence as many commands together as you like
  • You can schedule two commands to run in parallel (provided they require separate subsystems).
  • You can group commands together and schedule the full group in sequence by another group.
  • It is a far more robust way to create a sequence of events than any state machine, is well tested by many teams each year, and once you wrap your head around the paradigm, imo is the fastest way to build up extremely complex sequences of autonomous code.
  • prexisting commands like WaitCommand(time) exist that you don't even have to write.
  • Specialied commands like PIDCommand or setpoint command will instantiate nearly fully written from RobotBuilder.
  • The same commands can be easily tied to any joystick button whileHeld, whenPressed, whenRelease, etc. So the same command you write for autonomous (placeTote, liftTote, etc.) can be tied to a button for teleop.

To get up and running using Commands:
  1. You must run call Scheduler.getInstance().run() in your autonomousPeriodic and teleopPeriodic methods.
  2. You must build up your autonomous sequence as a Command group with as many commands sequenced within it as you like, and schedule it to start during AutonomousInit.
  3. Each command needs code for construction, initialization, execution, and how it determines when it is done running. If required, you can also provide code for each time your command ends and each time your command is interrupted by another command.
  4. For Teleop, if you use subsystems, you can assign a default command to the subsystem and the system will automatically schedule those commands to run. If you don't use subsystems (assuming that can actually work as I'm guessing), then it is up to you to schedule all commands you want to run during teleopInit().

Poseidon5817 12-02-2015 12:30
Re: Pd loop autonomous
 
I agree that command based is better, but it can be harder for newer teams/programmers, especially if you are used to using Simple or IterativeRobot. That is the main reason we are using IterativeRobot with no commands this year. It's simpler and we find it easier to adjust and add things, at least until we take some time to test and learn how to use the command based format effectively.

cstelter 12-02-2015 16:22
Re: Pd loop autonomous
 
Quote:
Originally Posted by Poseidon1671 (Post 1442504)
I agree that command based is better, but it can be harder for newer teams/programmers, especially if you are used to using Simple or IterativeRobot. That is the main reason we are using IterativeRobot with no commands this year. It's simpler and we find it easier to adjust and add things, at least until we take some time to test and learn how to use the command based format effectively.
That is a real concern in a 6 week period of time. I still encourage teams to start that way if at all possible. In the long run, I think they can do more with less headaches.

For kids to be successful in 6 weeks (assuming mostly rookies coming in), I question how simple 'SimpleRobot' really is. I would rather teach the kids how to use RobotBuilder which writes a ton of infrastructure code for them. RobotBuilder is the grease that makes programming in command based accessible for even students very new to programming. I realize some dislike the RobotBuilder infrastructure for its propensity to expose elements at the global level that ought to be kept private down in the subsystems, but I can overlook that because my goal as a mentor is to teach the students how to make the robot go and as much java/OO concepts as I can in the process of making the robot go.

Once RobotBuilder has done its thing, one can teach them where they need to go to enhance vanilla Robot Builder (mostly subsytem methods and command execute()), and teach them how Commands and CommandGroups work. Wrapping their head around how to write a loop that does states or even how to deal with an iterative function that has to do different things under different conditions is in some ways just as hard as grasping the object oriented concepts involved with commands, subsystems, schedulers and RobotDrive. In the end, they've learned more toward what real programming is about in today's industry (IMO). They'll be thinking OO and even starting to understand data abstraction, inheritance, and encapsulation even if we don't use those terms a lot. To me that's more valuable than learning how to write a state machine. But that is just one guy's opinion and I can certainly appreciate different perspectives on it.


All times are GMT -5. The time now is 10:52.

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