ardupilot/Tools/autotest/pysim/rover.py

165 lines
5.4 KiB
Python

#!/usr/bin/env python
'''
simple rover simulator core
'''
from aircraft import Aircraft
import util, time, math
from math import degrees, radians, sin, cos, pi, asin
from rotmat import Vector3, Matrix3
class Rover(Aircraft):
'''a simple rover'''
def __init__(self,
frame='',
max_speed=20,
max_accel=30,
wheelbase=0.335,
wheeltrack=0.296,
max_wheel_turn=35,
turning_circle=1.8,
skid_turn_rate=140, # degrees/sec
skid_steering=False):
Aircraft.__init__(self)
self.max_speed = max_speed
self.max_accel = max_accel
self.turning_circle = turning_circle
self.wheelbase = wheelbase
self.wheeltrack = wheeltrack
self.max_wheel_turn = max_wheel_turn
self.last_time = self.time_now
self.skid_steering = skid_steering
self.skid_turn_rate = skid_turn_rate
if self.skid_steering:
# these are taken from a 6V wild thumper with skid steering,
# with a sabertooth controller
self.max_accel = 14
self.max_speed = 4
def turn_circle(self, steering):
'''return turning circle (diameter) in meters for steering angle proportion in degrees
'''
if abs(steering) < 1.0e-6:
return 0
return self.turning_circle * sin(radians(35)) / sin(radians(steering*35))
def yaw_rate(self, steering, speed):
'''return yaw rate in degrees/second given steering_angle and speed'''
if self.skid_steering:
return steering * self.skid_turn_rate
if abs(steering) < 1.0e-6 or abs(speed) < 1.0e-6:
return 0
d = self.turn_circle(steering)
c = pi * d
t = c / speed
rate = 360.0 / t
return rate
def lat_accel(self, steering_angle, speed):
'''return lateral acceleration in m/s/s'''
yaw_rate = self.yaw_rate(steering_angle, speed)
accel = radians(yaw_rate) * speed
return accel
def lat_accel2(self, steering_angle, speed):
'''return lateral acceleration in m/s/s'''
mincircle = self.wheelbase/sin(radians(35))
steer = steering_angle/35
return steer * (speed**2) * (2/mincircle)
def steering_angle(self, lat_accel, speed):
'''return steering angle to achieve the given lat_accel'''
mincircle = self.wheelbase/sin(radians(35))
steer = 0.5 * lat_accel * mincircle / (speed**2)
return steer * 35
def update(self, servos):
# if in skid steering mode the steering and throttle values are used for motor1 and motor2
if self.skid_steering:
motor1 = 2*(servos[0]-0.5)
motor2 = 2*(servos[2]-0.5)
steering = motor1 - motor2
throttle = 0.5*(motor1 + motor2)
else:
steering = 2*(servos[0]-0.5)
throttle = 2*(servos[2]-0.5)
# how much time has passed?
t = self.time_now
delta_time = t - self.last_time
self.last_time = t
# speed in m/s in body frame
velocity_body = self.dcm.transposed() * self.velocity
# speed along x axis, +ve is forward
speed = velocity_body.x
# yaw rate in degrees/s
yaw_rate = self.yaw_rate(steering, speed)
# target speed with current throttle
target_speed = throttle * self.max_speed
# linear acceleration in m/s/s - very crude model
accel = self.max_accel * (target_speed - speed) / self.max_speed
# print('speed=%f throttle=%f steering=%f yaw_rate=%f accel=%f' % (speed, state.throttle, state.steering, yaw_rate, accel))
self.gyro = Vector3(0,0,radians(yaw_rate))
# update attitude
self.dcm.rotate(self.gyro * delta_time)
self.dcm.normalize()
# accel in body frame due to motor
accel_body = Vector3(accel, 0, 0)
# add in accel due to direction change
accel_body.y += radians(yaw_rate) * speed
# now in earth frame
accel_earth = self.dcm * accel_body
accel_earth += Vector3(0, 0, self.gravity)
# if we're on the ground, then our vertical acceleration is limited
# to zero. This effectively adds the force of the ground on the aircraft
accel_earth.z = 0
# work out acceleration as seen by the accelerometers. It sees the kinematic
# acceleration (ie. real movement), plus gravity
self.accel_body = self.dcm.transposed() * (accel_earth + Vector3(0, 0, -self.gravity))
# new velocity vector
self.velocity += accel_earth * delta_time
# new position vector
old_position = self.position.copy()
self.position += self.velocity * delta_time
# update lat/lon/altitude
self.update_position()
if __name__ == "__main__":
r = Rover()
d1 = r.turn_circle(r.max_wheel_turn)
print("turn_circle=", d1)
steer = 0.4*35
speed = 2.65
yrate = r.yaw_rate(steer, speed)
yaccel = r.lat_accel(steer, speed)
yaccel2 = r.lat_accel2(steer, speed)
print yaccel, yaccel2
sangle = r.steering_angle(yaccel, speed)
print steer, sangle
yrate2 = degrees(yaccel / speed)
t = 360.0 / yrate2
c = speed * t
d2 = c / pi
steer2 = degrees(asin(r.wheelbase / (d2 - (r.wheeltrack/2))))
print steer, steer2