#!/usr/bin/env python
## Square Navigator
#
# This is the "raw" version of a navigation component to drive ERTS in a square pattern
#
# See local variable nav in main().
# The assignment is to tune nav['steering_sensitivity'] to eliminate
# oversteer/understeer.
#
# Feel free to experiment with other parameters in nav as well, but
# do it one variable at a time
#
import os
import select
import sys
import termios
import tty
import cjson
#
# "Connectivity" mappings to CartFS
#
## ERTS mount point
ROOT = '/tmp/cartfs'
## synchronization point
CLOCK = 'clock'
## vehicle control interface
VCS = 'vcs/vcs_s'
## compass sensor
COMPASS = 'compass/compass_s'
## GPS sensor
GPS = 'gps/gps_s'
## driver interface
DRIVER = 'jdriver/jdriver_s'
## Switch to a raw TTY
#
# Switch terminal mode to "raw" to prevent interuption with CNTL-C (see cartfs.py)
#
def block_ctrl_c():
block_ctrl_c.old_term_settings = termios.tcgetattr(sys.stdin.fileno())
tty.setraw(sys.stdin.fileno())
## Switch out of a raw TTY
#
# Reverses the effect of block_ctrl_c(), enabling CTRL-C interruption
#
def unblock_ctrl_c():
ctrl_c_found = False
if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
capchars = os.read(0, 100)
# sweep the characters and check to see if one is ctrl-c
for c in capchars:
# when we find the crtl-c, set an exit flag to use once
# we restore the terminal settings
if c == '\x03':
ctrl_c_found = True
break
termios.tcsetattr(sys.stdin.fileno(), termios.TCSADRAIN,
block_ctrl_c.old_term_settings)
if ctrl_c_found:
raise KeyboardInterrupt
##Calculate a steering correction
#
# @param[in] compass A dictionary
# @param[in] driver Provides value of direction
#
# \return t The steering correction in the form of an inverse turning radius
#
# \return diff The current heading error, the difference between current and actual headings. A negative
# value indicates that the target is to the left of the current heading; a positive value indicates that the
# target is to the right.
#
# \pre
# - compass['heading'] The actual heading.
# - driver['direction'] The target heading.
#
# Compare the current and target headings to compute a new steering command. The steering control unit
# is the inverse of the turning radius (ITR). For example, to steer in a 3-meter # radius, the steering
# command is ITR = 0.3333...
#
# The steering correction is a new steering radius (as opposed to an increment).
# Its value is proportional to the error, with driver['turn_P_term']
# holding the proportionality factor.
#
# Steering is clipped to a radius held in driver['corner_radius']
#
def calc_inv_turn(compass, nav):
'''Calculate the (signed) difference between the current heading and
the target heading. Negative values indicate the target is to the left
of the current heading. Maximum difference is 180 degrees. The
difference is returned along with a turning control value which is the
inverse of a circle radius for the turn.'''
# Constrain heading to (-180, 180)
if compass['heading'] > 180:
compass['heading'] -= 360.0
# Normalize target heading to (-180, 180)
if nav['target_heading'] > 180:
nav['target_heading'] -= 360
# Find the error between our heading and our desired heading
heading_error = float(compass['heading']) - float(nav['target_heading'])
# Normalize heading error to (-180, 180)
if heading_error > 180.0:
heading_error -= 360.0
elif heading_error < -180.0:
heading_error += 360.0
# Calculate the steering correction
steering_correction = float(nav['steering_sensitivity']) * heading_error
# Get the smallest turn allowed
max_turn = 1 / float(nav['sq_corner'])
# Clip the turn command to (-max_turn, max_turn)
if steering_correction > max_turn:
steering_correction = max_turn
elif steering_correction < -max_turn:
steering_correction = -max_turn
return steering_correction, heading_error
## Square Driver task
#
# @param[in] compass A dictionary representing the status of the compass
# @param[in] vcs A dictionary representing the status of the Vehicle Control Module
# @param[in,out] nav A dictionary representing the driver state
#
#\return turn The desired inverse turning radius command
#\pre
# - compass['heading'] is vehicle's current true heading in degrees relative to magnetic north
# - vcs['distance'] is the cumulative distance travelled since start-up, in meters
# - driver['direction'] is the desired direction of travel (North, South, East, West).
# - driver['turn_state'] is the driver's control state (turn or straight)
# - driver['steering_slop'] compass point at which control moves from turn to straight
#
#\post
# - nav['control'] is the control state of the square-driver,
# turn or straight
# - nav['mark'] marks where vehicle left the last turn. If the vehicle is just
# now leaving a turn, this is reset to the value vsc['distance']
def get_next_turn_radius_inverse(vcs, compass, nav):
'''
Controller for driving in a rounded square, sides of length
2*corner_radius_inv+side_length. Each call gives an inverse radius
steering value.
'''
# Calcuate the next steering command
steering_correction, heading_error = calc_inv_turn(compass, nav)
# Check which part of the square we are on
if nav['control'] == 'turn':
# Check whether we have finished the corner
if abs(heading_error) <= float(nav['steering_slop']):
nav['control'] = 'straight'
nav['mark'] = float(vcs['distance']) + float(nav['sq_side'])
elif nav['control'] == 'straight':
# Check whether we have finished the side
delta_distance = float(nav['mark']) - float(vcs['distance'])
if delta_distance <= 0:
nav['control'] = 'turn'
nav['target_heading'] = (float(nav['target_heading']) - 90) % 360
# Return steering command and heading error
return steering_correction, heading_error
## The component cycle
#
#
def main():
# Change our working directory to the filesystem
os.chdir(ROOT)
# Open up the reads
clock_fd = os.open(CLOCK, os.O_RDONLY)
vcs_fd = os.open(VCS, os.O_RDONLY)
compass_fd = os.open(COMPASS, os.O_RDONLY)
gps_fd = os.open(GPS, os.O_RDONLY)
# Ensure the paths exist for the writes
path = os.path.split(DRIVER)[0]
if path != '' and not os.path.exists(path):
os.makedirs(path)
os.chmod(path, 0777)
# Open up the writes
driver_fd = os.open(DRIVER, os.O_WRONLY + os.O_CREAT, 0666)
## Navigation parameters
nav = {
'mode': 'auto', # autonomous mode
'enable': True, # Driver enabled
'sq_side': 10.0, # meters
'sq_corner': 3.0, # radius in meters
'speed': 50.0, # fixed for this assignment
'steering_slop': 5.0, # degrees
###################
'steering_sensitivity': 0.03 , # TUNE THIS VALUE #
###################
'control': 'turn', # {'turn', 'straight'}
'target_heading': 0.0, # degrees from magnetic north
'mark': 0.0, # starting position in meters
}
tick = 0
while True:
# Wait for the clock
block_ctrl_c()
clock_stat = os.fstat(clock_fd)
unblock_ctrl_c()
# Read the clock
os.lseek(clock_fd, 0, os.SEEK_SET)
clock_json = os.read(clock_fd, clock_stat.st_size)
clock = cjson.decode(clock_json)
# Read the VCS
os.lseek(vcs_fd, 0, os.SEEK_SET)
vcs_json = os.read(vcs_fd, os.fstat(vcs_fd).st_size)
vcs = cjson.decode(vcs_json)
# Read the compass
os.lseek(compass_fd, 0, os.SEEK_SET)
compass_json = os.read(compass_fd, os.fstat(compass_fd).st_size)
compass = cjson.decode(compass_json)
# Read the GPS
os.lseek(gps_fd, 0, os.SEEK_SET)
gps_json = os.read(gps_fd, os.fstat(gps_fd).st_size)
gps = cjson.decode(gps_json)
# Get the steering correction and error
turn_cmd, heading_error = get_next_turn_radius_inverse(vcs, compass, nav)
# Set up command to the vehicle.
driver = {}
driver['clock'] = clock['clock'] # use the clock's tick, no sync check
driver['mode'] = 'auto' # autonomous mode
driver['enable'] = True # this driver enabled
driver['direction'] = 'forward' # {'forward', 'backward'}
driver['percent_throttle'] = nav['speed'] # set to keep speed relatively low
driver['percent_braking'] = 0.0 # brakes off
driver['turn_radius_inverse'] = turn_cmd # new steering command
tick = (tick + 1) % 10
if tick == 0:
print nav['control'], heading_error
# Write the driver status
driver_json = cjson.encode(driver)
os.ftruncate(driver_fd, len(driver_json))
os.lseek(driver_fd, 0, os.SEEK_SET)
os.write(driver_fd, driver_json)
if __name__ == '__main__':
main()