Пример #1
0
def control(data):
    global vel_input
    global kp
    global kd
    global OFFSET

    circle_detected = False

    if os.path.isfile(path_to_watch):
        with open(path_to_watch, 'r') as content_file:
            content = content_file.readlines()
            for line in content:
                print "\n\nBEGINNING NEW PROCESSING OF TEXT FILE \n\n"
                if line not in cache:
                    cache.add(line)
                    print "Should start to turn! " + str(line)

                    # we will actually parse this line in the future
                    # every line has to be unique. So must follow format "# - command" per line

                    circle_detected = True

    else:
        print "~/circle.txt does not exist"

    if circle_detected:
        print "Circle Detected"
        # ignore any of the wall distance correction logic, just set angle, velocity and publish
        angle = -170
        msg = drive_param()
        msg.velocity = vel_input
        msg.angle = angle
        print "PUBLISHING MESSAGE: " + str(msg)
        pub.publish(msg)
        time.sleep(1)
    else:
        print "Circle Not Detected"
        data.pid_error = data.pid_error + OFFSET
        angle = data.pid_error * kp
        if angle > 100:
            angle = 100
        if angle < -100:
            angle = -100

        msg = drive_param()
        if data.pid_vel == 0:
            msg.velocity = -8
            angle = 0
        else:
            msg.velocity = vel_input

        msg.angle = angle
        print "PUBLISHING MESSAGE: " + str(msg)
        pub.publish(msg)
Пример #2
0
def control_callback(data):
  # TODO: Based on the error (data.data), determine the car's required velocity
  # amd steering angle.
  e_t1 = data.data
  global e_t0
  u = KP * e_t1 + KD * (e_t1 - e_t0)
  u = np.rad2deg(u)
  if u > 30:
      u = 30
  elif u < -30:
      u = -30
  if abs(u) <= 10:
      vel = 1.5
  elif abs(u) <= 20:
      vel = 1.0
  else:
      vel = 0.5
      
  u = np.deg2rad(u)
  msg = drive_param()
  msg.velocity = vel  # TODO: implement PID for velocity
  msg.angle = u    # TODO: implement PID for steering angle
  # print msg
  pub.publish(msg)
  e_t0 = data.data
Пример #3
0
    def send_actuation_command(self, pred):
        #create the drive param message
        msg = drive_param()
        msg.angle = 0.0
        msg.velocity = 1.0
        #get the label
        label = self.classes[pred[0].argmax()]

        if (label == "left"):
            msg.angle = 0.4108652353
        elif (label == "right"):
            msg.angle = -0.4108652353
        elif (label == "straight"):
            msg.angle = 0.0
        elif (label == "weak_left"):
            msg.angle = 0.10179
        elif (label == "weak_right"):
            msg.angle = -0.10179
        else:
            print("error:", label)
            msg.velocity = 0
            msg.angle = 0
        self.commands.append(msg.angle)
        self.times.append(time.time() - self.start_time)
        self.pub.publish(msg)
def shutdown_car():
    vel_input = 0
    angle = 0
    msg = drive_param()
    msg.velocity = vel_input
    msg.angle = angle
    pub.publish(msg)
Пример #5
0
def control(data):
	global prev_error
	global vel_input
	global kp
	global kd

	## Your code goes here
	# 1. Scale the error
	error = scale_factor*data.pid_error
	# 2. Apply the PID equation on error
	V0 = kp*error + kd*(prev_error-error)
	# 3. Make sure the error is within bounds
	if V0 < max_angle and V0 > -max_angle:
                angle = V0
	elif V0 > max_angle:
                angle = max_angle
	elif V0 < -max_angle:
                angle = -max_angle
	else:
                angle = 0
	angle+=servo_offset
	angle=round(angle,2)
	print("Angle: ", angle) 
	prev_error = V0
	## END
	msg = drive_param();
	if data.pid_vel == 0:
		msg.velocity = 0;
	else:
		msg.velocity = vel_input	
	msg.angle = angle
	pub.publish(msg)
