I want to drive a vehicle in x and y coordinate area with range from (0,0) to (6000,5000). I am using Visual Components (using Python API).
On input I have speed data and steering angle of wheels from Simulink. For example speed of 30 (0.25 m/sec) with the steering angle of wheels from 35 to -35.
I can use something like x = speed * COS( steeringAngle )
and y = speed * SIN( steeringAngle )
while adding up the values of x and y together respectively for each loop.
But the problem is it goes in one direction. If the vehicle turn and comes to area below the value of current x and y. I don't know how to actually get the right x and y value when the turn is so much that it goes to reduced values of x and y. A concept photo is attached. Anyone knows how to solve this?
Here is my code:
Speed= [1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000,1000] #Incoming Speed
Angle= [0,0,0,10,0,0,-20,-20,0,35,35,35,35,35,35,35,35] #Incoming Angle
item = app.findComponent('Ball') #Find component in VC
delay(0.1)
newposition = [10,2500,0,90,0,180] #New position array
x = newposition[0]
y = newposition[1]
z = newposition[2]
w = newposition[3]
p = newposition[4]
r = newposition[5] #wpr is yaw, pitch, roll
Xold=0
Yold=2500
Rold=0 #store x y and r
for s, a in zip(Speed, Angle): #get values from array
delay(1)
print "(Speed, Angle) ->",(s, a)
x=s
y=Yold
r=a #just store
rad=math.radians(a)
x=math.cos(rad)*s
y=math.sin(rad)*s #convert to radians
Xold=Xold+x
Yold=Yold+y
Rold=Rold+r #add to prev value
x=Xold
y=Yold
r=Rold
print x,y,r
mtx = item.PositionMatrix #get position of BALL
mtx.P = vcVector.new(x,y,z) #Position
mtx.WPR = vcVector.new(w,p,r) #Orientation as yaw,pitch,roll
item.PositionMatrix = mtx #store new location
item.update()
app.render()