from machine import Pin, ADC, PWM from time import ticks_us, ticks_diff, sleep_ms import time import random from ulab import numpy as np from ulab import scipy as spy import math from math import sin, fabs #THe code below executes an algorithm, that detects a gradient using the IR sensors and moves the robot accordingly #time.sleep(5) #INPUT parameters: fullrot = 2085 pwm_val = 20000 maxtim=5 err = 5 ma = 20000 x = 50 #x here is an error margin. This ensures the gradient has absolutely changed and that a new direction must be chosen history_length = 10 # Number of readings to store for averaging to detect when average gradient changes #Motor Vcc pins set up m1vcc = 0 m2vcc = 7 m1vccpin = Pin(m1vcc, Pin.OUT) m2vccpin = Pin(m2vcc, Pin.OUT) m1vccpin.value(1) m2vccpin.value(1) #Vcc pin set up for IR sensors. Cannot set this to max 65535 or sensors misbehave vcc = PWM(Pin(21), 10000) vcc.duty_u16(65000) vcc2 = PWM(Pin(22), 10000) vcc2.duty_u16(65000) # Motor pins m1dir = 10 m1pwm = 11 m2dir = 13 m2pwm = 12 # Encoder pins enca = 8 # Encoder A encb = 9# Encoder B enc2a =8 enc2b =9 # Set up pins enca_pin = Pin(enca, Pin.IN) encb_pin = Pin(encb, Pin.IN) freqq = 10000 m1pwmpin = PWM(Pin(m1pwm), freq=freqq) m2pwmpin = PWM(Pin(m2pwm), freq=freqq) m1dirpin = Pin(m1dir, Pin.OUT) m2dirpin = Pin(m2dir, Pin.OUT) # Setting up IRsensor pins ir_sensor1 = ADC(Pin(28)) # Analog input for IR sensor 1 ir_sensor2 = ADC(Pin(26)) # Analog input for IR sensor 2 # Setting up lists ir_values1 = [] ir_values2 = [] #PID parameters posi = 0 prevT = ticks_us() eprev = 0 eintegral = 0 # Function to read encoder and add or subtract from posi def read_encoder(pin): global posi if encb_pin.value() > 0: posi += 1 else: posi -= 1 # Attach interrupt enca_pin.irq(trigger=Pin.IRQ_RISING, handler=read_encoder) #Choosing desired color black = 1 white = 0 color = black # Motor control functions def move_forward(): m1pwmpin.duty_u16(pwm_val) m2pwmpin.duty_u16(pwm_val) m1dirpin.value(1) m2dirpin.value(1) def move_backward(): m1pwmpin.duty_u16(pwm_val) m2pwmpin.duty_u16(pwm_val) m1dirpin.value(0) m2dirpin.value(0) def stop_motors(): m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) #This section is for recalibrating and finding the optimal direction to move in def rotate_robot(): global posi, dominant, prevT, eprev, eintegral dir = random.uniform((-1*fullrot/2), fullrot/2) posi = 0 moverobot(dir) #get the sensor readings ir_value1 = ir_sensor1.read_u16() ir_value1 = ((ir_value1 - minir1)/ir1_diff)*scale ir_value2 = ir_sensor2.read_u16() ir_value2 = ((ir_value2 - minir2)/ir2_diff)*scale if ir_value1 > ir_value2: dominant = 1 move_forward() else: dominant = 2 move_backward() posi = 0 prevT = ticks_us() eprev = 0 eintegral = 0 def average_last_n_values(values, n): return sum(values[-n:]) / n def set_motor2(dir, pwm_val): m1pwmpin.duty_u16(pwm_val) m2pwmpin.duty_u16(pwm_val) if dir == 1: m1dirpin.value(0) m2dirpin.value(1) elif dir == -1: m1dirpin.value(1) m2dirpin.value(0) else: m1pwmpin.off() m2pwmpin.off() #Function calibrates the IR sensors of the robot def calibrate_robot(): pmw_val = 10000 #slow rotations for stable values ir_values1 = [] ir_values2 = [] global posi posi = 0 while abs(posi) < fullrot: #get the sensor readings ir_value1 = ir_sensor1.read_u16() ir_value2 = ir_sensor2.read_u16() ir_values1.append(ir_value1) ir_values2.append(ir_value2) #rotate m1pwmpin.duty_u16(pwm_val) m2pwmpin.duty_u16(pwm_val) m1dirpin.value(0) m2dirpin.value(1) print('posi',posi) sleep_ms(10) stop_motors() maxir1 = max(ir_values1) minir1 = min(ir_values1) maxir2 = max(ir_values2) minir2 = min(ir_values1) ir1_diff = maxir1 - minir1 ir2_diff = maxir2 - minir2 if color == white: scale = -1000 elif color == black: scale = 1000 ir_values1 = [] ir_values2 = [] return maxir1, minir1, maxir2, minir2, ir1_diff, ir2_diff, scale maxir1, minir1, maxir2, minir2, ir1_diff, ir2_diff, scale = calibrate_robot() #Code that moves robot to desired posi def moverobot(max_posi): global posi, prevT, eprev, eintegral kp = 3000 kd = 0.1 ki = 300 currT = ticks_us() deltaT = ticks_diff(currT, prevT) / 1e6 prevT = currT start_time = ticks_us() # Start time for this iteration while True: elapsed_time = ticks_diff(ticks_us(), start_time) / 1e6 # Calculate elapsed time if elapsed_time > maxtim: # Check if the time exceeds 15 seconds print(f"Skipping iteration due to timeout.") break # Skip this iteration if time exceeds 15 seconds if posi > (max_posi + err) or posi < (max_posi - err): e = posi - max_posi dedt = (e - eprev) / deltaT eintegral += e * deltaT u = kp * e + kd * dedt + ki * eintegral pwr = int(fabs(u)) if pwr > ma: pwr = ma dir = 1 if u > 0 else -1 set_motor2(dir, pwr) eprev = e print("Desired", max_posi, "Position:", posi, "U") else: m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) print('correct angle') break sleep_ms(10) print('Final position', posi, "Desired position", max_posi) accuracy = abs(((abs(posi-max_posi))/max_posi))*100 print('accuracy', accuracy) posi = 0 prevT = ticks_us() eprev = 0 eintegral = 0 return elapsed_time, accuracy # Return the time taken for this iteration # Main loop rotate_robot() while True: # Read the analog values from both IR sensors ir_value1 = ir_sensor1.read_u16() ir_value1 = ((ir_value1 - minir1)/ir1_diff)*scale ir_value2 = ir_sensor2.read_u16() ir_value2 = ((ir_value2 - minir2)/ir2_diff)*scale # Append the current values to the lists ir_values1.append(ir_value1) ir_values2.append(ir_value2) if len(ir_values1)>= history_length: # Keep only the last `history_length` values if len(ir_values1) > history_length: ir_values1.pop(0) if len(ir_values2) > history_length: ir_values2.pop(0) # Calculate the average of the last `history_length` readings avg_ir_value1 = average_last_n_values(ir_values1, history_length) avg_ir_value2 = average_last_n_values(ir_values2, history_length) print("Avg IR1:", avg_ir_value1, "Avg IR2:", avg_ir_value2,"R1:", ir_value1, "R2:", ir_value2) # For debugging # Check if the current average value is greater than the previous average value if dominant == 1: if avg_ir_value1 < avg_ir_value2 - x: rotate_robot() ir_values1 = [] ir_values2 = [] elif dominant == 2: if avg_ir_value2 < avg_ir_value1 - x: rotate_robot() ir_values1 = [] ir_values2 = [] time.sleep(0.01) # Small delay to reduce noise in readings else: continue