I have spent a bit of time trying to create Kinematics objects in Python, and I am struggling with how to do so effectively.
I am wondering if anyone has gotten this to work yet, and/ or knows what I am missing. I have been through the documentation Here and looked at the example Here. I have also looked at the help files of each method, but am still doing some things wrong.
But I keep getting stuck. I am starting with a simple 2 wheel tank drive (with the hopes that it will work in sim) just to get my bearings.
Here is the code I have thus far…
import wpilib from wpilib.kinematics import DifferentialDriveKinematics from wpilib.kinematics import DifferentialDriveWheelSpeeds from wpilib.kinematics import ChassisSpeeds import wpilib.drive if wpilib.RobotBase.isSimulation(): is_sim = True import physics import time else: is_sim = False class MyRobot(wpilib.TimedRobot): """Main robot class""" def robotInit(self): """Robot-wide initialization code should go here""" self.lstick = wpilib.Joystick(0) self.rstick = wpilib.Joystick(1) self.l_motor = wpilib.Jaguar(1) self.r_motor = wpilib.Jaguar(2) # Position gets automatically updated as robot moves self.gyro = wpilib.AnalogGyro(1) self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor) self.motor = wpilib.Jaguar(4) self.limit1 = wpilib.DigitalInput(1) self.limit2 = wpilib.DigitalInput(2) self.position = wpilib.AnalogInput(2) self.Kine = DifferentialDriveKinematics(0.381 * 2) if is_sim: self.physics = physics.PhysicsEngine() self.last_tm = time.time() if is_sim: # TODO: this needs to be builtin def robotPeriodic(self): now = time.time() tm_diff = now - self.last_tm self.last_tm = now self.physics.update_sim(now, tm_diff) def autonomousInit(self): """Called when autonomous mode is enabled""" self.timer = wpilib.Timer() self.timer.start() def autonomousPeriodic(self): #wheelSpeeds = wpilib.kinematics.DifferentialDriveWheelSpeeds(2.0, 3.0) #ChassisSpeeds = DifferentialDriveKinematics.toChassisSpeeds(wheelSpeeds) ChassisSpeed = ChassisSpeeds(1.0, 0.0, 0.0) wheelSpeeds = self.Kine.toWheelSpeeds(ChassisSpeed) print(wheelSpeeds) if self.timer.get() < 2.0: self.drive.arcadeDrive(-1.0, 0) else: self.drive.arcadeDrive(0, 0) def teleopPeriodic(self): """Called when operation control mode is enabled""" self.drive.arcadeDrive(self.lstick.getY(), self.lstick.getX()) #self.drive.arcadeDrive(self.lstick.getRawAxis(1), self.lstick.getRawAxis(3)) # Move a motor with a Joystick y = self.rstick.getY() # stop movement backwards when 1 is on if self.limit1.get(): y = max(0, y) # stop movement forwards when 2 is on if self.limit2.get(): y = min(0, y) self.motor.set(y) if __name__ == "__main__": wpilib.run(MyRobot)
And that runs an error…
TypeError: __init__(): incompatible constructor arguments. The following argument types are supported: 1. wpilib.kinematics._kinematics.ChassisSpeeds() Invoked with: 1.0, 0.0, 0.0
I am trying to invoke this only with constants first, but even that is not working. The ultimate goal is to get Odometry working on our mecha drive, but I wanted to start simply.
Thank you for reading,