added test stuff on test mode and added backward in cm

Author: HeathLegge1

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

@@ -182,7 +182,19 @@ public void teleopPeriodic() {
 
   /** This function is called once when test mode is enabled. */
   @Override
-  public void testInit() {}
+  public void testInit() {
+    try{
+      drivebaseIODev.turndegress(1, 90, "left");
+      Thread.sleep(1000);
+      drivebaseIODev.turndegress(1, 90, "right");
+      Thread.sleep(1000);
+      drivebaseIODev.forwardincm(1, 5);
+      Thread.sleep(1000);
+      drivebaseIODev.backwardincm(1,5);
+    }catch(Exception e){
+
+    }
+  }
 
   /** This function is called periodically during test mode. */
   @Override

src/main/java/frc/robot/subsystem/drivebase/drivebase.java

@@ -30,5 +30,8 @@ public Command forwardincm(double velocity, int centermeters) {
   public Command turndegress(double velocity, int angle, String direction){
     return run(() -> io.turndegress(velocity, angle, direction));
   }
+  public Command backwardincm(double velocity, int centermeters) {
+    return run(() -> io.backwardincm(velocity, centermeters));
+  }
 
 }

src/main/java/frc/robot/subsystem/drivebase/drivebaseIO.java

@@ -18,6 +18,7 @@ public default void backward(double velocity){}
   public default void turn(double velocity, String direction){}
   public default void stop(){}
   public default void forwardincm(double velocity, int centermeters){}
+  public default void backwardincm(double velocity, int centermeters){}
   public default void turndegress(double velocity, int angle, String direction){}
 
 }

src/main/java/frc/robot/subsystem/drivebase/drivebaseIODev.java

@@ -76,6 +76,18 @@ public void forwardincm(double velocity, int centermeters){
             System.out.println("not yet");
         }
         stop();
+    }
+    @Override
+    public void backwardincm(double velocity, int centermeters){
+        topleft.getEncoder().setPosition(0);
+        double position = topleft.getEncoder().getPosition();
+        double cimcufarance = Math.PI * WheelDiameterinCM;
+        double rotationstotake = centermeters / cimcufarance;
+        backward(velocity);
+        while (position != rotationstotake) {
+            System.out.println("not yet");
+        }
+        stop();
     }
         @Override
     public void turndegress(double velocity, int angle, String direction){