MarkusXL
Second Officer
United States
Offline
|
Thanks! With a small mod and some settings changes, we now have a code snippet that makes the S1 drive in a sinusoidal driving pattern. It looks eerily cool. With these settings it is crawling rather slow so it's good for a small area. Yank up the Vd and Tm setting to see what happens, but remember you may have to adjust the Compensator down. Not too far down, though, because if compensator = 0.0, no turning at all, it's just a straight line. Increasing Tm should increase the "amplitude" of the sinusoidal driving pattern, and vis versa.
TODO: Merge this with the "Finding Gaps in Blue Tape Line" program to make a smoother more efficient search pattern.
Note - in these last two snippets, we are keeping "Theta Zero", the "direction of translation in degrees", or "T0" in our formula, locked at 0. Just as with Vo, the Turning Factor, we can input T0 to our mecanum function from a result of some computation - I'm thinking a cos() result... I can't even imagine the crazy driving pattern that may result...
from time import sleep
from math import sin, cos, radians, pi, fabs
Tm = 0.2
Vd = 25
sign_changer = -1.0 # This is our tick-tock mechanism for left-right sinusoidal turns
compensator = 0.3 # Adjust turn radii to compensate for speed
def mechanum(Vd, Vo, T0):
V0 = Vd * (sin(radians(T0) + pi / 4) + Vo)
V1 = Vd * (cos(radians(T0) + pi / 4) - Vo)
V2 = Vd * (cos(radians(T0) + pi / 4) + Vo)
V3 = Vd * (sin(radians(T0) + pi / 4) - Vo)
return [V0, V1, V2, V3]
for n in range(28, 200):
circle_1 = (compensator * (1 + sin(radians(n * 10)))) * sign_changer
print(circle_1)
if fabs(circle_1) < .01: # Shifts Turns from Left To Right, mostly! sign_changer = sign_changer * -1.0
print(sign_changer)
V = mechanum(Vd,circle_1,0)
chassis_ctrl.set_wheel_speed(V[0], V[1], V[2], V[3])
print(V)
time.sleep(Tm)
|
|