diff --git a/carInterface4wd.py b/carInterface4wd.py deleted file mode 100644 index 86734df..0000000 --- a/carInterface4wd.py +++ /dev/null @@ -1,209 +0,0 @@ -from typing import Any, List, Tuple -import math -import time -import vrep # type: ignore - - -class Tripwire(object): - """Abstraction object for the tripwire, providing intuitive functions for the timer flag - """ - def __init__(self, vrep_interface: Any, name: str = 'Proximity_sensor') -> None: - self.vr = vrep_interface - self.handle = self.vr.simxGetObjectHandle(name, vrep.simx_opmode_oneshot_wait) - - self.vr.simxReadProximitySensor(self.handle, vrep.simx_opmode_streaming) - self.last_state = False - - def check_tripped(self) -> bool: - """Returns true on a not tripped -> tripped transition. This must be called often enough to register the edge. - """ - # boolean detectionState, array detectedPoint, - # number detectedObjectHandle, array detectedSurfaceNormalVector - # Specify -1 to retrieve the absolute position. - in_proximity = self.vr.simxReadProximitySensor(self.handle, vrep.simx_opmode_streaming)[0] - tripped = (not self.last_state and in_proximity) - self.last_state = in_proximity - return tripped - - -class Car(object): - """Abstraction object for the car, providing intuitive functions for getting - car state and setting outputs. - """ - def __init__(self, vrep_interface: Any, steering_limit: float = 30, steering_slew_rate: float = (60/0.016)) -> None: - self.vr = vrep_interface - self.car_handle = self.vr.simxGetObjectHandle('AckermannSteeringCar', vrep.simx_opmode_oneshot_wait) - self.camera_handle = [] - self.camera_handle.append(self.vr.simxGetObjectHandle('LineCamera0', vrep.simx_opmode_oneshot_wait)) - self.camera_handle.append(self.vr.simxGetObjectHandle('LineCamera1', vrep.simx_opmode_oneshot_wait)) - - self.motor_fl_handle = self.vr.simxGetObjectHandle('FrontLeftMotor', vrep.simx_opmode_oneshot_wait) - self.motor_fr_handle = self.vr.simxGetObjectHandle('FrontRightMotor', vrep.simx_opmode_oneshot_wait) - self.vr.simxSetJointTargetVelocity(self.motor_fl_handle, 0, vrep.simx_opmode_oneshot_wait) - self.vr.simxSetJointTargetVelocity(self.motor_fr_handle, 0, vrep.simx_opmode_oneshot_wait) - - self.motor_rl_handle = self.vr.simxGetObjectHandle('RearLeftMotor', vrep.simx_opmode_oneshot_wait) - self.motor_rr_handle = self.vr.simxGetObjectHandle('RearRightMotor', vrep.simx_opmode_oneshot_wait) - self.vr.simxSetJointTargetVelocity(self.motor_rl_handle, 0, vrep.simx_opmode_oneshot_wait) - self.vr.simxSetJointTargetVelocity(self.motor_rr_handle, 0, vrep.simx_opmode_oneshot_wait) - - self.steer_fl_handle = self.vr.simxGetObjectHandle('FrontLeftSteering', vrep.simx_opmode_oneshot_wait) - self.steer_fr_handle = self.vr.simxGetObjectHandle('FrontRightSteering', vrep.simx_opmode_oneshot_wait) - self.vr.simxSetJointTargetPosition(self.steer_fl_handle, 0, vrep.simx_opmode_oneshot_wait) - self.vr.simxSetJointTargetPosition(self.steer_fr_handle, 0, vrep.simx_opmode_oneshot_wait) - - # Open these variables in streaming mode for high efficiency access. - self.vr.simxGetObjectPosition(self.car_handle, -1, vrep.simx_opmode_streaming) - self.vr.simxGetObjectVelocity(self.car_handle, vrep.simx_opmode_streaming) - - for handle in self.camera_handle: - self.vr.simxGetVisionSensorImage(handle, 1, vrep.simx_opmode_streaming) - - # Get the wheel diameter so we can set velocity in physical units. - # Let's assume all the wheels are the same. - # TODO: check this assumption? - wheel_dia = self.vr.get_bounding_size('RearLeftWheel_respondable')[0] - # multiply linear velocity by this to get wheel radians/s - self.speed_factor = 2 / wheel_dia - self.wheelbase_width = 0.0755 - self.wheelbase_length = 0.25 - - # Default parameters - self.steering_limit = steering_limit - self.steering_slew_rate = steering_slew_rate # degrees per second - self.steering_deadband = 1.0 # degree, below which changes don't register - - # state variables - self.steering_state = 0.0 - - # - # Some helper functions to get/set important car data/state - # - def get_sim_time(self) -> float: - """Returns the current simulation time in seconds. - """ - return vrep.simxGetLastCmdTime(self.vr.client_id) / 1000.0 - - def get_position(self) -> Tuple[float, float, float]: - """Returns the car's absolute position as a 3-tuple of (x, y, z), in meters. - """ - # Specify -1 to retrieve the absolute position. - return self.vr.simxGetObjectPosition(self.car_handle, -1, - vrep.simx_opmode_streaming) - - def get_velocity(self) -> Tuple[float, float, float]: - """Returns the car's linear velocity as a 3-tuple of (x, y, z), in m/s. - """ - return self.vr.simxGetObjectVelocity(self.car_handle, - vrep.simx_opmode_streaming)[0] - - def get_steering_angle(self) -> float: - """Returns the car's steering angle in degrees. - """ - return self.steering_state - - def get_line_camera_image(self, camera_index: int) -> List[int]: - """Returns the line sensor image as an array of pixels, where each pixel is - between [0, 255], with 255 being brightest. - Likely non-physical (graphical) units of linear perceived pixel intensity. - """ - # Magical '1' argument specifies to return the data as greyscale. - handle = self.camera_handle[camera_index] - _, image = self.vr.simxGetVisionSensorImage(handle, 1, - vrep.simx_opmode_streaming) - for i, intensity in enumerate(image): - if intensity < 0: # undo two's complement - image[i] = 256 + intensity - return image - - def set_speed(self, speed: float, blocking: bool = False) -> None: - """Sets the car's target speed in m/s. Subject to acceleration limiting in - the simulator. - """ - if blocking: - op_mode = vrep.simx_opmode_oneshot_wait - else: - op_mode = vrep.simx_opmode_oneshot - - desired_speed = speed * self.speed_factor - self.vr.simxSetJointTargetVelocity(self.motor_fl_handle, desired_speed, op_mode) - self.vr.simxSetJointTargetVelocity(self.motor_fr_handle, desired_speed, op_mode) - self.vr.simxSetJointTargetVelocity(self.motor_rl_handle, desired_speed, op_mode) - self.vr.simxSetJointTargetVelocity(self.motor_rr_handle, desired_speed, op_mode) - - def _set_steering(self, angle_deg: float) -> None: - angle_rad = math.radians(angle_deg) - if angle_deg != 0: - steer_angle_left = math.atan(1 / (-self.wheelbase_width + 1/math.tan(angle_rad))) - steer_angle_right = math.atan(1 / (self.wheelbase_width + 1/math.tan(angle_rad))) - else: - steer_angle_left = 0 - steer_angle_right = 0 - self.vr.simxSetJointTargetPosition(self.steer_fl_handle, steer_angle_left, vrep.simx_opmode_oneshot) - self.vr.simxSetJointTargetPosition(self.steer_fr_handle, steer_angle_right, vrep.simx_opmode_oneshot) - - def set_steering_fast(self, angle_cmd: float, dt: float) -> float: - """Sets the car's steering angle in degrees. - Fast: No steering angle rate limiting. - Returns the actual commanded angle. - """ - angle = max(min(angle_cmd, self.steering_limit), -self.steering_limit) - self.steering_state = angle # update state - self._set_steering(angle) - return angle - - def set_steering(self, angle_cmd: float, dt: float) -> float: - """Sets the car's steering angle in degrees. - Steering angle rate limiting happens here. - Returns the actual commanded angle. - """ - angle = self.steering_state # get present angle - angle_err = angle_cmd - angle - - # check if can reach angle in single time step? - # if so, then don't need slew rate limit - if (abs(angle_err) < dt*self.steering_slew_rate): - angle = min(angle_cmd, self.steering_limit) # check saturation - angle = max(angle, -self.steering_limit) - self.steering_state = angle # update state - self._set_steering(angle) - return angle # can reach angle in single time step - - if (angle_cmd > self.steering_state +1): # add some dead band - angle = self.steering_state + dt * self.steering_slew_rate - if (angle_cmd < self.steering_state -1): - angle = self.steering_state - dt * self.steering_slew_rate - angle = min(angle, self.steering_limit) - angle = max(angle, -self.steering_limit) - self.steering_state = angle # update state - self._set_steering(angle) - return angle - - def set_line_camera_parameters(self, camera_index: int, height: float = 0.3, - orientation: float = 30, fov: float = 120) -> None: - """Sets the car's line camera parameters. - This is provided so you don't have to learn how to mess with the V-REP - scene to tune your camera parameters. - Args: - height -- height of the camera, in meters. - orientation -- downward angle of the camera, in degrees from horizontal. - fov -- field of vision of the camera, in degrees. - *** note: puts camera above orgin of car *** - """ - handle = self.camera_handle[camera_index] - self.vr.simxSetObjectPosition(handle, vrep.sim_handle_parent, - (height, 0, 0), - vrep.simx_opmode_oneshot_wait) - self.vr.simxSetObjectOrientation(handle, vrep.sim_handle_parent, - (0, - -math.radians(orientation), - math.radians(90)), - vrep.simx_opmode_oneshot_wait) - self.vr.simxSetObjectFloatParameter(handle, 1004, math.radians(fov), - vrep.simx_opmode_oneshot_wait) - - def set_steering_limit(self, steering_limit: float) -> None: - """Sets the car's steering limit in degrees from center. - Attempts to steer past this angle (on either side) get clipped. - """ - self.steering_limit = steering_limit diff --git a/controller4wd.py b/controller4wd.py deleted file mode 100644 index fbfb828..0000000 --- a/controller4wd.py +++ /dev/null @@ -1,237 +0,0 @@ -# modifications by Doug from Spring 2016 using single loop -# modified Spring 2018 to handle angle and rate limits for steering servo -from typing import Any, List, Optional - -import math -import time -import os.path -import numpy as np # type: ignore -import vrep # type: ignore -import vrepInterface # type: ignore -import sys -#from carInterface import Car, Tripwire -from carInterface4wd import Car, Tripwire -from simpleCsvDict import SimpleCsvDictWriter - - -class SimulationAssignment(): - """Everything you need to implement for the assignment is here. - You may """ - def __init__(self, vr: Any, car: Car, target_speed: float) -> None: - # You may initialize additional state variables here - self.last_sim_time = car.get_sim_time() - - self.target_speed = target_speed - - self.old_lat_err = 0.0 - self.int_err = 0.0 # integral error - - def setup_car(self, vr: Any, car: Car) -> None: - """Sets up the car's physical parameters. - """ - # - # ASSIGNMENT: You may want to tune these paraneters. - # - # car.set_boom_sensor_offset(0.1) - # In the scene, we provide two cameras. - # Camera 0 is used by default in the control loop and is the "near" one. - # Some teams have also used a "far" camera in the past (Camera 1). - # You can use additional cameras, but will have to add then in the V-REP scene - # and bind the handle in Car.__init__. The V-REP remote API doesn't provide a - # way to instantiate additional vision sensors. - car.set_line_camera_parameters(0, height=0.3, orientation=60, fov=90) - car.set_line_camera_parameters(1, height=0.4, orientation=15, fov=60) - # You should measure the steering servo limit and set it here. - # A more accurate approach would be to implement servo slew limiting. - car.set_steering_limit(30) - - def get_line_camera_error(self, image: List[int]) -> float: - """Returns the distance from the line, as seen by the line camera, in - pixels. The actual physical distance (in meters) can be derived with some - trig given the camera parameters. - """ - # - # ASSIGNMENT: You should implement your line tracking algorithm here. - # The default code implements a very simple, very not-robust line detector. - # - # NOTE: unlike the actual camera, get_line_camera_image() returns pixel - # intensity data in the range of [0, 255] and aren't physically based (i.e. - # intensity in display intensity units, no integration time). - # - INTENSITY_THRESHOLD = 192 - - weighted_sum = 0 - element_sum = 0 - for i, intensity in enumerate(image): - if intensity > INTENSITY_THRESHOLD: - weighted_sum += i - element_sum += 1 - if element_sum == 0: - return 0 - return weighted_sum / element_sum - 63 - - def control_loop(self, vr: Any, car: Car, csvfile: Optional[SimpleCsvDictWriter]) -> None: - """Control iteration. This is called on a regular basis. - Args: - vr -- VRepInterface object, which is an abstraction on top of the VREP - Python API. See the class docstring near the top of this file. - car -- Car object, providing abstractions for the car (like steering and - velocity control). - csvfile -- Optional to log data. None to disable. - """ - # Constant speed for now. You can tune this and/or implement advanced controllers. - #car.set_speed(self.target_speed) - - sim_time = car.get_sim_time() - dt = sim_time - self.last_sim_time - self.last_sim_time = sim_time - - # - # ASSIGNMENT: Tune / implement a better controller loop here. - # - - # Here are two different sensors you can play with. - # One provides an ideal shortest-distance-to-path (in meters), but isn't - # path-following and may jump at crossings. - # The other uses a more realistic line sensor (in pixels), which you can - # plug your track detection algorithm into. - line_camera_image0 = car.get_line_camera_image(0) - line_camera_image1 = car.get_line_camera_image(1) - line0_err = self.get_line_camera_error(line_camera_image0) - line1_err = self.get_line_camera_error(line_camera_image1) - - # line camera has 0.7 m field of view - lat_err = -(np.float(line0_err)/128)*0.7 # pixel to meter conversion - - - if dt > 0.0: - lat_vel = (lat_err - self.old_lat_err)/dt - else: - lat_vel = 0.0 - self.old_lat_err = lat_err - - # calculate integral error - self.int_err = self.int_err + dt*lat_err - - # Proportional gain in steering control (degrees) / lateral error (meters) - kp = 200 - kd = 0 # deg per m/s - ki = 0 # deg per m-s - steer_angle = -kp * lat_err - kd * lat_vel - ki * self.int_err - - steer_angle = car.set_steering(steer_angle, dt) # use set_steering to include servo slew rate limit - # steer_angle = car.set_steering_fast(steer_angle,dt) # use set_steering_fast for no delay - - # Constant speed for now. You can tune this and/or implement advanced controllers. - car.set_speed(self.target_speed) - - # Print out and record debugging info - pos = car.get_position() - vel_vector = car.get_velocity() - vel = math.sqrt(vel_vector[0]**2 + vel_vector[1]**2 + vel_vector[2]**2) - print('t=%6.3f (x=%5.2f, y=%5.2f, sp=%5.2f): lat_err=%5.2f, int_err=%5.2f, line0_err=%3i, steer_angle=%3.1f' - % (sim_time, pos[0], pos[1], vel, - lat_err, self.int_err, (line0_err or 0), steer_angle)) - - if csvfile is not None: - csvfile.writerow({'t': sim_time, - 'x': pos[0], 'y': pos[1], - 'linescan': line_camera_image0, - 'line_pos': line0_err + 63, # needs to be in camera pixels so overlaid plots work - 'linescan_far': line_camera_image1, - 'line_pos_far': line1_err + 63, - 'speed': vel, - 'lat_err': lat_err, - 'steer_angle': steer_angle, - }) - - -if __name__ == "__main__": - import argparse - parser = argparse.ArgumentParser(description='ee192 Python V-REP Car controller.') - parser.add_argument('--synchronous', metavar='s', type=bool, default=True, - help="""enable synchronous mode, forcing the simulator to - operate in lockstep with the simulator - potentially - increases accuracy / repeatability at the cost of - performance""") - parser.add_argument('--restart', metavar='r', type=bool, default=False, - help="""whether to restart the simulation if a simulation - is currently running""") - parser.add_argument('--csvfile', metavar='c', default='car_data.csv', - help='csv filename to log to') - parser.add_argument('--csvfile_overwrite', metavar='csvfile_overwrite', type=bool, default=True, - help='overwrite the specified csvfile without warning') - parser.add_argument('--laps', metavar='l', type=int, default=1, - help="""Number of laps to run, default of 1. 0 means infinite.""") - parser.add_argument('--velocity', metavar='v', type=float, default=1.0, - help="""Set the Velocity, in m/s.""") - args = parser.parse_args() - - # Check that we won't overwrite an existing csvfile before mucking with the - # simulator. - if (args.csvfile is not None and os.path.exists(args.csvfile) - and not args.csvfile_overwrite): - print("csvfile '%s' already exists: aborting." % args.csvfile) - sys.exit() - - # Stop the existing simulation if requested. Not using a separate - # VRepInterface (and hence VREP API client id) seems to cause crashes. - if args.restart: - with vrepInterface.VRepInterface.open() as vr: - vr.simxStopSimulation(vrep.simx_opmode_oneshot_wait) - - # This needs not be in the main body, or the API throws an exception a split second after simulation start. - with vrepInterface.VRepInterface.open() as vr: - ret = vr.simxStartSimulation(vrep.simx_opmode_oneshot_wait) - - # Open a V-REP API connection and get the car. - with vrepInterface.VRepInterface.open() as vr: - if args.synchronous: - vr.simxSynchronous(1) - - car = Car(vr) - wire = Tripwire(vr) - - success = False - while not success: - try: - car.get_sim_time() - success = True - except vrepInterface.VRepAPIError: - print("waiting for simulation start") - time.sleep(1) - - assignment = SimulationAssignment(vr, car, args.velocity) - assignment.setup_car(vr, car) - - csvfile = None - if args.csvfile: - csvfile = SimpleCsvDictWriter(args.csvfile) - - try: - done = False - completed_laps = -1 - while not done: - assignment.control_loop(vr, car, csvfile) - finish_tripped = wire.check_tripped() - if finish_tripped: - completed_laps += 1 - if completed_laps > 0: # discard the first finish line crossing, which happens at the start - print("finished lap " + str(completed_laps)) - if completed_laps >= args.laps and args.laps != 0: - done = True - - if args.synchronous: - vr.simxSynchronousTrigger() - except KeyboardInterrupt: - # Allow a keyboard interrupt to break out of the loop while still shutting - # down gracefully. - print("caught keyboard interrupt, terminating control loop") - pass - finally: - print("ending simulation") - vr.simxStopSimulation(vrep.simx_opmode_oneshot_wait) - - if csvfile is not None: - csvfile.close() # close file and writer - print("csv files closed") diff --git a/tracks/cory-track+heavy.ttt b/tracks/cory-track+heavy.ttt deleted file mode 100644 index cb9f3b3..0000000 Binary files a/tracks/cory-track+heavy.ttt and /dev/null differ diff --git a/tracks/cory-track.ttt b/tracks/cory-track.ttt deleted file mode 100644 index 1735bd5..0000000 Binary files a/tracks/cory-track.ttt and /dev/null differ