def callback(data):
theta = 50;
a = getRange(data, theta)
b = getRange(data, 0)
swing = math.radians(theta)
alpha = math.atan2( a * math.cos(swing) - b , a * math.sin(swing) )
AB = b * math.cos(alpha)
AC = 1
CD = AB + AC * math.sin(alpha)
#error = AB - desired_trajectory
error = CD - desired_trajectory
#ABprime = a * math.cos(math.asin(a * math.sin(swing) / math.sqrt(a*a + b*b - 2*a*b*math.cos(swing))) - math.pi / 2 + swing)
print "a {}\nb {}".format(a, b)
print "AB {}".format(AB)
#print "ABprime {}".format(ABprime)
print "error {}".format(error)
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def path_error(data):
global x1
global y1
global x2
global y2
global vel
y = data.pose.position.y
x = data.pose.position.x
l = 0.5
quaternion = (data.pose.orientation.x, data.pose.orientation.y,
data.pose.orientation.z, data.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
theta = (-1) * euler[2]
y = y + l * math.sin(theta)
x = x + l * math.cos(theta)
m = (y2 - y1) / (x2 - x1)
c = y1 - m * x1
error = (-1) * (y - m * x - c)
print "Error:", error
print "theta:", theta
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
global vel
theta = 50 # Angle to scan wall
prev_vel = vel # Previous velocity
vel = vel_input # Set velocity to chosen speed
a = getRange(data, theta)
b = getRange(data, 0)
swing = math.radians(theta)
## Your code goes here
# Stop car if something in front of it.
for x in numpy.arange(math.floor(90 - safety_angle_deg),
math.ceil(90 + safety_angle_deg), 1):
dist = getRange(data, int(round(x)))
if (dist * math.sin(math.radians(x)) < safety_dist and dist != -1):
vel = 0 # Stop car if something is in front of it (car_width)
print("STOP: ", dist, "Angle: ", x)
if (vel != 0 and prev_vel == 0):
vel = vel_input # Start driving (at vel_input) if nothing in front of car
alpha = round(math.atan((a * math.cos(swing) - b) / (a * math.sin(swing))),
2)
alpha_deg = round(math.degrees(alpha), 2)
AB = b * math.cos(alpha)
CD = AB + delay_dist * math.sin(alpha)
error = -round(desired_trajectory - CD, 2)
print("Alfa: ", alpha_deg, "Error: ", error)
## END
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
global error
a = getRange(data,50)
b = getRange(data,0)
c = getRange(data,40)
swing = math.radians(50)
alpha = math.atan((a*math.cos(swing)-b)/(a*math.sin(swing)))
curr_dist = b*math.cos(alpha)
future_dist = curr_dist+car_length*math.sin(alpha)
error = desired_trajectory - future_dist
detectTurn(alpha)
error = decideTurn_new(error)
# error = movingAverage(error)
# error= decideTurn(c-b,error)
# print("future distance: " + str(future_dist))
# print("current distance: " + str(curr_dist))
# print("error: " + str(error))
print("alpha: " + str(math.degrees(alpha)))
# print("Range at 0: " + str(b))
# print("Range at 40: " + str(c))
# print "Difference in ranges: ", c-b
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
dist_in_front = getRange(data, 180)
theta = 30
left_dist = getRange(data, 270)
right_dist = getRange(data, 90) # Ray perpendicular to right side of car
a_right = getRange(data,
(90 + theta)) # Ray at degree theta from right_dist
a_left = getRange(data, (270 - theta))
theta = math.radians(theta)
# Calculating the deviation of steering(alpha) from right
alpha_r = math.atan(
(a_right * math.cos(theta) - right_dist) / a_right * math.sin(theta))
curr_pos_r = right_dist * math.cos(alpha_r) # Present Position
fut_pos_r = curr_pos_r + car_length * math.sin(
alpha_r) # projection in Future Position
# Calculating the deviation of steering(alpha) from left
alpha_l = math.atan(
(a_left * math.cos(theta) - left_dist) / a_left * math.sin(theta))
curr_pos_l = left_dist * math.cos(alpha_l) # Present Position
fut_pos_l = curr_pos_l + car_length * math.sin(
alpha_l) # projection in Future Position
error = -(fut_pos_r - fut_pos_l) # total error
# print('error: ', error) #Testing
# Sending PID error to Control
msg = pid_input()
msg.pid_error = error
msg.pid_vel = dist_in_front # pid_vel used for distance in front
pub.publish(msg)
def callback(data): theta = 50; #omega = 90; a = getRange(data,theta) b = getRange(data,0) swing = math.radians(theta) print(b) ## Your code goes here #tangle = (a * np.cos(theta) - b) / a * np.sin(theta) #alpha = np.arctan(tangle) #AB = b * np.cos(alpha) #AC = 10 # 0.5 #CD = AB + AC * np.sin(alpha) #error = CD - 1 if (b < 1): error = -0.4 if (b > 1): error = 0.4 vel = 8 #frontDist = getRange(data, omega) #if frontDist <= 1: # vel = 8 #else: # vel = 8 ## END msg = pid_input() msg.pid_error = error msg.pid_vel = vel pub.publish(msg)
def callback(data):
b1 = getRange(data,0) # Note that the 0 implies a horizontal ray..the actual angle for the LIDAR may be 30 degrees and not 0
a1 = getRange(data,theta1)
err1 = calc_error(desired_trajectory, theta1, b1, a1 )
b2 = getRange(data,0) # Note that the 0 implies a horizontal ray..the actual angle for the LIDAR may be 30 degrees and not 0
a2 = getRange(data,theta2)
err2 = calc_error(desired_trajectory, theta2, b2, a2 )
# print("a: " + str(a))
# print("b: " + str(b))
# print("swing: " + str(swing))
# print("alpha: " + str(alpha))
# print("AB: " + str(AB))
# print("CD: " + str(CD))
print("error1: " + str(err1))
print("error2: " + str(err2))
error = .35*err1 + .65*err2
# TODO: if EC then take into account prev err and change car_length
msg = pid_input()
msg.pid_error = error*-1 # this is the error that you wantt o send to the PID for steering correction.
msg.pid_vel = vel # velocity error is only provided as an extra credit field.
pub.publish(msg)
def callback(data):
theta = 100
a = getRange(data, theta)
b = getRange(
data, 35
) # Note that the 0 implies a horizontal ray..the actual angle for the LIDAR may be 30 degrees and not 0.
swing = math.radians(theta)
## Your code goes here to compute alpha, AB, and CD..and finally the error.
alpha = math.atan((a * math.cos(swing) - b) / (a * math.sin(swing)))
AB = b * math.cos(alpha)
AC = 1.5
AD = getRange(data, theta)
#assuming AC is distance from car to another wall at angle alpha
#CD = AB + (AC * math.sin(alpha))
CD = AD * math.cos(math.radians(65))
error = desired_trajectory - CD
rospy.loginfo('ab:' + str(AB) + ' cd:' + str(CD))
## END
msg = pid_input()
msg.pid_error = error # this is the error that you wantt o send to the PID for steering correction.
msg.pid_vel = vel # velocity error is only provided as an extra credit field.
pub.publish(msg)
def callback(data):
theta = 50
omega = 90
a = getRange(data, theta)
b = getRange(data, 0)
swing = math.radians(theta)
print(b)
## Your code goes here
tangle = (a * np.cos(theta) - b) / a * np.sin(theta)
alpha = np.arctan(tangle)
AB = b * np.cos(alpha)
AC = 0.5
CD = AB + AC * np.sin(alpha)
error = CD - 1
frontDist = getRange(data, omega)
if frontDist <= 1:
vel = 0
else:
vel = 8
## END
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def path_error(data): global x1 global y1 global x2 global y2 global vel y = data.pose.position.y x = data.pose.position.x l = 0.5; quaternion = (data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] theta = (-1)*euler[2] y = y + l*math.sin(theta) x = x + l*math.cos(theta) m = (y2-y1) / (x2-x1) c = y1 - m*x1 error = (-1)*(y - m*x - c) print "Error:", error print "theta:", theta msg = pid_input(); msg.pid_error = error msg.pid_vel = vel pub.publish(msg)
def callback(data):
theta = 50;
a = getRange(data,theta)
b = getRange(data,0)
swing = math.radians(theta)
## Your code goes here
print("dist[0]", b)
print("dist[50]", a)
current_time = datetime.now()
AC = vel * current_time - previous_time
alpha = math.atan(((a * math.cos(theta)) - b)/(a * math.sin(theta)))
AB = b * math.cos(alpha)
CD = AB + (AC * math.sin(alpha))
## END
error = CD - 1
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
prev_time = current_time
def callback(data):
theta = 45
a = getRange(data, theta) # distance at angle
b = getRange(data, 0) # distance to wall
c = getRange(data, 90) # distance straight ahead
# print "a: ", a, "b: ", b
# ## Your code goes here
error = calc_error(theta, a, b)
vel = 20 - 1.6 * abs(error)
# if c < 1.7 or a < .5:
if c < 1.7 or a < .7: # if wall within 1.7 units or wall at angle within .7
error *= 4 # multiply error
vel = 8 # set velocity to min
# 8 = lowest forwards
# -10 = lowest backwards
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
theta = 60 # might want to change this
omega = 90
vel = 15
a = getRange(data, theta)
b = getRange(data, 0)
c = getRange(data, omega)
print("b: ", b)
print("omega: ", c)
swing = math.radians(theta)
alpha = math.atan2(a * math.cos(swing) - b, a * math.sin(swing))
AB = b * math.cos(alpha)
AC = 1
CD = AB + AC * math.sin(alpha)
error = CD - 1
# alpha = math.atan2(a * math.cos(swing) - b, a * math.sin(swing))
# aTob = b * math.cos(alpha)
if c < .8:
print("obstruction! vel = 0")
vel = 0
else:
vel = 10
# aToc = 1
# cTod = aTob + aToc * math.sin(alpha)
# error = cTod - desired_trajectory
# print(error)
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
#Variable L used as fix distance to drive with current direction.
global L
front_min = 2
theta = 30
# Ranges[] contains distances from car to the wall with different angles. Index 180 refers to the distance
# that always makes an angle of 90 degrees relative the cars direction. The jump between two elements are 0.25
# degrees. Index 380 = 50 degrees.
a = data.ranges[300]
b = data.ranges[180]
front = data.ranges[720]
swing = math.radians(theta)
#Compute angle alpha, distance AB and CD.
alpha = math.atan((a * math.cos(swing) - b) / (a * math.sin(swing)))
AB = b * math.cos(alpha)
CD = AB + L * math.sin(alpha)
#Converts alpha to degrees, is used to print out on screen.
alpha_deg = math.degrees(alpha)
error = CD - desired_trajectory
print("a: {}, b: {}, alpha: {}, error: {}".format(
a, b, alpha_deg, error))
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
global SPEED_FACTOR, AC, CENTER, SIDE
print(SIDE)
theta = 50
swing = math.radians(theta)
a = getRange(data, SIDE * (-pi / 2 + swing))
b = getRange(data, SIDE * (-pi / 2))
alpha = SIDE * atan((a * cos(swing) - b) / (a * sin(swing)))
if (CENTER == None):
CENTER = b * cos(alpha)
## Your code goes here
AB = b * cos(alpha)
CD = AB + (SIDE * SPEED_FACTOR * (AC * sin(alpha)))
error = CENTER - CD
## END
msg = pid_input()
msg.pid_error = error
print(CENTER, AB)
#msg.pid_vel = vel
pub.publish(msg)
def callback(data):
global vel
theta = 45.0
a = getRange(data, theta)
b = getRange(data, 0.0)
swing = math.radians(theta)
# compute the desired distance
alpha = math.atan((a * math.cos(swing) - b) / (a * math.sin(swing)))
dist_AB = b * math.cos(alpha)
# distance of projection is how much the car moves in 0.1 seconds
dist_AC = vel * 0.1
dist_CD = dist_AB + dist_AC * math.sin(alpha)
error = desired_trajectory - dist_CD
if math.isnan(error):
print 'nan occured:', a, b
error = 0.0
vel = base_vel * 1.0 / (vel_scale * abs(error) + 1)
print vel
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
for th, mindist in obstacle_check:
dist = getRange(data, th)
if dist < mindist:
print 'too close (%f m) at degree %f' % (dist, th)
msg.pid_vel = 0.0
break
pub.publish(msg)
def callback(data):
theta = 65
a = getRange(data, theta)
b = getRange(data, 15)
swing = math.radians(50)
## Your code goes here
intermed_top = a * math.cos(swing) - b
intermed_bot = a * math.sin(swing)
arctan_arg = intermed_top / intermed_bot
alpha = math.atan(arctan_arg)
AB = b * math.cos(alpha)
CD = AB + 1 * math.sin(alpha)
error = CD - desired_trajectory
#error = AB - desired_trajectory
#print(error)
## END
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data): global error global alpha print " " global flag_obstacle global final_desired_trajectory global final_direction global prev_direction global flag_left # a = getRange(data,50) # b = getRange(data,0) # # c = getRange(data,40) # swing = math.radians(50) # alpha = math.atan((a*math.cos(swing)-b)/(a*math.sin(swing))) # if flag_obstacle == 0: start_point,end_point = obs_decide(data) # else: # start_point = -1 # end_point = -1 desired_trajectory, direction = decide_obstacle_direction(data,start_point,end_point) # if desired_trajectory!=1.0: # flag_obstacle = 1 if direction == 0: flag_left = 1 elif direction == 2: flag_left = 0 if flag_left == 1: direction = 0 if direction == 0: final_desired_trajectory = 0.4 final_direction = direction elif desired_trajectory != -1: final_desired_trajectory = desired_trajectory final_direction = direction car_dist_right = getRange(data,0)*math.cos(alpha) car_dist_left = getRange(data,179)*math.cos(alpha) if decideReturn(start_point,end_point) == 1:# and (car_dist_left + car_dist_right) > 1.0: print "reset done" final_desired_trajectory = 0.7 final_direction = 1 #flag_obstacle = 0 #final_direction= 0 #final_desired_trajectory = 1 print "Final Desired",final_desired_trajectory print "new code" if final_direction == 0: error = followLeft(data,final_desired_trajectory) else: error = followRight(data,final_desired_trajectory) msg = pid_input() msg.pid_error = error msg.pid_vel = vel pub.publish(msg)
def scan_callback(data):
desired_distance = 0.30 #The distance entere here will be increased by 0.15 further in the algo so, 0.15 means 0.3
#Right follow function
#error, vel = followRight(data, desired_distance)
error, vel = followLeft(data, desired_distance)
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def scan_callback(data):
desired_distance = 2.5
#error, vel = followRight(data, desired_distance)
#error, vel = followLeft(data, desired_distance)
error, vel = followCenter(data, desired_distance)
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
"""
theta = 360;
a = getRange(data,theta)
b = getRange(data,0)
swing = math.radians(theta)
## Your code goes here
error = a-b
if(getRange(data,540)<.75):
vel = 0
else:
vel = 8
if getRange(data, 360) > 5: # Wide turn
error = -100
if getRange(data,360) > 5:
error = 100
else:
b = 50
c = 50
for i in range(170, 450, 10):
b2 = getRange(i)*math.cos(math.radians((i-180)/4))
if b2<b:
b = b2
c = getRange(i)*math.sin(math.radians((i-180)/4))
error = (b-.5)/c
"""
sum = 0
# print(data.intensities[180])
for i in range(180, 500, 10):
#if(data.intensities[i]<.9):
sum += (getRange(data, i) * math.cos(math.radians(
(i - 180) / 4)) - .75) / getRange(data, i) * math.sin(
math.radians((i - 180) / 4))
#else:
# sum = sum + (getRange(data,i)*math.cos(math.radians((i-180)/4))-.5-getRange(180)-getRange(900))/getRange(data,i)*math.sin(math.radians((i-180)/4))
error = sum / 36
if (getRange(data, 540) < .75):
vel = 0
else:
vel = 8
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def wallFollower(scan):
# Get distances directly left/right of car
distRight = getRange(scan,0) # was 0
distLeft = getRange(scan,180) # was 180
distForward = getRange(scan,90)
# Pick which wall to follow
# If the left wall is closer, follow left wall
if distRight > distLeft:
error = getLeftError(distLeft)
print("Following Left")
print("dist: ", distLeft)
print("error: ", error)
# If the right wall is closer, follow the right wall
else:
error = getRightError(distRight)
print(" Following Right")
print("dist: ", distRight)
print("error: ", error)
# If we're about to go straight into wall we need a behavior separate from the wall following
# we've been doing. If distForward is < some value we'll check if left or right is closer
# Then turn away from the closer one
if distForward < 2:
# Since this function only passes an error, we'll pass a specific error to let
# pdControl.py know distForward is small. Send a separate one for if left/right wall
# is closer
if distLeft < distRight:
error = 1234567 # send this specific number if we want to turn right
else:
error = 12345 # send this specific number if we want to turn left
# If we get to an area where distLeft and distRight are both out of range just turn left
# If the track we were going on was clockwise as opposed to counter clock wise we would turn right
# so we would go in the right direction
# if distRight == inf and distLeft == inf: # FIND OUT HOW TO CHECK FOR INF
# error = 12345
# Publish error
msg = pid_input()
msg.pid_error = error
msg.pid_vel = 1 # doesnt matter since we change it in pdControl.py
pub.publish(msg)
def callback(data):
global error
global alpha
#print " "
global flag_obstacle
global final_desired_trajectory
global final_direction
global prev_direction
global flag_left
# Get start_point, end_point, the min/max angles in the /scan range (defined in obs_decide function) that had a small dist reading
#start_point,end_point = obs_decide(data)
# Get trajectory, which is a distance, and direction: center, right, or left based on the obstacles in the laser range
# start_point, end_point
#final_desired_trajectory, direction = decide_obstacle_direction(data,start_point,end_point)
# OK so it seems like final_desired_trajectory and direction are not even used which means start_point, end_point are not even used
# So yea
# Get errror from the right and current distance from the right. Desired distance from right is 2
error_right, curr_dist_right = followRight(data, 2.)
# Get error from left and current distance from the left. Desired distance from left is 1
error_left, curr_dist_left = followLeft(data, 1.)
# Get error from center and current center distance. Desired distance is 1
error_center, curr_dist_center = followCentre(data, 1.)
# If distance to the left is smaller do wall following w/ the left wall (pid to get to desired distance from left wall)
if curr_dist_right >= curr_dist_left:
error = error_left
print 'Following Left'
print 'Error', error
# else if distance to right is smaller do wall following w/ the right wall (pid to get desired distance from right wall)
else:
error = error_right
print 'Following Right'
print 'Error', error
#print 'Is error same?', error
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
print ' '
def callback(data):
"""
theta = 360;
a = getRange(data,theta)
b = getRange(data,0)
swing = math.radians(theta)
## Your code goes here
error = a-b
if(getRange(data,540)<.75):
vel = 0
else:
vel = 8
if getRange(data, 360) > 5: # Wide turn
error = -100
if getRange(data,360) > 5:
error = 100
else:
b = 50
c = 50
for i in range(170, 450, 10):
b2 = getRange(i)*math.cos(math.radians((i-180)/4))
if b2<b:
b = b2
c = getRange(i)*math.sin(math.radians((i-180)/4))
error = (b-.5)/c
"""
sum = 0
# print(data.intensities[180])
for i in range(180, 500, 10):
#if(data.intensities[i]<.9):
sum += (getRange(data,i)*math.cos(math.radians((i-180)/4))-.75)/getRange(data,i)*math.sin(math.radians((i-180)/4))
#else:
# sum = sum + (getRange(data,i)*math.cos(math.radians((i-180)/4))-.5-getRange(180)-getRange(900))/getRange(data,i)*math.sin(math.radians((i-180)/4))
error = sum / 36
if(getRange(data,540)<.75):
vel = 0
else:
vel = 8
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
theta = 50
a = getRange(data, theta)
b = getRange(
data, 0
) # Note that the 0 implies a horizontal ray..the actual angle for the LIDAR may be 30 degrees and not 0.
swing = math.radians(theta)
## Your code goes here to compute alpha, AB, and CD..and finally the error.
## END
msg = pid_input()
msg.pid_error = error # this is the error that you wantt o send to the PID for steering correction.
msg.pid_vel = vel # velocity error is only provided as an extra credit field.
pub.publish(msg)
def callback(data): theta = 50; a = getRange(data,theta) b = getRange(data,0) swing = math.radians(theta) ## Your code goes here ## END msg = pid_input() msg.pid_error = error msg.pid_vel = vel pub.publish(msg)
def callback(data): global forward_projection theta = 70 # you need to try different values for theta a = getRange(data,theta) # obtain the ray distance for theta b = getRange(data,0) # obtain the ray distance for 0 degrees (i.e. directly to the right of the car) swing = math.radians(theta) ## Your code goes here to determine the error as per the alrorithm # Compute Alpha, AB, and CD..and finally the error. # TODO: implement msg = pid_input() # An empty msg is created of the type pid_input # this is the error that you want to send to the PID for steering correction. msg.pid_error = error msg.pid_vel = vel # velocity error can also be sent. pub.publish(msg)
def callback(data):
theta = 40
theta_plus90 = theta + 90
a = getRange(data, theta)
b = getRange(data, 0)
bb = getRange(data, theta_plus90)
swing = math.radians(theta)
alpha = math.atan2(a * math.cos(swing) - b, a * math.sin(swing))
AB = b * math.cos(alpha)
AC = 1
CD = AB + AC * math.sin(alpha)
#error = AB - desired_trajectory
error = CD - desired_trajectory
#ABprime = a * math.cos(math.asin(a * math.sin(swing) / math.sqrt(a*a + b*b - 2*a*b*math.cos(swing))) - math.pi / 2 + swing)
print "a {}\nb {} bb {}".format(a, b, bb)
print "AB {}".format(AB)
#print "ABprime {}".format(ABprime)
print "error {}".format(error)
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
### Publish not too many -> It will make SYNC LOSS in Teensy (BK v0.11)
global rate
rate += 1
if (rate % 10 == 0):
pub.publish(msg)
rate = 0
def callback(data):
theta = 50;
a = getRange(data, theta)
b = getRange(data, 0)
swing = math.radians(theta)
print a,b
## Your code goes here
alpha = math.atan((a * math.cos(swing) - b)/(a * math.sin(swing)))
AB = b * math.cos(alpha)
AC = 0.1 # need to change
CD = AB + AC * math.sin(alpha)
error = CD - 1 # need to change
print error
## END
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
global error
global detected_opening
global independence_time # amount of time before can fetch next turn instruction
global velocity
global turn
global instruction_index
print " "
detected_opening = detectOpening(data)
print "Detected opening? ", detected_opening
current_time = rospy.get_time()
# If detected new opening for the first time:
if detected_opening == True and current_time >= independence_time:
# set independence_time to the full timer duration
independence_time = TURN_DELAY + current_time
if turn != "stop":
instruction_index += 1 # so that next time detecting opening will call next instruction
turn = INSTRUCTIONS[instruction_index][0] # get first item of tuple
if turn == "stop":
velocity = 0.0
else:
velocity = TURN_VELOCITY
# Case where robot is free to move because it has already exceeded the previously set timer
elif current_time >= independence_time and instruction_index >= 0:
velocity = float(
INSTRUCTIONS[instruction_index][1]) # get second item of tuple
turn = "center"
# Regardless of which case above, robot will be doing a wall follow (left, center, or right)
error = followWallInDirection(turn, data)
print "On turn instruction index:", instruction_index
print "Turn: ", turn
msg = pid_input()
msg.pid_error = error
msg.pid_vel = velocity
pub.publish(msg)
def callback(data):
theta = 50
a = getRange(data, theta)
b = getRange(data, 0)
aL = getRange(data, 130)
bL = getRange(data, 180)
swing = math.radians(theta)
swingL = math.radians(130)
alpha = math.atan2(a * math.cos(swing) - b, a * math.sin(swing))
# alphaL = math.atan2( aL * math.cos(swingL) - bL , aL * math.sin(swingL) )
alphaL = math.atan2(aL * math.cos(swing) - bL, aL * math.sin(swing))
# alphaL = alphaL - 0.69
AB = b * math.cos(alpha)
ABL = bL * math.cos(alphaL)
AC = 1.0
CD = AB + AC * math.sin(alpha)
CDL = ABL + AC * math.sin(alphaL)
error = CD - desired_trajectory
errorL = CDL - desired_trajectory
errorL *= -1
print "alpha {} alphaL {}".format(alpha, alphaL)
print "a {} aL {}\nb {} bL {}".format(a, aL, b, bL)
print "AB {} ABL {}".format(AB, ABL)
print "CD {} CDL {}".format(CD, CDL)
print "error {} errorL {}".format(error, errorL)
msg = pid_input()
# msg.pid_error = error
msg.pid_error = errorL
msg.pid_vel = vel
global rate
rate += 1
if (rate % 10 == 0):
# Publish not too many -> It will make SYNC LOSS in Teensy (BK v0.11)
pub.publish(msg)
def callback(data):
theta = 50
a = getRange(data, theta)
# Note that the 0 implies a horizontal ray..the actual angle for the LIDAR may be 30 degrees and not 0.
b = getRange(data, 0)
swing = math.radians(theta)
# Your code goes here to compute alpha, AB, and CD..and finally the error.
alpha = math.atan2(a * math.cos(swing) - b, a * math.sin(swing))
AB = b * math.cos(alpha)
AC = 1
CD = AB + AC * math.sin(alpha)
error = CD - desired_trajectory
print "a {}\nb {}".format(a, b)
print "AB {}".format(AB)
print "error {}".format(error)
# END
msg = pid_input()
msg.pid_error = error # this is the error that you wantt o send to the PID for steering correction.
msg.pid_vel = vel # velocity error is only provided as an extra credit field.
pub.publish(msg)
def callback(data):
theta = 50
a = getRange(data, theta)
b = getRange(data, 0)
swing = math.radians(theta)
print "a -> {}\nb -> {}".format(a, b)
print "Swing -> {}".format(swing)
ABangle = math.atan2(a * math.cos(swing) - b, a * math.sin(swing))
AB = b * math.cos(ABangle)
print "AB -> {}".format(AB)
AC = 1
CD = AB + AC * math.sin(ABangle)
error = CD - desired_trajectory
print "Error -> {}".format(error)
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data): theta = 50; a = getRange(data,theta) b = getRange(data,0) swing = math.radians(theta) ## Your code goes here to compute alpha, AB, and CD..and finally the error. alpha = math.atan2((a*math.cos(swing) - b) , (a * math.sin(swing))) ab = b * math.cos(alpha) cd = ab + (car_length * math.sin(alpha)) error = cd - desired_trajectory print error ## END msg = pid_input() msg.pid_error = error msg.pid_vel = vel pub.publish(msg)
def callback(data):
global turnMode
corner_data = corner_service()
if(turnMode):
error = 1
if(not (corner_data.found)):
turnMode = False
else:
error = data.distance - desired_trajectory + data.theta
if(corner_data.found and corner_data.y < 0.3):
turnMode = True
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data):
theta = 40
a = getRange(data, theta)
b = getRange(
data, 0
) # Note that the 0 implies a horizontal ray..the actual angle for the LIDAR may be 30 degrees and not 0.
swing = math.radians(theta)
## Your code goes here to compute alpha, AB, and CD..and finally the error.
#rospy.loginfo("a: " + str(a))
#rospy.loginfo("b: " + str(b))
alpha = math.atan((a * math.cos(swing) - b) / (a * math.sin(swing)))
AB = b * math.cos(alpha)
CD = AB + car_length * math.sin(alpha)
error = desired_trajectory - CD
## END
msg = pid_input()
msg.pid_error = error # this is the error that you wantt o send to the PID for steering correction.
msg.pid_vel = vel # velocity error is only provided as an extra credit field.
pub.publish(msg)
def callback(data):
theta = 50
a = getRange(data,theta) #our Hypotenuse
b = getRange(data,0) #our straight to the right
c = getRange(data,40) #40 degree sample
z = getRange(data,60)
swing = math.radians(theta)
realHypotenuse = b/math.cos(swing) #calculated if we were straight
#math shit
#realDistance = doMath(theta, a, b, realHypotenuse)
#print("realDistance: ", realDistance)
## Your code goes here
print("dist[0]", b)
print("dist[40]",c)
print("dist[50]", a)
print("dist[60]", z)
#error = -217.12 * math.pow(dist, 3) + 455.952 * math.pow(dist, 2) - 406.506 * dist + 135.61
alpha = math.atan((a*math.cos(swing)-b)/(a*math.sin(swing)))
dist = b*math.cos(alpha)
addDist(dist)
average = avgDist()
print("distance", dist)
#function = 64.5136 * average ** 3 + 116.125 * average ** 2 + 185.694 * average - 139.287
#old
function = 482.565 * average ** 3 - 1013.39 * average ** 2 + 791.781 * average - 223.207
#function = 482.565 * math.pow(average,3) - 1013.39 * math.pow(average,2) + (791.781 * average) - 223.207
shouldTurn = False
if(a > z): #change us to turning state
shouldTurn = True
if(shouldTurn):
print("TURN TURN TURN")
error = 90
elif(a < realHypotenuse):
#angled towards the wall
print("Angled Right")
if(average < desired_trajectory):
#too close
print("Toooo Close")
#turn away from wall (left)
error = function
else:
#far away
#go straight
error = -.5 * function
elif(a >= realHypotenuse):
#angled away from wall
print("Angled Left")
if(average < desired_trajectory):
#too close
#go straight
error = -.5 * function
else:
#far away
print("Far Away")
#turn toward wall (right)
error = function
#error = dist - desired_trajectory
## END
msg = pid_input()
msg.pid_error = error
msg.pid_vel = vel
pub.publish(msg)
def callback(data): global error global alpha print " " global flag_obstacle global final_desired_trajectory global final_direction global prev_direction global flag_left # a = getRange(data,50) # b = getRange(data,0) # # c = getRange(data,40) # swing = math.radians(50) # alpha = math.atan((a*math.cos(swing)-b)/(a*math.sin(swing))) # if flag_obstacle == 0: start_point,end_point = obs_decide(data) # else: # start_point = -1 # end_point = -1 final_desired_trajectory, direction = decide_obstacle_direction(data,start_point,end_point) # if desired_trajectory!=1.0: # flag_obstacle = 1 # if direction == 0: # flag_left = 1 # elif direction == 2: # flag_left = 0 # if flag_left == 1: # direction = 0 # if direction == 0: # final_desired_trajectory = 0.4 # final_direction = direction # elif desired_trajectory != -1: # final_desired_trajectory = desired_trajectory # final_direction = direction # car_dist_right = getRange(data,0)*math.cos(alpha) # car_dist_left = getRange(data,179)*math.cos(alpha) # if decideReturn(start_point,end_point) == 1:# and (car_dist_left + car_dist_right) > 1.0: # print "reset done" # final_desired_trajectory = 0.8 # final_direction = 1 # #flag_obstacle = 0 # #final_direction= 0 # #final_desired_trajectory = 1 # print "Final Desired",final_desired_trajectory # print "new code" # if final_direction == 0: error_right, curr_dist_right = followRight(data,2.) error_left, curr_dist_left = followLeft(data,1.) error_center, curr_dist_center = followCentre(data,1.) if curr_dist_right >= curr_dist_left: error = error_left print 'Following Left' print 'Error', error else: # error = followRight(data,final_desired_trajectory) error = error_right print 'Following Right' print 'Error', error # if curr_dist_center: print 'Is error same?', error msg = pid_input() msg.pid_error = error msg.pid_vel = vel pub.publish(msg)
