MarkusXL
lvl.4
United States
Offline
|
By adding a cosine function to input number streams into the Direction of Translation argument "Theta Zero" of our mecanum function, truly non-linear movement is acheived.
Even at painfully slow speeds, the S1 now appears to "drift" and "slide" through turns. I'm tickled pink. This is a trip to behold.
Adjust all the variables at the top to play with this expanded and cleaned up code snippet.
TODO: Add yet another math function to tweak the Max Wheel RPM argument Vd during the loop. Now we're talking crazy, as the value of Vd can be negative, to go backwards. Maybe the S1 can move like a Spriograph thingy...
from time import sleep
from math import sin, cos, radians, pi, fabs
Vd = 25 # Init and set Max Wheel RPM (approximately)
Vo = 0 # Init Translation Heading Vo
T0 = 0 # Init Translation Angle Theta Zero
Tm = 0.2 # Init Time Delay between loops
sign_changer1 = -1.0 # Tick tock for Heading Angle Vo
sign_changer2 = -1.0 # Tick tock for Lateral sliding T0
start1 = 30 # Start heading count (try 0 and see for yourself)
stop1 = 230 # Stop heading count (the bigger the merrier)
trfact = 0.3 # Turn Radius Factor
ltfact = 10 # Lateral Translation Factor
def mecanum(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(start1, stop1):
Vo = (trfact * (1 + sin(radians(n * 10)))) * sign_changer1
T0 = (ltfact * (1 + cos(radians(n * 10)))) * sign_changer2
if fabs(Vo) < .01:
sign_changer1 = sign_changer1 * -1.0
print(sign_changer1)
if fabs(T0) > 45:
sign_changer2 = sign_changer2 * -1.0
print(sign_changer2)
print(Vo, T0)
V = mecanum(Vd, Vo, T0)
chassis_ctrl.set_wheel_speed(V[0], V[1], V[2], V[3])
print(V[0], V[1], V[2], V[3])
sleep(Tm)
|
|