Пример #6
0
    def control(self,data):
	global prev_error
	global vel_input
	global kp
	global kd

	## Your code goes here
	# 1. Scale the error
	# 2. Apply the PID equation on error
	# 3. Make sure the error is within bounds
	angle = data.pid_error*kp;
	if angle > 100:
		angle = 100
	if angle < -100:
		angle = -100

	## END

        # do extra processing with the classifier decision
        if not self.shouldTurn:
          angle = min(turn_threshold,angle)

	msg = drive_param();
	if(data.pid_vel == 0):
		msg.velocity = -8
	else:
		msg.velocity = vel_input	
	msg.angle = angle
	pub.publish(msg)
Пример #7
0
def laser_callback(laser_data):
    global throttle
    global turn
    global deadman_butt
    global VMAX
    global old_velocity
    min_dist = get_min_dist(laser_data)
    velocity_ratio = 0.5 * min_dist
    msg = drive_param()
    msg.angle = turn * 24 * np.pi / 180 + servo_offset
    if min_dist < 1:  # and throttle * VMAX < -1.5:
        velocity_ratio = 0.2 * min_dist
    if velocity_ratio > 1:
        velocity_ratio = 1

    if throttle < 0:
        print("velocity ratio", velocity_ratio)
        msg.velocity = velocity_ratio * throttle * VMAX
    else:
        msg.velocity = throttle * 0.5

    if not deadman_butt:
        msg.velocity = 0
        msg.angle = 0

    if msg.velocity < 0:
        if msg.velocity - old_velocity < -0.05:
            msg.velocity = old_velocity - 0.05
        old_velocity = msg.velocity

    print "Velocity", msg.velocity
    print "Angle in Degrees", msg.angle * 180 / np.pi

    pub.publish(msg)
Пример #8
0
def callback(data):

    print " "
    x_dot = data.linear.x # dot is the representation of velocity (derivative of x)
    y_dot = data.linear.y
    theta_dot = data.angular.z # Velocity which car rotates with respect to map
    
    # velocity = math.sqrt(x_dot**2 + y_dot**2)
    velocity = x_dot
    # angle = math.atan2(WHEELBASE_LENGTH * theta_dot, velocity)
    angle = theta_dot
#    angle = -1.0 * angle # Need to flip the sign because left steering is negative

    print "Computed velocity: ", velocity
    print "Computed angle: ", angle
    print "Angle in Degrees", angle*180/np.pi
    angle = np.clip(angle, -0.4189, 0.4189) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max
    if velocity < 1.0 and velocity >= 0.0:
      velocity = 1.0
#    if angle < 0.035 and angle > -0.035: # 2 degrees
#        velocity = 3.0
    print "Output velocity: ", velocity
    print "Output angle: ", angle

    msg = drive_param()
    msg.velocity = velocity
    msg.angle = angle
    pub.publish(msg)
Пример #9
0
def control(data):
	global prev_error
	global vel_input
	global kp
	global kd

	scaling_factor = 10

	## Your code goes here
	# 1. Scale the error
	# 2. Apply the PID equation on error
	# 3. Make sure the error is within bounds
 	error = data.pid_error*scaling_factor
	angle = kp*error+kd*(prev_error - error)
	#deriv = kd*(error-prev_error)
	if(angle < -100):
		angle = -100
	if(angle > 100):
		angle = 100
	
	prev_error = data.pid_error
	## END

	msg = drive_param();
	msg.velocity = vel_input	
	msg.angle = angle
	pub.publish(msg)
Пример #10
0
def control(data):
    global prev_error
    global vel_input
    global kp
    global kd

    ## Your code goes here
    # 1. Scale the error
    # 2. Apply the PID equation on error
    # 3. Make sure the error is within bounds
    #error = data.pid_error * 10
    #angle = kp * error + kd * (prev_error - error)
    prev_error = data.pid_error
    angle = prev_error
    if (angle < -100):
        angle = -100
    if (angle > 100):
        angle = 100

    ## END

    msg = drive_param()
    msg.velocity = vel_input
    msg.angle = angle
    pub.publish(msg)
