So I recently tried porting a PD loop from Vex Code to Python in RMS because I couldn’t seem to find any example code teaching how to do it. With the help of Connor from the former 1814D, I seem to have written what seems like what I think the loop would look like. Due to everyone being on break, I can’t actually test what it does on the robot and I wanted to see if I am in the right direction and what I did wrong. If anybody can either confirm that I am in the right path or help me fix any issues, please help a fellow coder in desperation.
global Auton, DriveStraightTarget, DriveStraightVelocity, DriveStraightPreviousError, ArmPIDTarget, ArmPIDVelocity, ArmPIDPreviousError, TrayPIDTarget, TrayPIDVelocity, TrayPIDPreviousError
def DriveStraight():
while True:
kP = .5 #SUBJECT TO CHANGE ONCE TESTED
kD = .4 #SUBJECT TO CHANGE ONCE TESTED
Error = BLMotor.rotation(RotationUnits.DEG) - DriveStraightTarget
derivative = Error - DriveStraightPreviousError
FRMotor.spin_with_voltage(DirectionType.FWD,(((Error * kP + derivative * kD) * .12) * (DriveStraightVelocity/100)), VoltageUnits.VOLT)
FLMotor.spin_with_voltage(DirectionType.FWD,(((Error * kP + derivative * kD) * .12) * (DriveStraightVelocity/100)), VoltageUnits.VOLT)
BRMotor.spin_with_voltage(DirectionType.FWD,(((Error * kP + derivative * kD) * .12) * (DriveStraightVelocity/100)), VoltageUnits.VOLT)
BLMotor.spin_with_voltage(DirectionType.FWD,(((Error * kP + derivative * kD) * .12) * (DriveStraightVelocity/100)), VoltageUnits.VOLT)
DriveStraightPreviousError = Error
sys.sleep(.02)
def ArmPID():
while True:
kP = .5 #SUBJECT TO CHANGE ONCE TESTED
kD = .4 #SUBJECT TO CHANGE ONCE TESTER
Error = ArmPot.value(RotationUnits.DEG) - ArmPIDTarget
derivative = Error - ArmPIDPreviousError
ArmMotor.spin_with_voltage(DirectionType.FWD,(((Error * kP + derivative * kD) * .12) * (ArmPIDVelocity/100)), VoltageUnits.VOLT)
ArmPIDPreviousError = Error
sys.sleep(.02)
def TrayPID():
while True:
kP = .5 #SUBJECT TO CHANGE ONCE TESTED
kD = .4 #SUBJECT TO CHANGE ONCE TESTED
Error = TrayPot.value(RotationUnits.DEG) - TrayPIDTarget
derivative = Error - TrayPIDPreviousError
TrayMotor.spin_with_voltage(DirectionType.FWD,(((Error * kP + derivative * kD) * .12) * (TrayPIDVelocity/100)), VoltageUnits.VOLT)
TrayPIDPreviousError = Error
sys.sleep(.02)
def StartAuton():
TrayMotor.set_stopping(BrakeType.HOLD)
IntakeLeft.set_stopping(BrakeType.HOLD)
IntakeRight.set_stopping(BrakeType.HOLD)
ArmMotor.set_stopping(BrakeType.HOLD)
BLMotor.set_stopping(BrakeType.HOLD)
FLMotor.set_stopping(BrakeType.HOLD)
FRMotor.set_stopping(BrakeType.HOLD)
TrayMotor.reset_rotation()
IntakeLeft.reset_rotation()
IntakeRight.reset_rotation()
ArmMotor.reset_rotation()
BLMotor.reset_rotation()
FLMotor.reset_rotation()
FLMotor.reset_rotation()
FRMotor.reset_rotation()
def pre_auton():
DriveStraightPreviousError = 0
ArmPIDPreviousError = 0
TrayPIDPreviousError = 0
Inertial.start_calibration()
con.screen.clear_screen()
brain.screen.clear_screen()
brain.screen.draw_image_from_file("MenuPage.png",0,0)
brain.screen.render()
Auton = 'MenuPage'
pass
def autonomous():
DriveStraightProcess = multiprocessing.Process(target=DriveStraight, args=DriveStraightTarget)
ArmPIDProcess = multiprocessing.Process(target=ArmPID, args=ArmPIDTarget)
TrayPIDProcess = multiprocessing.Process(target=TrayPID, args=TrayPIDTarget)
DriveStraightProcess.start()
ArmPIDProcess.start()
TrayPIDProcess.start()
DriveStraightProcess.join()
ArmPIDProcess.join()
TrayPIDProcess.join()
StartAuton()
ArmPIDVelocity = 100 #PUTTING RANDOM NUMBERS FOR ALL OF THIS
ArmPIDTarget = 50
sys.sleep(.02)
TrayPIDVelocity = 100
TrayPIDTarget = 50
sys.sleep(.02)
DriveStraightVelocity = 100
DriveStraightTarget = 1000
sys.sleep(.02)
pass