Drive distance auto (#6)

* added drive distance command

* added pid control to drive distance auto

* finished driveDistance command

* created TurnToAngle auto command.

* added groups to auto call.

* changed angle kP value.

Co-authored-by: PJ Reiniger <[email protected]>

2020InfiniteRecharge/snobotsim/SnobotSim.json

@@ -17,7 +17,7 @@
                 "com.snobot.simulator:snobot_sim_joysticks:${version_number}",
                 "com.snobot.simulator:snobot_sim_utilities:${version_number}",
 
-                "edu.wpi.first.ntcore:ntcore-jni:${wpilibVersion}:${nativeclassifier}"
+                "edu.wpi.first.ntcore:ntcore-jni:${wpilibVersion}:${nativeclassifier}",
                 "edu.wpi.first.hal:hal-jni:${wpilibVersion}:${nativeclassifier}"
             ],
             "jni": [

2020InfiniteRecharge/src/main/java/frc/robot/RobotContainer.java

@@ -8,14 +8,17 @@
 package frc.robot;
 
 import frc.robot.commands.DriveByJoystick;
+import frc.robot.commands.autonomous.DriveDistance;
 import frc.robot.commands.autonomous.TimedDriveStraight;
+import frc.robot.commands.autonomous.TurnToAngle;
 import frc.robot.subsystems.Chassis;
 import frc.robot.subsystems.ControlPanel;
 import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.ShooterConveyor;
 import frc.robot.subsystems.ShooterIntake;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -60,6 +63,16 @@ public RobotContainer() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return new TimedDriveStraight(chassis, 5, 10);
+    SequentialCommandGroup group = new SequentialCommandGroup();
+    group.addCommands(new DriveDistance(chassis, 100, 1));
+    group.addCommands(new TurnToAngle(chassis, 90, 3));
+    group.addCommands(new DriveDistance(chassis, 50, 1));
+    group.addCommands(new TurnToAngle(chassis, 45, 3));
+    group.addCommands(new DriveDistance(chassis, 25, 1));
+
+    return group;
+    //return new TimedDriveStraight(chassis, 5, 10);
+    //return new DriveDistance(chassis, 100, 1);
+    //return new TurnToAngle(chassis, 90, 5);
   }
 }

2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/AutoShoot.java

@@ -4,9 +4,10 @@
 
 public class AutoShoot extends CommandBase {
 
+
 	public AutoShoot() {
 		// Use requires() here to declare subsystem dependencies
-		//super.addRequirements(Shooter); When a subsystem is written, add the requires line back in.
+        //super.addRequirements(Shooter); When a subsystem is written, add the requires line back in.
 	}
 
     public void initialize(){

2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/DriveDistance.java

@@ -0,0 +1,59 @@
+package frc.robot.commands.autonomous;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Chassis;
+
+public class DriveDistance extends CommandBase {
+
+    Chassis chassis;
+
+    private double m_distance;
+    private double m_allowableError;
+    private double m_error;
+
+    private double AUTO_KP = 0.1;
+
+	public DriveDistance(Chassis chassis, double distance, double allowableError) {
+		// Use requires() here to declare subsystem dependencies
+        //super.addRequirements(Shooter); When a subsystem is written, add the requires line back in.
+        this.chassis = chassis;
+
+        m_distance = distance + chassis.getAverageEncoderDistance();
+        m_allowableError = allowableError;
+	}
+
+    public void initialize(){
+    }
+
+	// Called repeatedly when this Command is scheduled to run
+	public void execute() { 
+        double currentDistance;
+
+        currentDistance = chassis.getAverageEncoderDistance();
+
+        m_error = m_distance - currentDistance;
+
+        double speed = m_error * AUTO_KP;
+
+        chassis.setSpeed(speed);
+
+        //System.out.println("error:" + m_error + "speed:" + speed);
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	public boolean isFinished() {
+        if(Math.abs(m_error) < m_allowableError){
+            System.out.println("Done!");
+            return true;
+        }
+        else {
+            System.out.println("drive to distance" + "error:" + m_error + "allowableError" + m_allowableError);
+            return false;
+        }
+	}
+
+	// Called once after isFinished returns true
+	public void end(boolean interrupted) {
+        chassis.setSpeed(0);
+  }
+}

2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/TurnToAngle.java

@@ -0,0 +1,59 @@
+package frc.robot.commands.autonomous;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Chassis;
+
+public class TurnToAngle extends CommandBase {
+
+    Chassis chassis;
+
+    private double m_angle;
+    private double m_allowableError;
+    private double m_error;
+
+    private double AUTO_KP = 0.05;
+
+	public TurnToAngle(Chassis chassis, double angle, double allowableError) {
+		// Use requires() here to declare subsystem dependencies
+        //super.addRequirements(Shooter); When a subsystem is written, add the requires line back in.
+        this.chassis = chassis;
+
+        m_angle = angle + chassis.getHeading();
+        m_allowableError = allowableError;
+	}
+
+    public void initialize(){
+    }
+
+	// Called repeatedly when this Command is scheduled to run
+	public void execute() { 
+        double currentAngle;
+
+        currentAngle = chassis.getHeading();
+
+        m_error = m_angle - currentAngle;
+
+        double turnSpeed = m_error * AUTO_KP;
+
+        chassis.setSpeedAndSteer(0, turnSpeed);
+
+        //System.out.println("error:" + m_error + "speed:" + speed);
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	public boolean isFinished() {
+        if(Math.abs(m_error) < m_allowableError){
+            System.out.println("Done!");
+            return true;
+        }
+        else {
+            System.out.println("Turn to angle" + "error:" + m_error + "allowableError" + m_allowableError);
+            return false;
+        }
+	}
+
+	// Called once after isFinished returns true
+	public void end(boolean interrupted) {
+        chassis.setSpeed(0);
+  }
+}