Пример #11
0
    def control_callback(self, data):
        #calculate frame time
        self.currentTime = time.time()#float(data.header.stamp.nsecs) * pow(10, -9)
        #print(self.currentTime - self.lastTime)

        #get and store error
        et = data.pid_error
        self.storeError(et)

        self.etInt += self.integralError(et)

        #weighted pid equation for angle increment
        ut = KP * (et) + KI * self.etInt + KD * self.derivativeError(et)
        self.angle += ut #* (self.currentTime - self.lastTime)
        if np.isnan(self.angle):
            self.angle = np.nanmean(self.angleWindow)
        self.angle = max(min(self.angle, SPD_DEC_ANGLE_MAX), -1*SPD_DEC_ANGLE_MAX) #clamp angle between -/+ max angle

        #store historical data
        self.lastError = et
        self.storeAngle(self.angle)
        self.lastTime = self.currentTime

        msg = drive_param()
        msg.angle = self.angle    # TODO: implement PID for steering angle
        msg.velocity = self.angleMaxVelocity(self.angle) * self.velocityMultiple # TODO: implement PID for velocity
        print("ERROR: ", et)
        print("ANGLE: ", np.rad2deg(self.angle))
        #print("VEL: ", msg.velocity)
        self.drivePub.publish(msg)
Пример #12
0
def control(data):
    global prev_error
    global vel_input
    global kp
    global kd

    ## Your code goes here
    # 1. Scale the error
    # 2. Apply the PID equation on error
    # 3. Make sure the error is within bounds
    pid_offset = -0.158
    angle = (data.pid_error + pid_offset) * kp
    print("Printing pid_error: ", data.pid_error, "angle: ", angle)
    #print("Printing angle: %d", angle)
    if angle > 100:
        angle = 100
    if angle < -100:
        angle = -100

    ## END

    msg = drive_param()
    if (data.pid_vel == 0):
        msg.velocity = -8
    else:
        msg.velocity = vel_input
    msg.angle = angle
    pub.publish(msg)
def control_callback(msg):
    
    global sum_error
    global prev_error
    global prev_angle

    # TODO: Based on the error (msg.data), determine the car's required velocity and steering angle.
    error = msg.data
    sum_error = sum_error + error
    pid_output = KP*error + KI*sum_error + KD*(error - prev_error)/0.025  #TODO: compute pid response for steering angle based on the error
    prev_error = error
    
    angle = pid_output
    angle = np.clip(angle, -0.4189, 0.4189)  # 0.4189 radians = 24 degrees because car can only turn 24 degrees max

    ang_deg =  math.degrees(angle)
    if abs(ang_deg) >= 0.0 and abs(ang_deg) <= 10.0:
        vel = 1.5

    elif abs(ang_deg) >=10 and abs(ang_deg) <= 20.0:
        vel = 1.0

    else:
        vel = 0.5

    msg = drive_param()
    msg.velocity = vel  # TODO: implement PID for velocity
    msg.angle = angle    # TODO: implement PID for steering angle
    pub.publish(msg)
    print("Error: {:0.2f}, PID o/p: {:0.2f}, Angle: {:0.2f}, Velocity: {:0.2f}".format(error, pid_output, ang_deg, vel))
Пример #14
0
def control_callback(data):
    # TODO: Based on the error (data.data), determine the car's required velocity and steering angle.
    global errs
    global err_i
    global err_i_sum
    e = data.data

    errs[0] = errs[1]  #shift data up in the array
    errs[1] = e

    err_p = e
    err_d = errs[1] - errs[0]

    global count

    if count == 100:
        err_i = err_i_sum / 100
        count = 0
    count += 1
    err_i_sum += e

    steer_d = np.clip(KP * err_p + KD * err_d + KI * err_i, -30.0, 30.0)
    steer = steer_d / 180.0 * np.pi  #convert in to radians

    velocity = np.clip(1 + KPV * err_p + KDV * err_d, 0, 1.0)

    if np.mod(count, 10) == 0:
        print("Errors", err_p, err_d, err_i, velocity, steer_d)

    msg = drive_param()
    msg.velocity = 1  #TODO: implement PID for velocity 0.3
    msg.angle = steer  # TODO: implement PID for steering angle
    pub.publish(msg)
