Change robot mass in sim config

frc1418 · Jan 21, 2019 · 563a035 · 563a035
1 parent e63ac19
commit 563a035
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 23 deletions.
diff --git a/robot/components/trajectory_follower.py b/robot/components/trajectory_follower.py
@@ -12,7 +12,7 @@ class TrajectoryFollower:
     # TODO FIND THE REAL VALUES
     WHEEL_DIAMETER = 0.5
     KV = 1.101
-    KA = 0.225  # 0.102
+    KA = 0.164  # 0.102
 
     drivetrain: drive.DifferentialDrive
     navx: navx.AHRS
@@ -27,8 +27,8 @@ def on_enable(self):
         self.left_follower = pf.followers.EncoderFollower(None)
         self.right_follower = pf.followers.EncoderFollower(None)
 
-        self.left_follower.configurePIDVA(1.0, 0, 0, 1 / 10.903, 0)
-        self.right_follower.configurePIDVA(1.0, 0, 0, 1 / 10.903, 0)
+        self.left_follower.configurePIDVA(1.0, 0, 0.1, 1 / 10.903, 1 / 73.220)
+        self.right_follower.configurePIDVA(1.0, 0, 0.1, 1 / 10.903, 1 / 73.220)
 
         self._cofigure_encoders()
 
@@ -44,12 +44,11 @@ def _cofigure_encoders(self):
         self.right_follower.configureEncoder(self.r_encoder.get(), 360, self.WHEEL_DIAMETER)
 
     def is_following(self, trajectory_name):
-        return (self._current_trajectory is not None and
-            self._current_trajectory == trajectory_name)
+        return self._current_trajectory is not None and 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,7 +64,7 @@ def execute(self):
 
         # This is a poor man's P controller
         angle_difference = pf.boundHalfDegrees(desired_heading - gyro_heading)
-        turn = (2.3 * (-1.0 / 80.0) * angle_difference) + (0.05 * (angle_difference - self.last_difference))
+        turn = (1.1 * (-1.0 / 80.0) * angle_difference) + (0.05 * (angle_difference - self.last_difference))
         # turn = 0.7 * (-1.0 / 80.0) * angle_difference
         # turn = 0
 

diff --git a/robot/physics.py b/robot/physics.py
@@ -17,7 +17,7 @@ def __init__(self, physics_controller):
 
         self.drivetrain = tankmodel.TankModel.theory(
             motor_cfgs.MOTOR_CFG_CIM,           # motor configuration
-            110 * units.lbs,                      # robot mass
+            80 * units.lbs,                      # robot mass
             12.75,                              # drivetrain gear ratio
             2,                                  # motors per side
             17.75 * units.inch,                      # robot wheelbase
@@ -42,12 +42,12 @@ def update_sim(self, hal_data, now, tm_diff):
         """
 
         # Simulate the drivetrain
-        lf_motor = hal_data['pwm'][9]['value']
-        rf_motor = hal_data['pwm'][7]['value']
+        lf_motor = hal_data['pwm'][0b00]['value']
+        rf_motor = hal_data['pwm'][0b10]['value']
 
         # Not needed because front and rear should be in sync
-        # lr_motor = hal_data['pwm'][8]['value']
-        # rr_motor = hal_data['pwm'][6]['value']
+        # lr_motor = hal_data['pwm'][0b01]['value']
+        # rr_motor = hal_data['pwm'][0b11]['value']
 
         x, y, angle = self.drivetrain.get_distance(lf_motor, rf_motor, tm_diff)
         self.physics_controller.distance_drive(x, y, angle)

diff --git a/robot/robot.py b/robot/robot.py
@@ -27,10 +27,10 @@ def createObjects(self):
         #   Dig | 0/1
         #   2^1 | Left/Right
         #   2^0 | Front/Rear
-        self.lf_motor = wpilib.Victor(0b00) # =>0
-        self.lr_motor = wpilib.Victor(0b01) # =>1
-        self.rf_motor = wpilib.Victor(0b10) # =>2
-        self.rr_motor = wpilib.Victor(0b11) # =>3
+        self.lf_motor = wpilib.Victor(0b00)  # =>0
+        self.lr_motor = wpilib.Victor(0b01)  # =>1
+        self.rf_motor = wpilib.Victor(0b10)  # =>2
+        self.rr_motor = wpilib.Victor(0b11)  # =>3
 
         self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
                                                          wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))

diff --git a/robot/sim/config.json b/robot/sim/config.json
@@ -7,10 +7,10 @@
       "starting_y": 14
   	},
     "pwm": {
-      "9": "lf_motor",
-      "8": "lr_motor",
-      "7": "rf_motor",
-      "6": "rr_motor"
+      "0": "lf_motor",
+      "1": "lr_motor",
+      "2": "rf_motor",
+      "3": "rr_motor"
     }
   }
 }
diff --git a/robot/trajectory_generator.py b/robot/trajectory_generator.py
@@ -9,6 +9,10 @@
 PICKLE_FILE = os.path.join(os.path.dirname(__file__), TRAJECTORY_DIRECTORY, 'trajectories.pickle')
 
 trajectories = {
+    "diagonal_higher": [
+        pf.Waypoint(0, 0, 0),  # Waypoints are relative to first, so start at 0, 0, 0
+        pf.Waypoint(15, 8, 0)
+    ],
     "diagonal": [
         pf.Waypoint(0, 0, 0),  # Waypoints are relative to first, so start at 0, 0, 0
         pf.Waypoint(15, 5, 0)
@@ -46,8 +50,8 @@ def _generate_trajectories():
             pf.SAMPLES_HIGH,
             dt=0.02,  # 20ms
             max_velocity=10.903,      # These are in ft/sec and
-            max_acceleration=53.251,  # set the units for distance to ft.
-            max_jerk=120
+            max_acceleration=73.220,  # set the units for distance to ft.
+            max_jerk=140
         )[1]  # The 0th element is just info
 
         modifier = pf.modifiers.TankModifier(generated_trajectory).modify(WHEELBASE_WIDTH)