From 16745feeb3f05412f0c582aae7f1c2461cf5a83b Mon Sep 17 00:00:00 2001 From: sullynumber9 Date: Mon, 24 Mar 2025 14:51:51 -0400 Subject: [PATCH 1/5] prelimintary motion testing script in python: --- firmware/RAD/motion-testbench/motion-test.py | 172 +++++++++++++++++++ firmware/RAD/motion-testbench/timer-test.py | 11 ++ 2 files changed, 183 insertions(+) create mode 100644 firmware/RAD/motion-testbench/motion-test.py create mode 100644 firmware/RAD/motion-testbench/timer-test.py diff --git a/firmware/RAD/motion-testbench/motion-test.py b/firmware/RAD/motion-testbench/motion-test.py new file mode 100644 index 00000000..3abbd516 --- /dev/null +++ b/firmware/RAD/motion-testbench/motion-test.py @@ -0,0 +1,172 @@ +# Initializing Params + +import time + +def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceleration, v_i, v_max, start_time): + + set_point = current_pos + steps_to_move + + # dont want to move at all, so save the resources + + if (set_point == current_pos): + print("\n\t\tset point == curernt") + return 0 + # dont want to move at all + else: + + # calculation of the projected time + + projected_time = time_calc(v_i, v_max, acceleration, steps_to_move) + + print("\n\t================================= VELOCITY PRINTING ====================================") + + # taking of the current time + + current_time = time.time() + + time_elapsed = current_time - start_time + print("time elapsed", time_elapsed) + + # if we are going clockwise + + if (cw_enable): + print("cw enable selected") + if (time_elapsed <= projected_time/2): + acceleration = abs(acceleration) + else: + acceleration = -abs(acceleration) + + if (ccw_enable): + print("ccw enable selected") + if (time_elapsed <= projected_time/2): + acceleration = -abs(acceleration) + print("yes") + else: + acceleration = abs(acceleration) + + print("accel", acceleration) + + # calculation of the current velocity + + v_f = v_i + acceleration*(time_elapsed) + + # returning the velocity + + # final velocity shown is less than v_max, return that velocity + # final velocity shown is greater than v_max, return v_max + + print("-v_max", -v_max) + print("v_max", v_max) + + # logic issue here that needs to be fixed + + # something going very wrong with the program that needs to be fixed in here ----- + + + if (cw_enable): + + if (v_f < v_max): + print("v-final", v_f) + print("hi?") + return + + elif (abs(v_f) > v_max): + print("v-final", v_max) + return v_max + + + if (ccw_enable): + if (v_f > -v_max): + print("v-final", v_f) + print("hi?") + return + + elif (abs(v_f) > v_max): + print("v-final", v_max) + return v_max + + + +def time_calc(v_i, v_max, acceleration, steps_to_move): # verified with 1 test case + # calculating the maximum time of the movement + + + # Time to reach the maximum point + + print("\n\t ===================== TIME PRINTING ==========================") + + t_max = abs((v_max - v_i) / acceleration) # 5 + print("tmax", t_max) + + # Time to reach the minimum point (vf = 0, vi = v_max) + + t_min = abs(-v_max / acceleration) # 5 + print("tmin", t_min) + + + # finding the steps of increase' + + steps_increase = v_max*t_max - 0.5*acceleration*(t_max**2) + print("steps inc", steps_increase) + + steps_decrease = v_max*t_min + 0.5*-acceleration*(t_min**2) + print("steps dec", steps_increase) + + # finding the total number of standard steps + + standard_steps = steps_to_move - steps_increase - steps_decrease + print("standard steps", standard_steps) + + # Finding the time at the level value + + t_level = standard_steps / v_max + print("t_level", t_level) + + # add all the time values to get the total projected time + + t_total = t_max + t_min + t_level + print("total time", t_total) + + return t_total + + +current_pos = 0 +steps_to_move = 90 +cw_enable = 1 +ccw_enable = 0 +acceleration = 2 # arbitrary, to be changed later +time_wait = 7 + +# this is assuming a current velocity can be passed in, which, if we are switching to velocity-based PID, could be feesible +v_i = 0 # this will be the initial velocity +v_max = 10 #arbitarry maximum velocity value + +start_time = time.time() + + +#def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceleration, v_i, v_max, start_time): + +print("hi?") + +t = time_calc(v_i, v_max, acceleration, steps_to_move) + +time.sleep(time_wait) + + +v = velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceleration, v_i, v_max, start_time) + +# current areas to look into + +# how it will work if we are moving one direction, then want to start moving in the other drection. Need to stop first + + + + + + + + + + + + diff --git a/firmware/RAD/motion-testbench/timer-test.py b/firmware/RAD/motion-testbench/timer-test.py new file mode 100644 index 00000000..6fbb963d --- /dev/null +++ b/firmware/RAD/motion-testbench/timer-test.py @@ -0,0 +1,11 @@ +import time + +start_time = time.time() + +time.sleep(2) + +end_time = time.time() + +duration = end_time - start_time + +print(duration) \ No newline at end of file From f8d406a2004064db21768c20fc455407d59b5475 Mon Sep 17 00:00:00 2001 From: sullynumber9 Date: Tue, 15 Apr 2025 11:15:30 -0400 Subject: [PATCH 2/5] Push of motion testbench --- firmware/RAD/motion-testbench/motion-test.py | 34 ++++++++++++++------ 1 file changed, 25 insertions(+), 9 deletions(-) diff --git a/firmware/RAD/motion-testbench/motion-test.py b/firmware/RAD/motion-testbench/motion-test.py index 3abbd516..fbfcd91f 100644 --- a/firmware/RAD/motion-testbench/motion-test.py +++ b/firmware/RAD/motion-testbench/motion-test.py @@ -48,7 +48,22 @@ def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceler # calculation of the current velocity - v_f = v_i + acceleration*(time_elapsed) + print("time elapsed ", time_elapsed) + print("proj time", projected_time) + print("left", projected_time - time_elapsed) + + if (time_elapsed >= (projected_time / 2)): + v_peak = v_i + abs(acceleration*(projected_time/2)) + v_f = v_peak + acceleration*(projected_time - time_elapsed) # need to decrease acceleration + + # vi becomes v_peak + print("vf vpeak") + print(v_f) + print(v_peak) + + + else: + v_f = v_i + acceleration*(time_elapsed) # returning the velocity @@ -64,21 +79,22 @@ def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceler if (cw_enable): - - if (v_f < v_max): - print("v-final", v_f) - print("hi?") - return - elif (abs(v_f) > v_max): + if (abs(v_f) > v_max): print("v-final", v_max) + print(v_f) return v_max + elif (v_f < v_max): + print("v-final", v_f) + print("hi?") + return + if (ccw_enable): if (v_f > -v_max): print("v-final", v_f) - print("hi?") + #print("hi?") return elif (abs(v_f) > v_max): @@ -135,7 +151,7 @@ def time_calc(v_i, v_max, acceleration, steps_to_move): # verified with 1 test c cw_enable = 1 ccw_enable = 0 acceleration = 2 # arbitrary, to be changed later -time_wait = 7 +time_wait = 10 # this is assuming a current velocity can be passed in, which, if we are switching to velocity-based PID, could be feesible v_i = 0 # this will be the initial velocity From 14087964deda7d854b3c25ffbf1c17a09376a7b8 Mon Sep 17 00:00:00 2001 From: sullynumber9 Date: Tue, 6 May 2025 18:31:59 -0400 Subject: [PATCH 3/5] Motion Profile Testbench - should be mostly working, mo dify params at line 181 to alter testbench settings --- firmware/RAD/motion-testbench/motion-test.py | 70 +++++++++++++++----- 1 file changed, 52 insertions(+), 18 deletions(-) diff --git a/firmware/RAD/motion-testbench/motion-test.py b/firmware/RAD/motion-testbench/motion-test.py index fbfcd91f..d2e37d84 100644 --- a/firmware/RAD/motion-testbench/motion-test.py +++ b/firmware/RAD/motion-testbench/motion-test.py @@ -31,16 +31,27 @@ def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceler if (cw_enable): print("cw enable selected") + + # checks if we are below the halfway point, if so, make acceleration positive if (time_elapsed <= projected_time/2): acceleration = abs(acceleration) + + # checks if we are beyond the halfway point, if so, make acceleration negative else: acceleration = -abs(acceleration) + + # this makes sense because positive acceleration leads to faster motion. We need to slow down motion at halfway + + # if counterclockwise if (ccw_enable): print("ccw enable selected") + + # If less than half of the time has passed and we are going CCW, NOW we use negative! + # the reason for this is because we are moving in the opposite direction + # if not we use positive if (time_elapsed <= projected_time/2): acceleration = -abs(acceleration) - print("yes") else: acceleration = abs(acceleration) @@ -48,17 +59,31 @@ def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceler # calculation of the current velocity - print("time elapsed ", time_elapsed) - print("proj time", projected_time) - print("left", projected_time - time_elapsed) + print("time elapsed ", time_elapsed) # prints out the current time elapsed + print("proj time", projected_time) # reprints the current projected time + print("left", projected_time - time_elapsed) # prints out the current left time + + # if the elapsed time is greater than the projected time divided by 2 if (time_elapsed >= (projected_time / 2)): - v_peak = v_i + abs(acceleration*(projected_time/2)) - v_f = v_peak + acceleration*(projected_time - time_elapsed) # need to decrease acceleration + + # ACCELERATION MUST DECREASE HERE! + + if (cw_enable): + # Here, acceleration is positive + v_peak = v_i + abs(acceleration*(projected_time/2)) # this should still be positive because it needs to decrease accordingly + + elif (ccw_enable): + # here, acceleration will be negative + v_peak = v_i + -abs(acceleration*(projected_time/2)) # this should still be positive because it needs to decrease accordingly + + v_fixed = acceleration*(time_elapsed-(projected_time/2)) + v_f = v_peak + v_fixed # need to decrease acceleration # vi becomes v_peak print("vf vpeak") print(v_f) + print(v_fixed) print(v_peak) @@ -77,29 +102,36 @@ def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceler # something going very wrong with the program that needs to be fixed in here ----- + # if clockwise is enabled if (cw_enable): + + # absolute value of v_f is greater than v_max if (abs(v_f) > v_max): - print("v-final", v_max) + print("CASE 1") + print("v-final: MEASUREMENT TO RETURN:", v_max) print(v_f) return v_max elif (v_f < v_max): - print("v-final", v_f) + print("CASE 2") + print("v-final: MEASUREMENT TO RETURN:", v_f) print("hi?") - return + return v_f if (ccw_enable): if (v_f > -v_max): - print("v-final", v_f) + print("CASE 3") + print("v-final: MEASUREMENT TO RETURN:", v_f) #print("hi?") - return + return v_f elif (abs(v_f) > v_max): - print("v-final", v_max) - return v_max + print("CASE 4") + print("v-final: MEASUREMENT TO RETURN:", -v_max) + return -v_max @@ -120,7 +152,7 @@ def time_calc(v_i, v_max, acceleration, steps_to_move): # verified with 1 test c print("tmin", t_min) - # finding the steps of increase' + # finding the steps of increase and decrease steps_increase = v_max*t_max - 0.5*acceleration*(t_max**2) print("steps inc", steps_increase) @@ -148,10 +180,12 @@ def time_calc(v_i, v_max, acceleration, steps_to_move): # verified with 1 test c current_pos = 0 steps_to_move = 90 -cw_enable = 1 -ccw_enable = 0 +cw_enable = 0 +ccw_enable = 1 acceleration = 2 # arbitrary, to be changed later -time_wait = 10 +time_wait = 14 #change this to test different times + +# issue: boundary change at the halfway # this is assuming a current velocity can be passed in, which, if we are switching to velocity-based PID, could be feesible v_i = 0 # this will be the initial velocity @@ -175,7 +209,7 @@ def time_calc(v_i, v_max, acceleration, steps_to_move): # verified with 1 test c # how it will work if we are moving one direction, then want to start moving in the other drection. Need to stop first - +# this concern might be irrelevant From d20c37dcbe2f27ff2cc430f25f0ef449ff09194b Mon Sep 17 00:00:00 2001 From: sullynumber9 Date: Sat, 17 May 2025 22:03:36 -0400 Subject: [PATCH 4/5] Refined motion profiling testbench, porting to C incoming --- firmware/RAD/motion-testbench/motion-test.py | 6 ++ .../motion-testbench-refined.py | 83 +++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 firmware/RAD/motion-testbench/motion-testbench-refined.py diff --git a/firmware/RAD/motion-testbench/motion-test.py b/firmware/RAD/motion-testbench/motion-test.py index d2e37d84..f63cb48b 100644 --- a/firmware/RAD/motion-testbench/motion-test.py +++ b/firmware/RAD/motion-testbench/motion-test.py @@ -1,7 +1,13 @@ # Initializing Params +# Note to future me: if you need to return steps instead of the velocity, that's not hard to do, just divide by time. we will work something out + import time + +# do not need cw_enable and ccw_enable, you will always be passed steps + + def velocity_function(current_pos, steps_to_move, cw_enable, ccw_enable, acceleration, v_i, v_max, start_time): set_point = current_pos + steps_to_move diff --git a/firmware/RAD/motion-testbench/motion-testbench-refined.py b/firmware/RAD/motion-testbench/motion-testbench-refined.py new file mode 100644 index 00000000..7565b619 --- /dev/null +++ b/firmware/RAD/motion-testbench/motion-testbench-refined.py @@ -0,0 +1,83 @@ +import time + +# return an instantaneous velocity - currently anticipating to do that + +def time_calc(v_i, v_max, acceleration, steps_to_move): + # steps_to_move is the number of steps in the full motion + + # time based calculations + + t_max = abs((v_max - v_i) / acceleration) # 5 + t_min = abs(-v_max / acceleration) # 5 + + + # finding the steps of increase and decrease (needed later for the total time) + + steps_increase = v_max*t_max - 0.5*acceleration*(t_max**2) + steps_decrease = v_max*t_min + 0.5*-acceleration*(t_min**2) + standard_steps = steps_to_move - steps_increase - steps_decrease + + # Finding the time at the level value + t_level = standard_steps / v_max + t_total = t_max + t_min + t_level + + # return the total time for the motion (need to return velocity at a random instant and comapre against this time?) + + return t_total + +def velocity_function(current_pos, steps_to_move, acceleration, v_i, v_max, start_time): + + set_point = current_pos + steps_to_move + + # dont want to move at all, so save the resources + + if (set_point == current_pos): + return 0 + + else: + + # calculation of the projected time + + projected_time = time_calc(v_i, v_max, acceleration, steps_to_move) + + # taking of the current time + + current_time = time.time() + + time_elapsed = current_time - start_time + + # Checking where we are in the motion (based on current time, that will set if acceleration is positive or negative) + # I think we still will need this + + if (time_elapsed <= projected_time/2): + acceleration = abs(acceleration) + + else: + acceleration = -abs(acceleration) + + # this makes sense because positive acceleration leads to faster motion. We need to slow down motion at halfway + + # if counterclockwise + + # if the elapsed time is greater than the projected time divided by 2 + + if (time_elapsed >= (projected_time / 2)): + + # ACCELERATION MUST DECREASE HERE! + + v_peak = v_i + abs(acceleration*(projected_time/2)) # this should still be positive because it needs to decrease accordingly + + v_fixed = acceleration*(time_elapsed-(projected_time/2)) + v_f = v_peak + v_fixed # need to decrease acceleration + + + else: + v_f = v_i + acceleration*(time_elapsed) + + # returning the velocity + + if (abs(v_f) > v_max): + return v_max + + elif (v_f < v_max): + return v_f \ No newline at end of file From bb7f092133aedf6548949dd547727fe57b18b986 Mon Sep 17 00:00:00 2001 From: sullynumber9 Date: Wed, 21 May 2025 20:13:38 -0400 Subject: [PATCH 5/5] Updated push of refined motion testbench: --- .../motion-testbench/motion-testbench-refined.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/firmware/RAD/motion-testbench/motion-testbench-refined.py b/firmware/RAD/motion-testbench/motion-testbench-refined.py index 7565b619..e90bf1c8 100644 --- a/firmware/RAD/motion-testbench/motion-testbench-refined.py +++ b/firmware/RAD/motion-testbench/motion-testbench-refined.py @@ -25,7 +25,7 @@ def time_calc(v_i, v_max, acceleration, steps_to_move): return t_total -def velocity_function(current_pos, steps_to_move, acceleration, v_i, v_max, start_time): +def velocity_function(current_pos, steps_to_move, acceleration, v_i, v_max, start_time): # moving opposite direction, pass in negative vi set_point = current_pos + steps_to_move @@ -49,11 +49,11 @@ def velocity_function(current_pos, steps_to_move, acceleration, v_i, v_max, star # Checking where we are in the motion (based on current time, that will set if acceleration is positive or negative) # I think we still will need this - if (time_elapsed <= projected_time/2): - acceleration = abs(acceleration) + if (time_elapsed >= projected_time/2): + acceleration = -abs(acceleration) else: - acceleration = -abs(acceleration) + acceleration = abs(acceleration) # this makes sense because positive acceleration leads to faster motion. We need to slow down motion at halfway @@ -61,14 +61,13 @@ def velocity_function(current_pos, steps_to_move, acceleration, v_i, v_max, star # if the elapsed time is greater than the projected time divided by 2 - if (time_elapsed >= (projected_time / 2)): + if (time_elapsed >= (projected_time / 2)): # find an intersection point, not projected time / 2 + # intersection time from up to down still needs to be found # ACCELERATION MUST DECREASE HERE! v_peak = v_i + abs(acceleration*(projected_time/2)) # this should still be positive because it needs to decrease accordingly - - v_fixed = acceleration*(time_elapsed-(projected_time/2)) - v_f = v_peak + v_fixed # need to decrease acceleration + v_f = v_peak + acceleration*(time_elapsed-(projected_time/2)) # need to decrease acceleration else: