Add rally.py

rally.py

@@ -0,0 +1,80 @@
+import time
+import math
+import board
+import adafruit_hcsr04
+import pwmio
+import digitalio
+import rotaryio
+from adafruit_motor import motor
+
+# Mathematics
+diameter = 0.047 # 47mm wheel diameter
+circumference = diameter * math.pi
+target_dist = 3
+
+# Encoder constants
+GEAR_RATIO = 50                     # The gear ratio of the motor
+COUNTS_PER_REV = 12 * GEAR_RATIO    # The counts per revolution of the motor's output shaft
+ENCODER_NAMES = ["A", "D"]
+
+# Motor constants
+FREQUENCY = 25000               
+DECAY_MODE = motor.SLOW_DECAY
+
+# Create a digitalinout object for the user switch
+user_sw = digitalio.DigitalInOut(board.USER_SW)
+user_sw.direction = digitalio.Direction.INPUT
+user_sw.pull = digitalio.Pull.UP
+
+# Set the pwm and motor pins
+pwm_a_p = pwmio.PWMOut(board.MOTOR_A_P, frequency=FREQUENCY)
+pwm_a_n = pwmio.PWMOut(board.MOTOR_A_N, frequency=FREQUENCY)
+
+pwm_d_p = pwmio.PWMOut(board.MOTOR_D_P, frequency=FREQUENCY)
+pwm_d_n = pwmio.PWMOut(board.MOTOR_D_N, frequency=FREQUENCY)
+
+mot_right = motor.DCMotor(pwm_a_p, pwm_a_n)
+mot_left = motor.DCMotor(pwm_d_p, pwm_d_n)
+
+mot_right.decay_mode = DECAY_MODE
+mot_left.decay_mode = DECAY_MODE
+
+# Create the encoder objects
+enc_right = rotaryio.IncrementalEncoder(board.ENCODER_A_B, board.ENCODER_A_A, divisor=1)
+enc_left = rotaryio.IncrementalEncoder(board.ENCODER_D_B, board.ENCODER_D_A, divisor=1)
+
+enc_right.position = 0
+enc_left.position = 0
+
+# Set the sensor pins
+sonar = adafruit_hcsr04.HCSR04(trigger_pin=board.TRIG, echo_pin=board.ECHO)
+
+
+
+while sonar.distance < 20:
+    print("I am waiting...", sonar.distance)
+    
+    if sonar.distance >= 20:
+        break
+
+print(">>>>> Starting movement!!!")
+mot_right.throttle = 0.5
+mot_left.throttle = 0.5    
+
+while True:
+    # Find the motor revolutions
+    revleft = enc_left.position / COUNTS_PER_REV
+    revright = enc_right.position / COUNTS_PER_REV
+    
+    distleft = revleft * circumference
+    distright = revright * circumference
+    
+    avg = (distleft + distright) / 2
+
+    print(f"Distance traveled: {avg:.3f} m")
+        
+    if avg >= target_dist:
+        print("Target reached!!!")
+        mot_left.throttle = 0
+        mot_right.throttle = 0
+        break