reset_hard() function in resetbot.py

Author: ksiegall

XRPLib/resetbot.py

@@ -1,31 +1,48 @@
 import sys
-import time
 """
 A simple file for shutting off all of the motors after a program gets interrupted from the REPL.
 Run this file after interrupting a program to stop the robot by running "import XRPLib.resetbot" in the REPL.
 """
-print("Stopping all motors and shutting down the robot...")
+print(sys.modules)
 
-if "XRPLib.encoded_motor" in sys.modules:
+def reset_motors():
     from XRPLib.encoded_motor import EncodedMotor
     # using the EncodedMotor since the default drivetrain uses the IMU and takes 3 seconds to init
     for i in range(4):
         motor = EncodedMotor.get_default_encoded_motor(i+1)
         motor.set_speed(0)
         motor.reset_encoder_position()
 
-if "XRPLib.board" in sys.modules:
+def reset_led():
     from XRPLib.board import Board
     # Turn off the on-board LED
     Board.get_default_board().led_off()
 
-if "XRPLib.servo" in sys.modules:
+def reset_servos():
     from XRPLib.servo import Servo
     # Turn off both Servos
     Servo.get_default_servo(1).free()
     Servo.get_default_servo(2).free()
 
-if "XRPLib.webserver" in sys.modules:
+def reset_webserver():
     from XRPLib.webserver import Webserver
     # Shut off the webserver and close network connections
-    Webserver.get_default_webserver().stop_server()
+    Webserver.get_default_webserver().stop_server()
+
+def reset_hard():
+    reset_motors()
+    reset_led()
+    reset_servos()
+    reset_webserver()
+
+if "XRPLib.encoded_motor" in sys.modules:
+    reset_motors()
+
+if "XRPLib.board" in sys.modules:
+    reset_led()
+
+if "XRPLib.servo" in sys.modules:
+    reset_servos()
+
+if "XRPLib.webserver" in sys.modules:
+    reset_webserver()

XRPLib/servo.py

@@ -36,7 +36,8 @@ def __init__(self, signal_pin:int):
 
         self._servo = PWM(Pin(signal_pin, Pin.OUT))
         # Initialize base frequency for the PWM
-        self._servo.freq(50)
+        self._servo.init(freq=50)
+        self._is_init = True
         self.MICROSEC_PER_DEGREE: int = 10000
         self.LOW_ANGLE_OFFSET: int = 500000
 
@@ -46,10 +47,14 @@ def set_angle(self, degrees: float):
         :param degrees: The angle to set the servo to [0,200]
         :ptype degrees: float
         """
+        if not self._is_init:
+            self._servo.init(freq=50)
+            self._is_init = True
         self._servo.duty_ns(int(degrees * self.MICROSEC_PER_DEGREE + self.LOW_ANGLE_OFFSET))
 
     def free(self):
         """
         Allows the servo to spin freely without holding position
         """
-        self._servo.duty_ns(0)
+        self._servo.deinit()
+        self._is_init = False