Пример #15
0
    def publish_commands(self, speed_force, angle_force):
        """
        TODO: describe what do
        :param speed_force: (float)
        :param angle_force: (float)
        """

        # Convert angle_force to angle command
        angle_cmd = self.steering_p_gain * angle_force + self.steering_d_gain * (
            angle_force - self.last_angle_force)
        self.last_angle_force = angle_force

        # Make sure angle command is within steering settings
        if abs(angle_cmd) > self.max_turn_angle:
            angle_cmd = (angle_cmd / abs(angle_cmd)) * self.max_turn_angle

        # Convert speed_force to speed command
        if speed_force <= 0:  # In the case that the car needs to backup, flip the turning angle
            angle_cmd = -1 * angle_cmd
            speed_cmd = -1 * self.min_speed
        else:
            speed_percentage = speed_force / self.force_offset_x
            speed_cmd = speed_percentage * (self.max_speed -
                                            self.min_speed) + self.min_speed

        # Publish the command
        msg = drive_param()
        msg.angle = angle_cmd
        msg.velocity = speed_cmd
        self.pub_drive_param.publish(msg)
        rospy.loginfo('speed [force, cmd]: [' + str(speed_force) + ', ' +
                      str(speed_cmd) + '] angle [force, cmd]: [' +
                      str(angle_force) + ', ' + str(angle_cmd) + ']')
        return
Пример #16
0
def publish_drive_param(json_string):
    msg = drive_param()
    msg.steer = json_string['drive']['steer']
    msg.throttle = json_string['drive']['throttle']
    pub_drive_parameters.publish(msg)
    logger.log_debug("[ROSMODULE] drive_param message sent")
    return
Пример #17
0
def control(data):
	global prev_error
	global vel_input
	global kp 
	global kd

	## Your code goes here
	# 1. Scale the error
	# 2. Apply the PID equation on error
	# 3. Make sure the error is within bounds
	error = data.pid_error * 10
	angle = kp * error + kd *(error - prev_error)
	if angle < -100:
		angle = -100
	elif angle > 100:
		angle = 100 
	prev_error = error 	


	## END

	msg = drive_param();
	msg.velocity = data.pid_vel	
	msg.angle = angle
	print(angle)
	print(data.pid_vel)
	pub.publish(msg)
Пример #18
0
def control(data):
	global integral
	global prev_error
	print integral
	velocity = data.pid_vel
	angle = servo_offset
	error = data.pid_error

	if error!=0.0:	
		integral = integral + error	
		control_error = kp*error + kd*(prev_error - error) + ki*integral
		integral = integral/1.3
		print control_error

		angle = angle - control_error

	elif error == 0.0:
		angle = servo_offset

	prev_error = error

	print "Control error",control_error
	print "Velocity",velocity
	print "Angle",angle
	msg = drive_param();
	msg.velocity = velocity
	msg.angle = angle
	pub.publish(msg)
def control(data):
	global prev_error
	global kp
	global kd
	global angle
        global prev_time

	## Your code goes here
	# 1. Scale the error
	error = data.pid_error
        cur_time = time.time()
	# 2. Apply the PID equation on error to compute steering
	v0 = (kp*error) + kd*(prev_error - error)/(cur_time-prev_time)
	# 3. Make sure the steering value is within bounds for talker.py
        prev_time = time.time()
	prev_error = error
	angle = -v0

	angle = angle if -100 < angle < 100 else 100 if angle > 100 else -100
	vel_input = data.pid_vel if -100 < data.pid_vel < 100 else 100 if data.pid_vel > 100 else -100
	## END

	msg = drive_param();
	msg.velocity = vel_input
	msg.angle = angle
	print "Angle:", msg.angle
	print "Velocity:", msg.velocity
        print "prev_error:", error
        print "-----------------------"
	pub.publish(msg)
def control_callback(msg):
    curr_error = msg.data

    global past_error

    pid_output = KP*curr_error + KD*(curr_error - past_error)/0.025  
    past_error = curr_error

    angle = pid_output
    angle = np.clip(angle, -0.4189, 0.4189)  # 0.4189 radians = 24 degrees because car can only turn 24 degrees max

    # Car starts driving down center
    direction = rospy.get_param('DIRECTION')
    vel = rospy.get_param('VELOCITY')

    if DIRECTION == 'stop':
        vel = 0.0
        angle = 0.0

    degree_angle =  math.degrees(angle)
    if  0.0 <= abs(degree_angle)  and abs(degree_angle) <= ANGLE_LEVEL_1:
        vel = vel
    elif  ANGLE_LEVEL_1<= abs(degree_angle) and abs(degree_angle) <= ANGLE_LEVEL_2:
        vel = SPEED_LEVEL_2
    else:
        vel = SPEED_LEVEL_3

    rospy.loginfo('vel %f angle_degrees %f angle_rad %f curr_error %f DIRECTION %s', vel, degree_angle, angle, curr_error, direction)
    msg = drive_param()
    msg.velocity = vel   
    msg.angle = angle  
    pub.publish(msg)
