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)