physics.py

from pyfrc.physics.drivetrains import four_motor_drivetrain
import wpilib

class PhysicsEngine:
    
    
    def __init__(self, controller):
        self.controller = controller
        
    def update_sim(self, hal_data, now, tm_diff):
        # Simulate the arm
        
        
        # Simulate the drivetrain
        lf_motor = -hal_data['CAN'][1]['value']/1023
        lr_motor = -hal_data['CAN'][2]['value']/1023
        rf_motor = -hal_data['CAN'][3]['value']/1023
        rr_motor = -hal_data['CAN'][4]['value']/1023
        
        fwd, rcw = four_motor_drivetrain(lr_motor, rr_motor, lf_motor, rf_motor)
        self.controller.drive(fwd, rcw, tm_diff)