Пример #21
0
def selfdrive_control(data):
    global START
    if START == 1:
        msg_param = drive_param()
        msg_param.velocity = data.velocity
        msg_param.angle = data.angle
        pub_param.publish(msg_param)
Пример #22
0
def control(data):
    global prev_error
    global vel_input
    global kp
    global kd

    ## Your code goes here
    # 1. Scale the error
    # 2. Apply the PID equation on error
    # 3. Make sure the error is within bounds
    current_error = 10 * data.pid_error
    angle = kp * current_error + kd * (prev_error - current_error)
    prev_error = current_error

    if angle > 100:
        angle = 100
    elif angle < -100:
        angle = -100
    else:
        angle = angle

    #vel_input = data.pid_vel
    ## END

    msg = drive_param()
    msg.velocity = vel_input
    msg.angle = angle
    print msg
    pub.publish(msg)
Пример #23
0
def callback(data):
	#vertical: left_stick, axes[1]  (down: -1.0 -- up: 1.0)
	#horizontal: right_stick, axes[2]  (right: -1.0 -- left: 1.0)
	if (data.buttons[1]==1):
		print("braking")
		horizontal = 0
		vertical = -20
		stdscr.refresh()
		stdscr.addstr(1, 25, 'Emergency Stop')
		stdscr.addstr(2, 25, '%.2f' % vertical)
		stdscr.addstr(3, 25, '%.2f' % horizontal)

	else:
		horizontal = -data.axes[2]*100*scale_hori       #Warning... this one changes often

		vertical = data.axes[1]*100*scale_vert

		stdscr.refresh()
		stdscr.addstr(1, 25, '               ')
		stdscr.addstr(2, 25, '%.2f' % vertical)
		stdscr.addstr(3, 25, '%.2f' % horizontal)


	# msg_speed = drive_speed()
	# msg_speed.speed = vertical
	# pub_speed.publish(msg_speed)

	# msg_angle = drive_angle()
	# msg_angle.angle = horizontal
	# pub_angle.publish(msg_angle)

	msg_param = drive_param()
	msg_param.velocity = vertical
	msg_param.angle = horizontal
	pub_param.publish(msg_param)
Пример #24
0
def stop():
    msg = drive_param()
    msg.steer = 0
    msg.throttle = 0
    pub_drive_parameters.publish(msg)
    logger.log_info("[ROSMODULE] Stop signal sent")
    return
Пример #25
0
def callback(data):

    # Note: These following numbered steps below are taken from R. Craig Coulter's paper on pure pursuit.

    # 1. Determine the current location of the vehicle (we are subscribed to vesc/odom)
    # Hint: Read up on PoseStamped message type in ROS to determine how to extract x, y, and yaw.
    

        

    # 2. Find the path point closest to the vehicle that is >= 1 lookahead distance from vehicle's current location.



    # 3. Transform the goal point to vehicle coordinates. 
    
    

    # 4. Calculate the curvature = 1/r = 2x/l^2
    # The curvature is transformed into steering wheel angle by the vehicle on board controller.
    # Hint: You may need to flip to negative because for the VESC a right steering angle has a negative value.

    
    angle = np.clip(angle, -0.4189, 0.4189) # 0.4189 radians = 24 degrees because car can only turn 24 degrees max

    msg = drive_param()
    msg.velocity = VELOCITY
    msg.angle = angle
    pub.publish(msg)
Пример #26
0
def control(data):
    global kp
    global kd
    global servo_offset
    global prev_error
    global vel_input
    global mode

    msg = drive_param()
    msg.velocity = vel_input

    if mode == 'wall':
        pid_error = data.pid_error
        error = pid_error * kp
        errordot = kd * (pid_error - prev_error)

        angle = error + errordot

        if angle > 100:
            angle = 100
        elif angle < -100:
            angle = -100

        prev_error = pid_error

        print 'pid_error -> {}\nangle -> {}'.format(pid_error, angle)

        msg.angle = angle

    elif mode == 'corner':
        print 'corner mode, angle 100'
        msg.angle = 100

    pub.publish(msg)
