Change to correct kitbot measurements

requirements.txt

@@ -3,3 +3,4 @@ robotpy-ctre>=2018.0.0
 pynetworktables>=2018.0.0
 pybind11>=2.2.2
 robotpy-pathfinder>=0.2.5
+robotpy-navx>=2018.0.0

robot/autonomous/charge.py

@@ -8,7 +8,7 @@ class Charge(AutonomousStateMachine):
     MODE_NAME = 'Charge'
     DEFAULT = True
 
-    drive = drive.Drive
+    drive: drive.Drive
     follower: trajectory_follower.TrajectoryFollower
 
     @state(first=True)

robot/components/drive.py

@@ -8,7 +8,7 @@
 
 
 class Drive:
-    drivetrain = wpilib.drive.DifferentialDrive
+    drivetrain: wpilib.drive.DifferentialDrive
 
     def __init__(self):
         self.enabled = False

robot/components/trajectory_follower.py

@@ -1,6 +1,6 @@
 import pathfinder as pf
-from robotpy_ext.common_drivers import navx
-from typing import Tuple, List, Dict
+import navx
+from typing import Tuple, List
 import wpilib
 from wpilib import drive
 
@@ -18,7 +18,7 @@ class TrajectoryFollower:
     navx: navx.AHRS
     l_encoder: wpilib.Encoder
     r_encoder: wpilib.Encoder
-    generated_trajectories: Dict
+    generated_trajectories: dict
 
     def on_enable(self):
         self._current_trajectory = None
@@ -48,8 +48,8 @@ def is_following(self, trajectory_name):
             self._current_trajectory == trajectory_name)
 
     def execute(self):
-        if (self.left_follower.trajectory is None or self.right_follower.trajectory is None) or \
-           (self.left_follower.isFinished() and self.right_follower.isFinished()):
+        if ((self.left_follower.trajectory is None or self.right_follower.trajectory is None) or
+           (self.left_follower.isFinished() and self.right_follower.isFinished())):
             self._current_trajectory = None
             return
 
@@ -65,8 +65,8 @@ def execute(self):
 
         # This is a poor man's P controller
         angle_difference = pf.boundHalfDegrees(desired_heading - gyro_heading)
-        # turn = (1.0 * (-1.0 / 80.0) * angle_difference) + (0.05 * (angle_difference - self.last_difference))
-        turn = 0.5 * (-1.0 / 80.0) * angle_difference
+        turn = (2.3 * (-1.0 / 80.0) * angle_difference) + (0.05 * (angle_difference - self.last_difference))
+        # turn = 0.7 * (-1.0 / 80.0) * angle_difference
         # turn = 0
 
         self.last_difference = angle_difference

robot/physics.py

@@ -13,19 +13,16 @@ def __init__(self, physics_controller):
         :param physics_controller: `pyfrc.physics.core.Physics` object
                                        to communicate simulation effects to
         """
-
         self.physics_controller = physics_controller
 
-        # Change these parameters to fit your robot!
-
         self.drivetrain = tankmodel.TankModel.theory(
             motor_cfgs.MOTOR_CFG_CIM,           # motor configuration
             110 * units.lbs,                      # robot mass
             12.75,                              # drivetrain gear ratio
             2,                                  # motors per side
-            24 * units.inch,                      # robot wheelbase
-            25 * units.inch,     # robot width
-            30 * units.inch,     # robot length
+            17.75 * units.inch,                      # robot wheelbase
+            21.63 * units.inch,     # robot width
+            28 * units.inch,     # robot length
             6 * units.inch                        # wheel diameter
         )
 

robot/robot.py

@@ -7,7 +7,7 @@
 from wpilib.buttons import JoystickButton
 from components import drive, intake, trajectory_follower
 import wpilib.drive
-from robotpy_ext.common_drivers import navx
+import navx
 import math
 from trajectory_generator import load_trajectories
 
@@ -91,4 +91,4 @@ def teleopPeriodic(self):
 
 
 if __name__ == '__main__':
-    wpilib.run(Bot, physics_enabled=True)
+    wpilib.run(Bot)

robot/sim/config.json

@@ -1,8 +1,8 @@
 {
   "pyfrc": {
   	"robot": {
-  	  "w": 3,
-  	  "h": 2,
+  	  "w": 2.33,
+  	  "h": 1.80,
       "starting_x": 1.8,
       "starting_y": 14
   	},

robot/trajectory_generator.py

@@ -4,7 +4,7 @@
 import wpilib
 
 
-WHEELBASE_WIDTH = 2  # In feet
+WHEELBASE_WIDTH = 1.479  # In feet
 TRAJECTORY_DIRECTORY = 'trajectories'
 PICKLE_FILE = os.path.join(os.path.dirname(__file__), TRAJECTORY_DIRECTORY, 'trajectories.pickle')
 
@@ -62,8 +62,8 @@ def _generate_trajectories():
 
         renderer = get_user_renderer()
         if renderer:
-            renderer.draw_pathfinder_trajectory(modifier.getLeftTrajectory(), '#0000ff', offset=(-1, 0))
+            renderer.draw_pathfinder_trajectory(modifier.getLeftTrajectory(), '#0000ff', offset=(-0.9, 0))
             renderer.draw_pathfinder_trajectory(modifier.source, '#00ff00', show_dt=True)
-            renderer.draw_pathfinder_trajectory(modifier.getRightTrajectory(), '#0000ff', offset=(1, 0))
+            renderer.draw_pathfinder_trajectory(modifier.getRightTrajectory(), '#0000ff', offset=(0.9, 0))
 
     return generated_trajectories