from ulab import numpy as np from ulab import scipy as spy from machine import Pin, PWM, Timer import random import math from time import ticks_us, ticks_diff, sleep_ms from math import sin, fabs #This code is used to generate distances according to the Levy distribution, and direction according to a Gaussian distribution #sleep_ms(5000) #INPUT parameters: n = 2 # This is the number of iterations of this code you want performed. alpha = 2 #alpha is used to determine the extent of the tail of the Levy distribution. This should be between 0 and 2 #An alpha value of 2 correlates with a Gaussian distribution maxang = 180 # Maximum magnitude of an angle the robot achieves out of 360 fullrot = 2085 #this variable represents the encoder value that corresponds with a full 360 rotation maxtim = 60 # This is the maximum amount of time in seconds each iterations are allowed to run. maxtim2=7 # max time in seconds for levy walk posi values <5000 maxtim3=50# max time in seconds for levy walk posi values <50000 err = 15 #This is the maximum error allowed. The function moverobot will accept values within plus or minus err of the desired position err2 = 8 # error for Levy walk for levy walk posi values <5000 err3 = 15# error for Levy walk for levy walk posi values <50000 ma = 30000 # ma is the maximum power delivered to the robot out of 65535 ma2 = 40000 # ma is the maximum power delivered to the robot out of 65535 for posi <5000 ma3 = 20000 # ma is the maximum power delivered to the robot out of 65535 for posi <50000 scal = 500 #used to scale values from Levy distribution depending on size of environment, 500 should be suffucucent for an average desk lambd = 1.0 # Rate parameter (λ), for an exponential distribution with mean 1/lambd def generate_exponential(lambd): U = random.random() X = -math.log(1 - U) / lambd return X # Function to create a list of values according to a Levy distribution def LevyDist(alpha, n): if alpha <= 0 or alpha > 2: raise ValueError('alpha must be in the range (0, 2)') gamma = [random.uniform(-math.pi / 2, math.pi / 2) for _ in range(n)] gamma = np.array(gamma) W = [generate_exponential(lambd) for _ in range(n)] W = np.array(W) xi_alpha = (np.sin(alpha * gamma) / (np.cos(gamma)**(1/alpha))) * ((np.cos((1 - alpha) * gamma) / W)**((1 - alpha) / alpha)) return xi_alpha # Generating an array of values according to levy distribution Levy_list = LevyDist(alpha, n) Levy_list = scal*Levy_list # Generating a list of random directions between -180 and 180 maxang = (maxang/360) * fullrot direc = [random.uniform(-maxang, maxang) for _ in range(n)] direc = np.array(direc) #Code below is to make robot move in a square direc = np.array([0,fullrot/4, fullrot/4, fullrot/4,fullrot/4]) Levy_list =[500,500,500,500,0] # Motor pins m1dir = 10 m1pwm = 11 m2dir = 13 m2pwm = 12 # Encoder pins enca = 8 # Encoder A encb = 9# Encoder B enc2a =8 enc2b =9 # Setting 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) #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) #Parameters needed for PID controller posi = 0 #this is the variable use to track the position of the wheels 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) # Function to change direction of motors according to signal from PID controller for rotation def set_motor(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 to set both motors to right direction for Levy walk def Levy_walk(dir, pwm_val): m1pwmpin.duty_u16(pwm_val) m2pwmpin.duty_u16(pwm_val) if dir == 1: m1dirpin.value(1) m2dirpin.value(1) elif dir == -1: m1dirpin.value(0) m2dirpin.value(0) else: m1pwmpin.off() m2pwmpin.off() # Main function to execute the Levy walk def moverobot(d): # THis first section rotates the robot according to angles from the uniform distribution global posi, prevT, eprev, eintegral kp = 1000 kd = 0.1 ki = 1000 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: print(f"Skipping iteration {d+1} due to timeout.") m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) break if posi > (int(direc[d]) + err) or posi < (int(direc[d]) - err): e = posi - direc[d] 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_motor(dir, pwr) eprev = e print("direc[d]:", direc[d], "Position:", posi,'iteration', d+1) else: m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) print('correct angle') break sleep_ms(100) if direc[d] != 0: accuracydirec = abs(((abs(posi-direc[d]))/direc[d]))*100 #accuracy here is actually error else: accuracydirec = 100 print('Final position', posi, "Desired position", direc[d]) print('accuracy of direction', accuracydirec) #Setting up PID controller that moves robot according to length from Levy distribution. This section is for posi values less than 5000 posi = 0 prevT = ticks_us() eprev = 0 eintegral = 0 sleep_ms(1000)#resting for a second before ecexuting Levy walk start_time = ticks_us() # Start time for this iteration while True: if abs(Levy_list[d])< 5000: # The PID values have been altered slightly for better performance during longer distances kp = 900 kd = 0.1 ki = 1000 elapsed_time2 = ticks_diff(ticks_us(), start_time) / 1e6 # Calculate elapsed time if elapsed_time2 > maxtim2: print(f"Skipping iteration {d+1} due to timeout.") m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) failedwalks.append(Levy_list[d]) break if posi > (int(Levy_list[d]) + err2) or posi < (int(Levy_list[d]) - err2): e = posi - Levy_list[d] dedt = (e - eprev) / deltaT eintegral += e * deltaT u = kp * e + kd * dedt + ki * eintegral pwr = int(fabs(u)) if pwr > ma2: pwr = ma2 dir = 1 if u > 0 else -1 Levy_walk(dir, pwr) eprev = e print("Levy_list[d]:", Levy_list[d], "distance:", posi, "U",'iteration', d+1) else: m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) print('correct angle') break sleep_ms(100) #check if this is good # for posi values < 50000 elif abs(Levy_list[d])< 50000: kp = 3000 kd = 0.1 ki = 300 elapsed_time2 = ticks_diff(ticks_us(), start_time) / 1e6 # Calculate elapsed time if elapsed_time2 > maxtim3: # Check if the time exceeds 15 seconds print(f"Skipping iteration {d+1} due to timeout.") m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) failedwalks.append(Levy_list[d]) break if posi > (int(Levy_list[d]) + err3) or posi < (int(Levy_list[d]) - err3): e = posi - Levy_list[d] dedt = (e - eprev) / deltaT eintegral += e * deltaT u = kp * e + kd * dedt + ki * eintegral pwr = int(fabs(u)) ma3 = 20000 if pwr > ma3: pwr = ma3 dir = 1 if u > 0 else -1 Levy_walk(dir, pwr) eprev = e print("Levy_list[d]:", Levy_list[d], "distance:", posi, "U",'iteration', d+1) else: m1pwmpin.duty_u16(0) m2pwmpin.duty_u16(0) print('correct angle') break sleep_ms(100) print('Final distance', posi, "Desired distance", Levy_list[d]) if Levy_list[d] != 0: accuracywalk = abs(((abs(posi-Levy_list[d]))/Levy_list[d]))*100 #accuracy here is actually error else: accuracywalk = 100 print('accuracy of direction', accuracywalk) sleep_ms(1000) posi = 0 prevT = ticks_us() eprev = 0 eintegral = 0 return elapsed_time, elapsed_time2, accuracydirec, accuracywalk # List to store the time taken for rotation and Levy walk time_taken_list = [] time_taken_list2 = [] direc_time_pairs = [] Levy_list_time_pairs = [] accuracydirec_list = [] accuracywalk_list = [] failedwalks = [] # list used to check at what distances the code fails for d in range(len(direc)): time_taken, time_taken2, accuracydirec, accuracywalk = moverobot(d) time_taken_list.append(time_taken) time_taken_list2.append(time_taken2) direc_time_pairs.append((time_taken, direc[d])) Levy_list_time_pairs.append((time_taken2, Levy_list[d])) accuracydirec_list.append(100 - accuracydirec)#converting error to accuracy accuracywalk_list.append(100 - accuracywalk)#converting error to accuracy # Calculate the average times taken, and append the averages to the lists if time_taken_list: average_time_taken = sum(time_taken_list) / len(time_taken_list) time_taken_list.append(average_time_taken) else: average_time_taken = None if time_taken_list2: average_time_taken2 = sum(time_taken_list2) / len(time_taken_list2) time_taken_list2.append(average_time_taken2) else: average_time_taken2 = None average_accuracydirec = sum(accuracydirec_list)/len(accuracydirec_list) average_accuracywalk = sum(accuracywalk_list)/len(accuracywalk_list) # Sort the direc_time_pairs list based on time taken in descending order sorted_pairs = sorted(direc_time_pairs, key=lambda x: x[0], reverse=True) sorted_pairs2 = sorted(Levy_list_time_pairs, key=lambda x: x[0], reverse=True) # Print the 5 longest roation times and their corresponding direction values to ensure that PID controller is working as desired and not stalling print("\n \n \n5 Longest Times and Corresponding Direction Values:") for i in range(min(5, len(sorted_pairs))): print(f"Time: {sorted_pairs[i][0]:.2f}s, Direction: {sorted_pairs[i][1]:.2f} degrees") # Print the 5 longest Levy walk times and their corresponding length values to ensure that PID controller is working as desired and not stalling print("\n \n \n5 Longest distances and Corresponding distance Values:") for i in range(min(5, len(sorted_pairs2))): print(f"Time: {sorted_pairs2[i][0]:.2f}s, Distance: {sorted_pairs2[i][1]:.2f}") accuracydirec_list = np.array(accuracydirec_list) accuracywalk_list = np.array(accuracywalk_list) # Print average time print("Average time taken direc:", average_time_taken) print("Average time taken for walk:", average_time_taken2) print("Average Accuracy for direction:", average_accuracydirec) print("Median Accuracy for direction:", np.median(accuracydirec_list)) print("Average Accuracy for walk:", average_accuracywalk) print("Median Accuracy for walk:", np.median(accuracywalk_list)) print("Failed walks", failedwalks)