Пример #27
0
def control(data):
    global prev_error
    global integral_error
    global vel_input
    global kp
    global ki
    global kd

    # Your code goes here
    # 1. Scale the error
    # 2. Apply the PID equation on error to compute steering
    # 3. Make sure the steering value is within bounds for talker.py
    msg = drive_param()
    msg.velocity = vel_input
    pid_error = data.pid_error
    error = pid_error * kp
    errordot = kd * (pid_error - prev_error)
    angle = error + errordot
    if angle > 100:
        angle = 100
    elif angle < -100:
        angle = -100
    prev_error = pid_error
    msg.angle = angle
    print(msg.angle)
    print("-------------")
    print(msg.velocity)
    pub.publish(msg)
    msg.angle = angle
    pub.publish(msg)
Пример #28
0
def control(data):
	global prev_error
	global vel_input
	global kp
	global kd

	## Your code goes here
	# 1. Scale the error
	# 2. Apply the PID equation on error
	# 3. Make sure the error is within bounds
	angle = data.pid_error*kp;
	if angle > 100:
		angle = 100
	if angle < -100:
		angle = -100

	## END

	msg = drive_param();
	if(data.pid_vel == 0):
		msg.velocity = -8
	else:
		msg.velocity = vel_input	
	msg.angle = angle
	pub.publish(msg)
Пример #29
0
def control(data):
    global prev_error
    global vel_input
    global kp
    global kd

    ## Your code goes here
    # 1. Scale the error
    # 2. Apply the PID equation on error
    # 3. Make sure the error is within bounds
    error = data.pid_error * 10
    angle = -kp * error - kd * (error - prev_error)
    angleRad = 180 / angle
    if angleRad < -0.78:
        angleRad = -0.78
    elif angleRad > 0.78:
        angleRad = 0.78
    prev_error = error

    ## END

    msg = drive_param()
    msg.velocity = data.pid_vel
    msg.angle = angleRad
    print(angle)
    print(data.pid_vel)
    pub.publish(msg)
def control(data):
    global prev_error
    global vel_input
    global kp
    global kd

    ## Your code goes here
    # 1. Scale the error
    # 2. Apply the PID equation on error to compute steering
    # 3. Make sure the steering value is within bounds for talker.py
    error = data.pid_error
    distance_error = kp * error + kd * (prev_error - error)
    #angle = int(maprange((-10,10),(-100,100), distance_error))
    angle = -10 * distance_error
    #if (angle < 0):
    #angle = angle * 3
    if angle < -100:
        angle = -100
    elif angle > 100:
        angle = 100
    prev_error = error

    rospy.loginfo(error)
    rospy.loginfo(distance_error)
    rospy.loginfo(angle)
    rospy.loginfo("-------------")

    ## END
    msg = drive_param()
    msg.velocity = vel_input
    msg.angle = angle
    pub.publish(msg)
Пример #31
0
    def control(data):
        global kp
        global kd
        global prev_angle
        global prev_error
        global vel_input
        ## Your code goes here
        # 1. Scale the error
        # data.pid_error *= 10
        # 2. Apply the PID equation on error to compute steering
        pid = data.pid_error * kp + kd * (data.pid_error - prev_error)
        print("pid: " + str(pid))
        # 3. Make sure the steering value is within bounds for talker.py
        angle = pid
        prev_error = data.pid_error

        print("angle:" + str(angle))

        if angle < -100:
            angle = -100
        if angle > 100:
            angle = 100

        prev_angle = angle

        msg = drive_param()
        msg.velocity = vel_input
        msg.angle = angle
        pub.publish(msg)
Пример #32
0
def control(data):
    global kp
    global kd
    global servo_offset
    global prev_error
    global vel_input
    global mode

    msg = drive_param();
    msg.velocity = vel_input

    if mode == 'wall':
        pid_error = data.pid_error
        error = pid_error * kp
        errordot = kd * (pid_error - prev_error)

        angle = error + errordot

        if angle > 100:
            angle = 100
        elif angle < -100:
            angle = -100

        prev_error = pid_error

        print 'pid_error {}\nangle {}'.format(pid_error, angle)

        msg.angle = angle
        
    elif mode == 'corner':
        print 'corner mode, angle 100'
        msg.angle = 100
    
    pub.publish(msg)
Пример #33
0
def control_callback(data):
  # TODO: Based on the error (data.data), determine the car's required velocity
  # amd steering angle.

	msg = drive_param()
	msg.velocity = 0.5  # TODO: implement PID for velocity
	msg.angle = 20.0    # TODO: implement PID for steering angle
	pub.publish(msg)
