Changed trajectory code

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2021.1.2"
+    id "edu.wpi.first.GradleRIO" version "2021.3.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/frc/robot/Robot.java

@@ -8,8 +8,13 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.networktables.NetworkTable; 
+import edu.wpi.first.networktables.NetworkTableEntry; 
+import edu.wpi.first.networktables.NetworkTableInstance; 
+
 //import frc.robot.subsystems.Arm;
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -21,17 +26,19 @@ public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
-
+  
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
    */
-
+  
   @Override
   public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    NetworkTableInstance inst = NetworkTableInstance.getDefault();
+    NetworkTable table = inst.getTable("datatable"); 
   }
 
   /**
@@ -105,6 +112,7 @@ public void teleopPeriodic() {
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+    m_robotContainer.returnFinalPose();
   }
 
   /**

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

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
@@ -73,6 +74,10 @@ public RobotContainer() {
   private void configureButtonBindings() {
   }
 
+  public void returnFinalPose() {
+    SmartDashboard.putNumber("Final Pose", drivetrain.getPose().getRotation().getDegrees());
+  }
+
 
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
@@ -81,13 +86,22 @@ private void configureButtonBindings() {
    */
   public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
-    
+
     TrajectoryConfig config = new TrajectoryConfig(Units.feetToMeters(2), Units.feetToMeters(2));
     config.setKinematics(drivetrain.getKinematics());
-
+    
     Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
-      Arrays.asList(new Pose2d(), new Pose2d(1.0, 0, new Rotation2d())),
-      config);
+      Arrays.asList(new Pose2d(),new Pose2d(0, 1.0, new Rotation2d(0))),config);
+    drivetrain.reset(trajectory);
+
+    System.out.println("trajectory after reset: " + trajectory);
+    var transform = drivetrain.getPose().minus(trajectory.getInitialPose()); 
+    System.out.println("transofrm: " + transform);
+    trajectory = trajectory.transformBy(transform); 
+    System.out.println("trajectory: " + trajectory);
+
+    SmartDashboard.putNumber("Initial Rotation", trajectory.getInitialPose().getRotation().getDegrees());
+    SmartDashboard.putNumber("Pose", drivetrain.getPose().getRotation().getDegrees()); 
 
     RamseteCommand command = new RamseteCommand(
       trajectory,
@@ -101,7 +115,9 @@ public Command getAutonomousCommand() {
       drivetrain::setOutput,
       drivetrain
     );
-
+    //drivetrain.reset(trajectory);
     return command;
-  }
+  } 
+
+
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -20,7 +20,7 @@ public class Arm extends SubsystemBase {
    * Creates a new Arm.
    */
   public Arm() {
-
+    armMotor.setInverted(true);
   }
 
   public void turn(XboxController controller) {

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -6,6 +6,7 @@
 /*----------------------------------------------------------------------------*/
 
 package frc.robot.subsystems;
+
 //import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
@@ -16,6 +17,7 @@
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.util.Units;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -25,6 +27,7 @@
 import com.kauailabs.navx.frc.AHRS;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj.SPI;
 
 public class Drivetrain extends SubsystemBase {
@@ -33,28 +36,30 @@ public class Drivetrain extends SubsystemBase {
   CANSparkMax leftMotor = new CANSparkMax(Constants.MOTOR_LEFT_ID, MotorType.kBrushless);
   CANSparkMax rightMotor = new CANSparkMax(Constants.MOTOR_RIGHT_ID, MotorType.kBrushless);
 
+
   //NavX gyroscope
   AHRS gyro = new AHRS(SPI.Port.kMXP);
 
   // how robot need to go
-  DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(26));
+  DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(0.527);
   // how robot positioned
   DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(getHeading());
 
-  SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.4, 1, 0.4);
+  SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.323, 3.71, 0.313);
 
   // how robot turn wheel
-  PIDController leftPIDController = new PIDController(0.1,0.0,0.05);
-  PIDController rightPIDController = new PIDController(0.1,0.0,0.05); 
+  PIDController leftPIDController = new PIDController(1.84,0.0,0.0);
+  PIDController rightPIDController = new PIDController(1.84,0.0,0.0); 
 
-  Pose2d pose;
+  Pose2d pose = new Pose2d();
 
   DifferentialDrive drive = new DifferentialDrive(leftMotor, rightMotor);
   /**
    * Creates a new Drivetrain.
    */
   public Drivetrain() {
-
+    //gyro.calibrate();
+    //gyro.reset(); 
   }
 
   /**
@@ -68,15 +73,18 @@ public void driveWithJoysticks(XboxController controller) {
   public void periodic() {
     // This method will be called once per scheduler run
     pose = odometry.update(getHeading(), (0.0254 * (leftMotor.getEncoder().getPosition() * (1/10.75) * 6 * Math.PI)), (0.0254 * (rightMotor.getEncoder().getPosition() * (1/10.75) * 6 * Math.PI)));
-    SmartDashboard.putNumber("Gyro Angle", -gyro.getAngle()); 
+    SmartDashboard.putNumber("Gyro Angle", -gyro.getAngle());
+    SmartDashboard.putNumber("Left Encoder", leftMotor.getEncoder().getPosition());
+    SmartDashboard.putNumber("Right Encoder", rightMotor.getEncoder().getPosition());
+
   }
 
   /**
    * Returns rotation of the robot
    * @return Angle of the gyroscope on a 2d plane in degrees
    */
   public Rotation2d getHeading() {
-    return Rotation2d.fromDegrees(-gyro.getAngle());
+    return Rotation2d.fromDegrees(0);
   }
 
   /**
@@ -141,7 +149,13 @@ public PIDController getRightPIDController() {
    */ 
   public void setOutput(double leftVolts, double rightVolts) {
     leftMotor.set(leftVolts / 12);
-    rightMotor.set(rightVolts / 12);
+    rightMotor.set( rightVolts / 12);
   }
 
+  public void reset(Trajectory trajectory){
+    //gyro.reset(); 
+    odometry.resetPosition(trajectory.getInitialPose(), getHeading()); 
+    leftMotor.getEncoder().setPosition(0); 
+    rightMotor.getEncoder().setPosition(0);
+  }
 }