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'