def calibrate_motors(): print("Running calibrate()") # This initializes all variables to 0. angle, error, i, total_left_speed, total_right_speed = (0, ) * 5 # Robot goes for a certain distance, based on the motors' "tics." left_motor.clear_tics() right_motor.clear_tics() m.activate_motors(int(-left_motor.base_power / 2), int(-right_motor.base_power / 2)) while abs(left_motor.get_tics() + right_motor.get_tics()) / 2 < 1000: KIPR.msleep(10) angle += (s.get_change_in_angle() - gyro_bias) * 10 error = 0.034470956 * angle # Positive error means veering left. Negative means veering right. left_speed = (-left_motor.base_power + error) / 2 right_speed = (-right_motor.base_power + error) / 2 total_left_speed += left_speed total_right_speed += right_speed i += 1 m.activate_motors(left_speed, right_speed) m.deactivate_motors() # All motor power variables are set based on the gyro drive in calibration. avg_left_speed = total_left_speed / i avg_right_speed = total_right_speed / i left_motor.set_all_powers(-avg_left_speed * 2) right_motor.set_all_powers(-avg_right_speed * 2) print("Finished Calibrating. Moving back into starting box...\n")
def backwards_gyro(time): angle = 0 error = 0 sec = seconds() + time / 1000.0 while seconds() < sec: left_speed = -c.BASE_LM_POWER + error right_speed = -c.BASE_RM_POWER - error m.activate_motors(left_speed, right_speed) msleep(10) angle += (gyro_z() - bias) * 10 error = 0.003830106222 * angle # Positive error means veering right. Negative means veering left. m.deactivate_motors()
def wfollow_left(time, refresh_rate=c.LFOLLOW_REFRESH_RATE): print "Starting wfollow_left()" sec = seconds() + time / 1000.0 while seconds() < sec: if BumpedLeft() or BumpedLightLeft() or BumpedLightFrontLeft(): m.activate_motors(c.BASE_LM_POWER, int(c.LFOLLOW_SMOOTH_RM_POWER * 0.6)) else: m.activate_motors(int(c.LFOLLOW_SMOOTH_LM_POWER * 0.5), c.BASE_RM_POWER) msleep(refresh_rate) m.deactivate_motors()
def calibrate(): # Code to calibrate the bw values. This goes before every run. Ends with light sensor calibration. max_sensor_value_right = 0 min_sensor_value_right = 90000 max_sensor_value_left = 0 min_sensor_value_left = 90000 max_sensor_value_third = 0 min_sensor_value_third = 90000 cmpc(c.LEFT_MOTOR) cmpc(c.RIGHT_MOTOR) if c.IS_MAIN_BOT: calibrate_tics = 2000 else: # Clone bot calibrate_tics = 2500 print "Running calibrate()" m.activate_motors(int (c.BASE_LM_POWER / 3), int(c.BASE_RM_POWER / 3)) while gmpc(c.LEFT_MOTOR) < calibrate_tics and gmpc(c.RIGHT_MOTOR) < calibrate_tics: if analog(c.RIGHT_TOPHAT) > max_sensor_value_right: max_sensor_value_right = analog(c.RIGHT_TOPHAT) if analog(c.RIGHT_TOPHAT) < min_sensor_value_right: min_sensor_value_right = analog(c.RIGHT_TOPHAT) if analog(c.LEFT_TOPHAT) > max_sensor_value_left: max_sensor_value_left = analog(c.LEFT_TOPHAT) if analog(c.LEFT_TOPHAT) < min_sensor_value_left: min_sensor_value_left = analog(c.LEFT_TOPHAT) if analog(c.THIRD_TOPHAT) > max_sensor_value_third: max_sensor_value_third = analog(c.THIRD_TOPHAT) if analog(c.THIRD_TOPHAT) < min_sensor_value_third: min_sensor_value_third = analog(c.THIRD_TOPHAT) msleep(1) m.deactivate_motors() c.LEFT_TOPHAT_BW = int(((max_sensor_value_left + min_sensor_value_left) / 2)) - 1000 c.RIGHT_TOPHAT_BW = int(((max_sensor_value_right + min_sensor_value_right) / 2)) - 1000 if c.IS_MAIN_BOT: c.THIRD_TOPHAT_BW = int(((max_sensor_value_third + min_sensor_value_third) / 2)) else: # Clone bot c.THIRD_TOPHAT_BW = int(((max_sensor_value_third + min_sensor_value_third) / 2)) + 300 print "max_sensor_value_left: " + str(max_sensor_value_left) print "min_sensor_value_left: " + str(min_sensor_value_left) print "Target Left " + str(c.LEFT_TOPHAT_BW) print "max_sensor_value_right: " + str(max_sensor_value_right) print "min_sensor_value_right: " + str(min_sensor_value_right) print "Target Right " + str(c.RIGHT_TOPHAT_BW) print "max_sensor_value_third: " + str(max_sensor_value_third) print "min_sensor_value_third: " + str(min_sensor_value_third) print "Target Third " + str(c.THIRD_TOPHAT_BW) f.backwards_through_two_lines_in_calibration() f.align_close() m.backwards(600) msleep(25) ao() msleep(1000)
def wfollow_right_until_white_right(time=15000, refresh_rate=c.LFOLLOW_REFRESH_RATE): print "Starting wfollow_right_until_white_right()" sec = seconds() + time / 1000.0 while seconds() < sec and BlackRight(): if BumpedRight() or BumpedLightRight() or BumpedLightFrontRight(): m.activate_motors(int(c.LFOLLOW_SMOOTH_RM_POWER * 0.6), c.BASE_RM_POWER) else: m.activate_motors(c.BASE_LM_POWER, int(c.LFOLLOW_SMOOTH_RM_POWER * 0.5)) msleep(refresh_rate) m.deactivate_motors()
def forwards_gyro(time, should_stop=True): angle = 0 error = 0 sec = seconds() + time / 1000.0 while seconds() < sec: left_speed = c.BASE_LM_POWER + error right_speed = c.BASE_RM_POWER - error m.activate_motors(left_speed, right_speed) msleep(10) angle += (gyro_z() - u.gyro_bias) * 10 error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right. if should_stop: m.deactivate_motors()
def backwards_gyro_until_pressed_bump_switch(time=c.SAFETY_TIME): angle = 0 error = 0 if time == 0: time = c.SAFETY_TIME_NO_STOP sec = seconds() + time / 1000.0 while seconds() < sec and s.isBumpSwitchNotPressed(): left_speed = -c.BASE_LM_POWER + error right_speed = -c.BASE_RM_POWER - error m.activate_motors(left_speed, right_speed) msleep(10) angle += (gyro_z() - bias) * 10 error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right. if time != c.SAFETY_TIME_NO_STOP: m.deactivate_motors()
def forwards_gyro_until_black_cliffs(time=c.SAFETY_TIME): angle = 0 error = 0 if time == 0: time = c.SAFETY_TIME_NO_STOP sec = seconds() + time / 1000.0 while seconds() < sec and s.isRightOnWhite() and s.isLeftOnWhite(): left_speed = c.BASE_LM_POWER + error right_speed = c.BASE_RM_POWER - error m.activate_motors(left_speed, right_speed) msleep(10) angle += (gyro_z() - bias) * 10 error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right. if time != c.SAFETY_TIME_NO_STOP: m.deactivate_motors()
def backwards_gyro_until_item_is_in_claw(time=c.SAFETY_TIME): angle = 0 error = 0 if time == 0: time = c.SAFETY_TIME_NO_STOP sec = seconds() + time / 1000.0 while seconds() < sec and s.isNothingInClaw(): left_speed = -c.BASE_LM_POWER + error right_speed = -c.BASE_RM_POWER - error m.activate_motors(left_speed, right_speed) msleep(10) angle += (gyro_z() - u.gyro_bias) * 10 error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right. if time != c.SAFETY_TIME_NO_STOP: m.deactivate_motors()
def lfollow_until(self, boolean_function, mode=c.STANDARD, bias=10, *, should_stop=True, time=c.SAFETY_TIME): target = 100.0 * (self.value_midpoint - self.get_black_value()) / ( self.get_white_value() - self.get_black_value()) + bias last_error = 0 integral = 0 if time == 0: should_stop = False sec = seconds() + time / 1000 while seconds() < sec and not (boolean_function()): norm_reading = 100.0 * (self.get_value() - self.get_black_value() ) / (self.get_white_value() - self.get_black_value()) error = target - norm_reading # Positive error means black, negative means white. derivative = error - last_error # If rate of change is going negative, need to veer left last_error = error integral = 0.5 * integral + error if error > 25 or error < -25: kp = 3.2 ki = c.KI kd = c.KD elif error < 10 and error > -10: kp = 0.1 ki = c.KI / 1000 kd = c.KD / 1000 else: kp = 0.35 ki = c.KI / 100 kd = c.KD / 100 left_power = c.BASE_LM_POWER + ( (kp * error) + (ki * integral) + (kd * derivative)) * self.side * mode right_power = c.BASE_RM_POWER - ( (kp * error) + (ki * integral) + (kd * derivative)) * self.side * mode m.activate_motors(int(left_power), int(right_power)) c.CURRENT_LM_POWER = left_power c.CURRENT_RM_POWER = right_power msleep(1) if should_stop: m.deactivate_motors()
def backwards_gyro(time, should_stop=True): angle = 0 error = 0 memory = 0 if time == 0: should_stop = False time = c.SAFETY_TIME sec = seconds().time + time / 1000.0 while seconds().time < sec: left_speed = -left_motor.base_power + (error + memory) right_speed = -right_motor.base_power - (error + memory) m.activate_motors(left_speed, right_speed) msleep(10) angle += (get_change_in_angle() - u.gyro_bias) * 10 error = 0.034470956 * angle # Positive error means veering right. Negative means veering left. memory += 0.001 * error if should_stop: m.deactivate_motors()
def backwards_gyro(time, should_stop=True): angle = 0 error = 0 memory = 0 if time == 0: should_stop = False time = c.SAFETY_TIME sec = seconds() + time / 1000.0 while seconds() < sec: left_speed = -c.BASE_LM_POWER + error + memory right_speed = -c.BASE_RM_POWER + error + memory m.activate_motors(left_speed, right_speed) msleep(10) angle += (get_change_in_angle() - bias) * 10 error = 0.034470956 * angle # Positive error means veering right. Negative means veering left. memory += 0.001 * error if should_stop: m.deactivate_motors()
def drive_gyro_until(boolean_function, time=c.SAFETY_TIME, should_stop=True): angle = 0 error = 0 memory = 0 if time == 0: should_stop = False time = c.SAFETY_TIME sec = seconds() + time / 1000.0 while seconds() < sec and not (boolean_function()): left_speed = left_motor.base_power + (error + memory) right_speed = right_motor.base_power - (error + memory) m.activate_motors(left_speed, right_speed) msleep(10) angle += (get_change_in_angle() - u.gyro_bias) * 10 error = 0.034470956 * angle # Positive error means veering left. Negative means veering right. memory += 0.00001 * error if should_stop: m.deactivate_motors()
def drive_gyro_until_black_right_or_fourth(time=c.SAFETY_TIME, should_stop=True): angle = 0 error = 0 memory = 0 if time == 0: should_stop = False time = c.SAFETY_TIME sec = seconds() + time / 1000.0 while seconds() < sec and s.isFourthOnWhite() and s.isRightOnWhite(): left_speed = c.BASE_LM_POWER + error right_speed = c.BASE_RM_POWER + error m.activate_motors(left_speed, right_speed) msleep(10) angle += (get_change_in_angle() - bias) * 10 error = 0.034470956 * angle # Positive error means veering left. memory += 0.001 * error if should_stop: m.deactivate_motors()
def forwards_gyro_wall_assisted_on_left(time=c.SAFETY_TIME, kp=1): angle = 0 error = 0 target_angle = 0 first_bump = False if time == 0: time = c.SAFETY_TIME_NO_STOP sec = seconds() + time / 1000.0 while seconds() < sec: if s.isRoombaBumped(): if first_bump == True: time_at_last_check = seconds() time_on_wall = 0 time_on_wall += (seconds() - time_at_last_check) time_at_last_check = seconds() u.halve_speeds() if s.isLeftBumped() and not ( s.isRightBumped()) and time_on_wall < 0.1: print "Left is bumped, early on the wall." target_angle -= 10 print "(Left) Target angle: " + str(target_angle) elif s.isLeftBumped() and not ( s.isRightBumped()) and time_on_wall >= 0.1: print "Left is bumped, late on wall." target_angle -= 2 u.normalize_speeds() print "(Left) Target angle: " + str(target_angle) else: print "Both bumped. Turning away faster." target_angle -= 70 print "(Left) Target angle: " + str(target_angle) first_bump = False else: first_bump = True u.normalize_speeds() left_speed = c.BASE_LM_POWER - error right_speed = c.BASE_RM_POWER + error m.activate_motors(left_speed, right_speed) msleep(10) angle += (gyro_z() - bias) * 10 error = target_angle - 0.003830106222 * kp * angle # Negative error means veering left. Positive means veering right. if time != c.SAFETY_TIME_NO_STOP: m.deactivate_motors()
def calibrate_tophats_and_motors(big_tophat_bias=-1000, small_tophat_bias=600): # Code to calibrate the tophat midpoint values and motor speeds that go straight. # If sensing black when it should be sensing white, increase bias # If sensing white when it should be sensing black, decrease bias print("Running calibrate()") angle = 0 error = 0 i = 0 total_left_speed = 0 total_right_speed = 0 # Robot goes for a certain distance, based on the motors' "tics." left_motor.clear_tics() right_motor.clear_tics() m.activate_motors(int(-left_motor.base_power / 2), int(-right_motor.base_power / 2)) while abs(left_motor.get_tics() + right_motors.get_tics()) / 2 < calibrate_tics: left_speed = (-left_motor.base_power + error) / 2 right_speed = (-right_motor.base_power + error) / 2 total_left_speed += left_speed total_right_speed += right_speed i += 1 m.activate_motors(left_speed, right_speed) msleep(10) angle += (s.get_change_in_angle() - gyro_bias) * 10 error = 0.034470956 * angle # Positive error means veering left. Negative means veering right. # For all tophats, the sensor finds the maximum and minimum values it goes over during the calibration sequence. for tophat in Tophat.all_tophats: tophat.compare_and_replace_extremes() m.deactivate_motors() for tophat in Tophat.all_tophats: if tophat.tophat_type == c.BIG_TOPHAT: tophat.determine_midpoint_from_extremes(big_tophat_bias) else: tophat.determine_midpoint_from_extremes(small_tophat_bias) # All motor power variables are set based on the gyro drive in calibration. avg_left_speed = total_left_speed / i avg_right_speed = total_right_speed / i left_motor.set_all_powers(-avg_left_speed * 2) right_motor.set_all_powers(-avg_right_speed * 2) print("Finished Calibrating. Moving back into starting box...\n")
def test_gyro(time=20000): print "Starting test_gyro" theta = 0 m.activate_motors(c.BASE_LM_POWER, c.BASE_RM_POWER * -1) sec = seconds() + time / 1000.0 turn_time = seconds() + c.RIGHT_TURN_TIME / 1000.0 while seconds() < sec: sec_2 = seconds() + 1 while seconds() < sec_2: msleep(10) if seconds() > turn_time: print "Turn time reached" m.deactivate_motors() print str(theta) print str(gyro_x() - c.AVG_BIAS) msleep(300) if c.CURRENT_LM_POWER == 0 and c.CURRENT_RM_POWER == 0: pass else: theta += (gyro_x() - c.AVG_BIAS) print str(theta) print str(gyro_x() - c.AVG_BIAS)
def calibrate_tophats(big_tophat_bias=-1000, small_tophat_bias=600): """Code to calibrate the tophat midpoint values.""" # If sensing black when it should be sensing white, increase bias. # If sensing white when it should be sensing black, decrease bias. print("Running calibrate()") # Robot goes for a certain distance, based on the motors' "tics." left_motor.clear_tics() right_motor.clear_tics() m.activate_motors(int(-left_motor.base_power / 2), int(-right_motor.base_power / 2)) while abs(left_motor.get_tics() + right_motor.get_tics()) / 2 < calibrate_tics: for tophat in Tophat.all_tophats: tophat.compare_and_replace_extremes() KIPR.msleep(1) m.deactivate_motors() for tophat in Tophat.all_tophats: if tophat.tophat_type == c.BIG_TOPHAT: tophat.determine_midpoint_from_extremes(big_tophat_bias) else: tophat.determine_midpoint_from_extremes(small_tophat_bias) print("Finished Calibrating. Moving back into starting box...\n")
def drive_gyro(time, kp=10, ki=.1, kd=1, stop=True): error = 0 last_error = 0 integral = 0 derivative = 0 first_run_through = True sec = seconds() + time / 1000.0 while seconds() < sec: error = (gyro_x() - c.AVG_BIAS) * c.WALLAGREES_TO_DEGREES_RATE derivative = error - last_error # If rate of change is going negative, need to veer left last_error = error integral = 0.5 * integral + error c.ROBOT_ANGLE += error left_power = c.BASE_LM_POWER + ((kp * error) + (ki * integral) + (kd * derivative)) right_power = c.BASE_RM_POWER + ( (kp * error) + (ki * integral) + (kd * derivative)) # Addition decreases power here if left_power > 1300: left_power = 1300 elif left_power < -1000: left_power = -1000 if right_power < -1300: right_power = -1300 elif right_power > 1000: right_power = 1000 if first_run_through == True: m.activate_motors( int(left_power), int(right_power) ) # this is where the accelerate command would go if we had one. else: m.activate_motors(int(left_power), int(right_power)) first_run_through = False msleep(c.GYRO_TIME) if stop == True: m.deactivate_motors()
def forwards_gyro_wall_assisted_on_right(time=c.SAFETY_TIME, kp=1): angle = 0 error = 0 target_angle = 0 if time == 0: time = c.SAFETY_TIME_NO_STOP sec = seconds() + time / 1000.0 while seconds() < sec: if s.isRoombaBumped(): u.halve_speeds() kp = 5 target_angle -= 0.1 print "(Right) Target angle: " + str(target_angle) else: u.normalize_speeds() kp = 1 left_speed = c.BASE_LM_POWER - error right_speed = c.BASE_RM_POWER + error m.activate_motors(left_speed, right_speed) msleep(100) angle += (gyro_z() - bias) * 100 error = target_angle - 0.003830106222 * kp * angle # Negative error means veering left. Positive means veering right. if time != c.SAFETY_TIME_NO_STOP: m.deactivate_motors()
def calibrate_motor_powers(): angle = 0 error = 0 i = 0 total_left_speed = 0 total_right_speed = 0 sec = seconds() + 3 while seconds() < sec: left_speed = c.BASE_LM_POWER + error right_speed = c.BASE_RM_POWER - error total_left_speed += left_speed total_right_speed += right_speed i += 1 m.activate_motors(left_speed, right_speed) msleep(10) angle += (gyro_z() - bias) * 10 error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right. m.deactivate_motors() avg_left_speed = total_left_speed / i avg_right_speed = total_right_speed / i c.BASE_LM_POWER = int(avg_left_speed) c.BASE_RM_POWER = int(avg_right_speed) print "c.BASE_LM_POWER: " + str(c.BASE_LM_POWER) print "c.BASE_RM_POWER: " + str(c.BASE_RM_POWER)
def calibrate(): max_sensor_value_rcliff = 0 min_sensor_value_rcliff = 90000 max_sensor_value_lcliff = 0 min_sensor_value_lcliff = 90000 max_sensor_value_lfcliff = 0 min_sensor_value_lfcliff = 90000 max_sensor_value_rfcliff = 0 min_sensor_value_rfcliff = 90000 sec = seconds() + 9 print "Running calibrate()" m.activate_motors(-1, int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2)) print str(int(c.BASE_LM_POWER / 2)) print str(int(c.BASE_RM_POWER / 2)) while seconds() < sec: if get_create_rcliff_amt() > max_sensor_value_rcliff: max_sensor_value_rcliff = get_create_rcliff_amt() if get_create_rcliff_amt() < min_sensor_value_rcliff: min_sensor_value_rcliff = get_create_rcliff_amt() if get_create_lcliff_amt() > max_sensor_value_lcliff: max_sensor_value_lcliff = get_create_lcliff_amt() if get_create_lcliff_amt() < min_sensor_value_lcliff: min_sensor_value_lcliff = get_create_lcliff_amt() if get_create_rfcliff_amt() > max_sensor_value_rfcliff: max_sensor_value_rfcliff = get_create_rfcliff_amt() if get_create_rfcliff_amt() < min_sensor_value_rfcliff: min_sensor_value_rfcliff = get_create_rfcliff_amt() if get_create_lfcliff_amt() > max_sensor_value_lfcliff: max_sensor_value_lfcliff = get_create_lfcliff_amt() if get_create_lfcliff_amt() < min_sensor_value_lfcliff: min_sensor_value_lfcliff = get_create_lfcliff_amt() msleep(1) m.deactivate_motors() c.LCLIFF_BW = ( (max_sensor_value_lcliff + min_sensor_value_lcliff) / 2) + 750 c.RCLIFF_BW = ( (max_sensor_value_rcliff + min_sensor_value_rcliff) / 2) + 750 c.LFCLIFF_BW = ( (max_sensor_value_lfcliff + min_sensor_value_lfcliff) / 2) + 770 c.RFCLIFF_BW = ( (max_sensor_value_rfcliff + min_sensor_value_rfcliff) / 2) + 1000 print "LCLIFF_BW: " + str(c.LCLIFF_BW) print "RCLIFF_BW: " + str(c.RCLIFF_BW) print "LFCLIFF_BW: " + str(c.LFCLIFF_BW) print "RFCLIFF_BW: " + str(c.RFCLIFF_BW) print "max_sensor_value_rcliff: " + str(max_sensor_value_rcliff) print "min_sensor_value_rcliff: " + str(min_sensor_value_rcliff) msleep(500) f.forwards_until_black_lfcliff() f.align_close_fcliffs() msleep(300) f.forwards_until_black_lcliff() f.align_close_cliffs() msleep(300) f.forwards_until_bump() m.backwards(100) #m.activate_motors(1) #msleep(4250) #m.deactivate_motors() msleep(1500) ao() create_disconnect() wait_for_light(c.LIGHT_SENSOR) create_connect() shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
def forwards_until_depth_senses_object(): m.activate_motors(1) while NotDepthSensesObject(): pass m.deactivate_motors()
def forwards_until_bump(): print "Starting forwards_until_bump()" m.activate_motors(1) while leftIsNotBumped() and rightIsNotBumped(): pass m.deactivate_motors()
def left_point_turn_until_lfcliff_senses_black(): m.activate_motors(1, -1 * c.BASE_LM_POWER, c.BASE_RM_POWER) while NotBlackFrontLeft(): pass m.deactivate_motors()
def backwards_until_white_lcliff(): print "Start drive_until_black_lcliff" m.activate_motors() while BlackLeft(): pass m.deactivate_motors()
def calibrate(): # Initialize variables. c.MIN_SENSOR_VALUE_LCLIFF = 90000 c.MAX_SENSOR_VALUE_LCLIFF = 0 c.MAX_SENSOR_VALUE_RCLIFF = 0 c.MIN_SENSOR_VALUE_RCLIFF = 90000 c.MAX_SENSOR_VALUE_LFCLIFF = 0 c.MIN_SENSOR_VALUE_LFCLIFF = 90000 c.MAX_SENSOR_VALUE_RFCLIFF = 0 c.MIN_SENSOR_VALUE_RFCLIFF = 90000 angle = 0 error = 0 total_left_speed = 0 total_right_speed = 0 run_throughs = 0 total_tophat_reading = 0 sec = seconds() + 3 print "Running calibrate()" m.activate_motors(int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2)) while seconds() < sec: if get_create_lcliff_amt() > c.MAX_SENSOR_VALUE_LCLIFF: c.MAX_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt() if get_create_lcliff_amt() < c.MIN_SENSOR_VALUE_LCLIFF: c.MIN_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt() if get_create_rcliff_amt() > c.MAX_SENSOR_VALUE_RCLIFF: c.MAX_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt() if get_create_rcliff_amt() < c.MIN_SENSOR_VALUE_RCLIFF: c.MIN_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt() if get_create_lfcliff_amt() > c.MAX_SENSOR_VALUE_LFCLIFF: c.MAX_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt() if get_create_lfcliff_amt() < c.MIN_SENSOR_VALUE_LFCLIFF: c.MIN_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt() if get_create_rfcliff_amt() > c.MAX_SENSOR_VALUE_RFCLIFF: c.MAX_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt() if get_create_rfcliff_amt() < c.MIN_SENSOR_VALUE_RFCLIFF: c.MIN_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt() total_tophat_reading += analog(c.CLAW_TOPHAT) left_speed = int(c.BASE_LM_POWER / 2) + error right_speed = int(c.BASE_RM_POWER / 2) - error m.activate_motors(left_speed, right_speed) total_left_speed += left_speed total_right_speed += right_speed run_throughs += 1 msleep(10) angle += (gyro_z() - g.bias) * 10 error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right. m.deactivate_motors() # If sensing black when should be sensing white, decrease bias. # If sensing white when should be sensing black, increase bias c.LCLIFF_BW = ( (c.MAX_SENSOR_VALUE_LCLIFF + c.MIN_SENSOR_VALUE_LCLIFF) / 2) + 100 c.RCLIFF_BW = ( (c.MAX_SENSOR_VALUE_RCLIFF + c.MIN_SENSOR_VALUE_RCLIFF) / 2) + 100 c.LFCLIFF_BW = ( (c.MAX_SENSOR_VALUE_LFCLIFF + c.MIN_SENSOR_VALUE_LFCLIFF) / 2) + 100 c.RFCLIFF_BW = ( (c.MAX_SENSOR_VALUE_RFCLIFF + c.MIN_SENSOR_VALUE_RFCLIFF) / 2) + 100 c.BASE_LM_POWER = int((total_left_speed * 2) / run_throughs) c.BASE_RM_POWER = int((total_right_speed * 2) / run_throughs) c.FULL_LM_POWER = c.BASE_LM_POWER c.FULL_RM_POWER = c.BASE_RM_POWER c.HALF_LM_POWER = int(c.BASE_LM_POWER) / 2 c.HALF_RM_POWER = int(c.BASE_RM_POWER) / 2 print "LCLIFF_BW: " + str(c.LCLIFF_BW) print "RCLIFF_BW: " + str(c.RCLIFF_BW) print "LFCLIFF_BW: " + str(c.LFCLIFF_BW) print "RFCLIFF_BW: " + str(c.RFCLIFF_BW) print "BASE_LM_POWER: " + str(c.BASE_LM_POWER) print "BASE_RM_POWER: " + str(c.BASE_RM_POWER) msleep(100) c.CLAW_TOPHAT_NOTHING_READING = total_tophat_reading / run_throughs #print "\nPut coupler in claw and press the right button..." #s.wait_until(isRightButtonPressed) #c.CLAW_TOPHAT_COUPLER_READING = analog(c.CLAW_TOPHAT) #c.CLAW_TOPHAT_BW = (c.CLAW_TOPHAT_COUPLER_READING + c.CLAW_TOPHAT_NOTHING_READING) / 2 #print "c.CLAW_TOPHAT_BW: " + str(c.CLAW_TOPHAT_BW) #if c.CLAW_TOPHAT_COUPLER_READING > c.CLAW_TOPHAT_NOTHING_READING: # print "Coupler reading is higher." #else: # print "Nothing reading is higher." s.backwards_until_black_cliffs() s.align_far_cliffs() s.backwards_through_line_lfcliff() s.align_close_fcliffs() m.backwards(200) msleep(300) ao() # DON'T DELETE THESE NEXT 4 LINES. They are purposeful. It avoids the roomba going into sleep mode after the calibration and not starting right. create_disconnect() wait_for_light(c.LIGHT_SENSOR) create_connect() shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
def test_veer(time=10000): m.activate_motors() msleep(time) m.deactivate_motors() sd()
def calibrate_with_gyro_angle_calibration(): # Initialize variables. c.MIN_SENSOR_VALUE_LCLIFF = 90000 c.MAX_SENSOR_VALUE_LCLIFF = 0 c.MAX_SENSOR_VALUE_RCLIFF = 0 c.MIN_SENSOR_VALUE_RCLIFF = 90000 c.MAX_SENSOR_VALUE_LFCLIFF = 0 c.MIN_SENSOR_VALUE_LFCLIFF = 90000 c.MAX_SENSOR_VALUE_RFCLIFF = 0 c.MIN_SENSOR_VALUE_RFCLIFF = 90000 angle = 0 error = 0 total_left_speed = 0 total_right_speed = 0 run_throughs = 0 sec = seconds() + 3 print "Running calibrate()" m.activate_motors(int(c.BASE_LM_POWER / 2), int(c.BASE_RM_POWER / 2)) while seconds() < sec: if get_create_lcliff_amt() > c.MAX_SENSOR_VALUE_LCLIFF: c.MAX_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt() if get_create_lcliff_amt() < c.MIN_SENSOR_VALUE_LCLIFF: c.MIN_SENSOR_VALUE_LCLIFF = get_create_lcliff_amt() if get_create_rcliff_amt() > c.MAX_SENSOR_VALUE_RCLIFF: c.MAX_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt() if get_create_rcliff_amt() < c.MIN_SENSOR_VALUE_RCLIFF: c.MIN_SENSOR_VALUE_RCLIFF = get_create_rcliff_amt() if get_create_lfcliff_amt() > c.MAX_SENSOR_VALUE_LFCLIFF: c.MAX_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt() if get_create_lfcliff_amt() < c.MIN_SENSOR_VALUE_LFCLIFF: c.MIN_SENSOR_VALUE_LFCLIFF = get_create_lfcliff_amt() if get_create_rfcliff_amt() > c.MAX_SENSOR_VALUE_RFCLIFF: c.MAX_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt() if get_create_rfcliff_amt() < c.MIN_SENSOR_VALUE_RFCLIFF: c.MIN_SENSOR_VALUE_RFCLIFF = get_create_rfcliff_amt() left_speed = int(c.BASE_LM_POWER / 2) + error right_speed = int(c.BASE_RM_POWER / 2) - error m.activate_motors(left_speed, right_speed) total_left_speed += left_speed total_right_speed += right_speed run_throughs += 1 msleep(10) angle += (gyro_z() - g.bias) * 10 error = 0.003830106222 * angle # Positive error means veering left. Negative means veering right. m.deactivate_motors() c.LCLIFF_BW = ( (c.MAX_SENSOR_VALUE_LCLIFF + c.MIN_SENSOR_VALUE_LCLIFF) / 2) + 100 c.RCLIFF_BW = ( (c.MAX_SENSOR_VALUE_RCLIFF + c.MIN_SENSOR_VALUE_RCLIFF) / 2) + 100 c.LFCLIFF_BW = ( (c.MAX_SENSOR_VALUE_LFCLIFF + c.MIN_SENSOR_VALUE_LFCLIFF) / 2) + 100 c.RFCLIFF_BW = ( (c.MAX_SENSOR_VALUE_RFCLIFF + c.MIN_SENSOR_VALUE_RFCLIFF) / 2) + 200 c.BASE_LM_POWER = int((total_left_speed * 2) / run_throughs) c.BASE_RM_POWER = int((total_right_speed * 2) / run_throughs) c.FULL_LM_POWER = c.BASE_LM_POWER c.FULL_RM_POWER = c.BASE_RM_POWER c.HALF_LM_POWER = int(c.BASE_LM_POWER) / 2 c.HALF_RM_POWER = int(c.BASE_RM_POWER) / 2 print "LCLIFF_BW: " + str(c.LCLIFF_BW) print "RCLIFF_BW: " + str(c.RCLIFF_BW) print "LFCLIFF_BW: " + str(c.LFCLIFF_BW) print "RFCLIFF_BW: " + str(c.RFCLIFF_BW) print "BASE_LM_POWER: " + str(c.BASE_LM_POWER) print "BASE_RM_POWER: " + str(c.BASE_RM_POWER) msleep(100) s.backwards_until_black_cliffs() s.align_far_cliffs() s.turn_left_until_lfcliff_senses_black(0) g.determine_gyro_conversion_rate() msleep(100) g.turn_right_gyro(45) s.backwards_until_black_lfcliff() s.align_far_fcliffs() s.backwards_through_line_lfcliff() s.align_close_fcliffs() m.backwards(200) msleep(300) ao() # DON'T DELETE THESE NEXT 4 LINES. They are purposeful. It avoids the roomba going into sleep mode after the calibration and not starting right. create_disconnect() msleep(4000) #wait_for_light(c.LIGHT_SENSOR) create_connect() shut_down_in(120) # URGENT: PUT BACK IN BEFORE COMPETITION
def forwards_until_black_lcliff(): print "Start drive_until_black_lcliff" m.activate_motors(1) while NotBlackLeft(): pass m.deactivate_motors()