def startSimulation(self):
if self.clientId != -1:
#+++++++++++++++++++++++++++++++++++++++++++++
step = 0.005
vrep.simxSetFloatingParameter(
self.clientId, vrep.sim_floatparam_simulation_time_step, step,
vrep.simx_opmode_oneshot)
vrep.simxSynchronous(self.clientId, True)
vrep.simxStartSimulation(self.clientId, vrep.simx_opmode_oneshot)
#init the controller
planeController = PlaneCotroller(self.clientId)
planeController.take_off()
self.pdController = controller.PID(cid=self.clientId,
ori_mode=True)
#create a thread to controll the move of quadcopter
_thread.start_new_thread(self.pdThread, ())
print("thread created")
#to be stable
planeController.loose_jacohand()
# planeController.move_to(planeController.get_object_pos(planeController.copter),True)
# planeController.plane_pos = planeController.get_object_pos(planeController.copter)
# time.sleep(10)
self.run_simulation(planeController)
else:
print('Failed connecting to remote API server')
print('Simulation ended')
vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_oneshot)
def main():
try:
value = 0.1
#while True:
pid = controller.PID(0.001, 0, 0)
pid.set_desired(530)
pwm_update(pid, 530, 10)
best_error = rotation_count.get_counts()
#try:
#p, best_error = twiddle(535, 5)
#motor_voltage = motor_analog.get_float() * AREF
#except:
# print("Are you sure you have an ADC?")
print("Gains: {}, error: {}".format(p, best_error))
except KeyboardInterrupt:
motor.write_pulse_duty(0)
motor.disable_pwm()
led.set_low()
print("Stopping everything")
finally:
motor.write_pulse_duty(0)
motor.disable_pwm()
led.set_low()
print("Stopping everything")
def twiddle(target, time_limit, tol=0.2):
# Don't forget to call `make_robot` before you call `run`!
p = [0, 0, 0]
dp = [1, 1, 1]
pid = controller.PID(p[0], p[1], p[2])
pid.set_desired(target)
best_error = rotation_count.get_counts()
run_error = 0
while sum(dp) > tol:
for i in range(len(p)):
p[i] += dp[i]
# Reset the PID controller with the new parameters
pid.clear_PID() # re - initalise the controller
pid.Kp = p[0]
pid.ki = p[1]
pid.kd = p[2]
pwm_update(pid, target, time_limit)
if (run_error < best_error):
# the parameters are better
best_err = run_error
dp[i] *= 1.1
else:
# parameters are worse
dp[i] -= 2 * dp[i]
if (run_error < best_error):
best_err = run_error
dp[i] *= 1.1
else:
p[i] += dp[i]
dp[i] *= 0.9
return p, best_error
datacollector = traindatacollector.DataCollect();
model = torch.nn.Sequential(
torch.nn.Linear(3, 10),
torch.nn.Tanh(),
torch.nn.Linear(10, 1),
torch.nn.Tanh()
)
def convertState(state):
cos_theta = state[0];
sin_theta = state[1];
thetadot = state[2];
return np.arctan2(sin_theta,cos_theta),thetadot;
pid = controller.PID(0.3,0.6,0.1,output_limit=2.0);
target = 0.0;# 0 deg
theta = 0.0;# rad
thetadot = 0.0;# rad/s
for eps in range(10):
print("!!!Data Collect!!! "+str(eps)+" situation");
error_k_0 = 0.0;
error_k_1 = 0.0;
error_k_2 = 0.0;
state = env.reset();
pid.reset();
for time in range(500):
# show Pendulum
THREASHOLD_ANG_MAX = 10 #10 THREASHOLD_ANG_MIN = 5 THREASHOLD_COORDINATES = 5 SAMPLE_TIME = 0.1 ROBOT_LENGHT = 13 SCALE_FACTOR = 5 turning_flag = False priv_turn = 0 priv_forward = 0 ANGLE_CALIBRATION= -30 # Create Controllers posistion_controller = controller.PID(5, 5, 0) posistion_controller.setSampleTime(SAMPLE_TIME) posistion_controller.setSetpoint(0) # These constants are determined by looking at the robot # when the battery is fully charged #angle_controller = controller.PID(0.01, 5, 0) angle_controller = controller.PID(0.5, 0, 1) angle_controller.setSampleTime(SAMPLE_TIME) ang_ctrl_output = 0 pos_ctrl_output = 0 # 1D Variables for angle and distance dist_ref = 0.0 dist_robot = 0.0
def __init__(self):
self.start_time = None # To record the start time of navigation
self.total_time = None # To record total duration of naviagation
self.img = None # Current camera image
self.pos = None # Current position (x, y)
self.yaw = None # Current yaw angle
self.pitch = None # Current pitch angle
self.roll = None # Current roll angle
self.vel = None # Current velocity
self.steer = 0 # Current steering angle
self.throttle = 0 # Current throttle value
self.brake = 0 # Current brake value
self.nav_angles = [] # Angles of navigable terrain pixels
self.nav_dists = [] # Distances of navigable terrain pixels
self.ground_truth = ground_truth_3d # Ground truth worldmap
self.mode = 'forward' # Current mode (can be forward or stop)
self.throttle_set = 1.25 # Throttle setting when accelerating
self.brake_set = 0.5 # Brake setting when braking
# The stop_forward and go_forward fields below represent total count
# of navigable terrain pixels. This is a very crude form of knowing
# when you can keep going and when you should stop. Feel free to
# get creative in adding new fields or modifying these!
self.stop_forward = 20 # Threshold to initiate stopping
self.angle_forward = 20 # Threshold angle to go forward again
self.can_go_forward = True # tracks clearance ahead for moving forward
# pixel distance threshold for how close to a wall before turning around
self.mim_wall_distance = 25
# pitch angle for when the rover is considered to have climbed a wall
self.pitch_cutoff = 2.5
self.max_vel = 5 # Maximum velocity (meters/second)
# Image output from perception step
# Update this image to display your intermediate analysis steps
# on screen in autonomous mode
self.vision_image = np.zeros((160, 320, 3), dtype=np.float)
# Worldmap
# Update this image with the positions of navigable terrain
# obstacles and rock samples
self.worldmap = np.zeros((200, 200, 3), dtype=np.float)
self.sample_angles = None # Angles of sample pixels
self.sample_dists = None # Distances of sample pixels
self.sample_detected = False # set to True when sample found in image
self.sample_stop_forward = 5 # Threshold to initiate stopping
self.samples_pos = None # To store the actual sample positions
self.samples_found = 0 # To count the number of samples found
self.near_sample = 0 # Will be set to telemetry value data["near_sample"]
self.picking_up = 0 # Will be set to telemetry value data["picking_up"]
self.send_pickup = False # Set to True to trigger rock pickup
# path planning
self.width = 320 # width of camera images
self.height = 160 # height of camera images
self.grid = np.invert(grid) # world map for grid and local search
# positions of unknown map areas to discover
self.goal = [[78, 75], [60, 101], [16, 98], [114, 11], [118, 50],
[145, 95], [145, 95], [145, 40], [103, 189]]
# setup the policy grid for grid search
self.policy = [[-1 for col in range(len(grid[0]))]
for row in range(len(grid))]
self.grid_set = False # tracks if a grid policy is in place or not
self.dst_size = 10 # pixel count for warped image destination size
self.bottom_offset = 0 # pixel offset from the bottom of the screen
# amount of pixels per meter on the map seen through the camera
self.scale = 2 * self.dst_size
# source points for image warping
self.source = np.float32([[14, 140], [301, 140], [200, 96], [118, 96]])
# destination points for the image warping
self.destination = np.float32([
[self.width / 2 - self.dst_size, self.height - self.bottom_offset],
[self.width / 2 + self.dst_size, self.height - self.bottom_offset],
[
self.width / 2 + self.dst_size,
self.height - 2 * self.dst_size - self.bottom_offset
],
[
self.width / 2 - self.dst_size,
self.height - 2 * self.dst_size - self.bottom_offset
],
])
self.skip_next = False # tracks image processing and skips every second one
self.PID = controller.PID(2, 0.005, 0.5) # PID controller for speed
# keeps track of the turn direction for on the spot rotations
self.turn_dir = 'none'