Пример #34
0
def control(data):
	global prev_error
	global vel_input
	global kp
	global kd

	## Your code goes here
	# 1. Scale the error
	# 2. Apply the PID equation on error
	# 3. Make sure the error is within bounds
 	

	## END

	msg = drive_param();
	msg.velocity = vel_input	
	msg.angle = angle
	pub.publish(msg)
Пример #35
0
def control(data):


	## Your code goes here
	# 1. Scale the error
	# 2. Apply the PID equation on error
	# 3. Make sure the error is within bounds
 	
 	error = data.error
 	vel_input = data.velocity
 	v_theta = clamp(kp * error + kd * (prev_error - error))
 	angle -= v_theta

	## END

	msg = drive_param();
	msg.velocity = vel_input	
	msg.angle = angle
	pub.publish(msg)
Пример #36
0
def control(data):
	global integral
	global prev_error
	global vel_input
	global kp
	global ki
	global kd
	global kd_vel
	global kp_vel
	velocity = data.pid_vel
	angle = servo_offset
	error = 5*data.pid_error
	print "Error Control",error
	if error!=0.0:
		# if abs(error - prev_error)>0.5: 	
		# 	integral = integral + error	
		control_error = kp*error + kd*(prev_error - error)# + ki*integral
		print "Control", control_error
		# integral = integral/1.3
		
		# print "Control error",control_error
		angle = angle + control_error*np.pi/180
		# print "Control error",control_error

		control_error_vel = kp_vel*error + kd_vel*(prev_error - error)
		# print "Control error velocity",control_error_vel

		# velocity = velocity - abs(control_error_vel)/10
		velocity = velocity - (control_error_vel)/1000
	# else:
		# angle = servo_offset
		

	prev_error = error

	# if angle<-100*np.pi/180:
	# 	angle = -100*np.pi/180
	# if angle>100*np.pi/180:
	# 	angle = 100*np.pi/180
	# angle = angle%360
	if angle > 30*np.pi/180:
		angle = 30*np.pi/180
	if angle < -30*np.pi/180:
		angle = -30*np.pi/180



	# print "Velocity",velocity
	print "Angle",angle*180/np.pi
	msg = drive_param()

	# velocity = 1
	# # if angle > 20*np.pi/180 or angle < -20*np.pi/180:
	# # 	velocity = 0.3

	if angle >= 10*np.pi/180 or angle <= -10*np.pi/180:
		velocity = 0.8

	if angle > 20*np.pi/180 or angle < -20*np.pi/180:
		velocity = 0.3

	if angle >= -1*np.pi/180 and angle <= 1*np.pi/180:
		velocity = 2.0

	if velocity < 0:
		velocity = 1

	if velocity > 2.5:
		velocity = 2.5



	print "Velocity",velocity
	print "Angle",angle


	msg.velocity = velocity
	msg.angle = angle
	pub.publish(msg)
Пример #37
0
	stdscr.refresh()
#	signal.alarm(0)
	if key == curses.KEY_UP: 
		forward = forward + 0.1;
		stdscr.addstr(2, 20, "Up  ")
		stdscr.addstr(2, 25, '%.2f' % forward)
		stdscr.addstr(5, 20, "    ")
	elif key == curses.KEY_DOWN:
		forward = forward - 0.1; 
		stdscr.addstr(2, 20, "Down")
		stdscr.addstr(2, 25, '%.2f' % forward)
		stdscr.addstr(5, 20, "    ")
	if key == curses.KEY_LEFT:
		left = left - 0.1; 
		stdscr.addstr(3, 20, "left")
		stdscr.addstr(3, 25, '%.2f' % left)
		stdscr.addstr(5, 20, "    ")
	elif key == curses.KEY_RIGHT:
		left = left + 0.1; 
		stdscr.addstr(3, 20, "rgt ")
		stdscr.addstr(3, 25, '%.2f' % left)
		stdscr.addstr(5, 20, "    ")
	if key == curses.KEY_DC:
		left = 0
		forward = 0
		stdscr.addstr(5, 20, "Stop")
	msg = drive_param()
	msg.velocity = forward
	msg.angle = left
	pub.publish(msg)
curses.endwin()