def drive_timed(lmotor, rmotor, time): print("driving timed") _clear_ticks() end = seconds() + time if lmotor == 0 or rmotor == 0: print("please use pivot instead!") elif abs(rmotor) <= abs(lmotor): mod = rmotor / (lmotor * 1.0) newLeftSpeed = lmotor newRightSpeed = int(mod * lmotor) elif abs(lmotor) < abs(rmotor): mod = (lmotor * 1.0) / rmotor newLeftSpeed = int(mod * rmotor) newRightSpeed = rmotor while seconds() <= end: if int(_right_ticks() / mod) == int(_left_ticks() / mod): _drive(newLeftSpeed, newRightSpeed) if int(_right_ticks() / mod) > int(_left_ticks() / mod): _drive(newLeftSpeed, int(newRightSpeed / 1.3)) if int(_left_ticks() / mod) > int(_right_ticks() / mod): _drive(int(newLeftSpeed / 1.3), newRightSpeed) freeze_motors() print(get_motor_position_counter(RMOTOR)) ao()
def timedLineFollowLeftBack(time): # follows on starboard side sec = seconds() + time while seconds() < sec : if onBlackBack(): driveTimed(-90, -20, 20) else: driveTimed(-20, -90, 20) msleep(10)
def timedLineFollowLeftSmooth(time): sec = seconds() + time while seconds() < sec: if onBlackFront(): d.driveTimed(20, 40, 20) else: d.driveTimed(40, 20, 20) msleep(10)
def timedLineFollowRight(port, time): sec = seconds() + time while seconds() < sec: if not onBlack(port): driveTimed(20, 90, 20) else: driveTimed(90, 20, 20) msleep(10)
def timedLineFollowLeftSmooth(port, time): sec = seconds() + time while seconds() < sec: if onBlack(port): driveTimed(20, 40, 20) else: driveTimed(40, 20, 20) msleep(10)
def DEBUG(): freeze(c.LMOTOR) freeze(c.RMOTOR) ao() display('Program stop for DEBUG\nSeconds: {}'.format(seconds() - c.startTime)) display("NOTE: {}\t{}".format(seconds(), c.startTime)) exit(0)
def timedLineFollowLeft(time): sec = seconds() + time while seconds() < sec : if onBlackFront(): driveTimed(20, 90, 20) else: driveTimed(90, 20, 20) msleep(10)
def timedLineFollowBack(port, time): sec = seconds() + time while seconds() < sec: if onBlack(port): driveTimed(-90, -20, 20) else: driveTimed(-20, -90, 20) msleep(10)
def timedLineFollowRight(time): sec = seconds() + time while seconds() < sec: if not onBlackFront(): d.driveTimed(20, 90, 20) else: d.driveTimed(90, 20, 20) msleep(10)
def timedLineFollowRightSmooth(time): sec = seconds() + time while seconds() < sec: if not onBlackFront(): driveTimed(20, 40, 20) else: driveTimed(40, 20, 20) msleep(10)
def smoothLineFollowLeft(time, speed): #Max speed is 80 #Proportional adjustment LF sec = seconds() + time while seconds() < sec: num = ((w.analog(5) - 1500) / 120) d.driveTimed(speed-num, speed+num, 20) msleep(10)
def timedLineFollowLeftBack(time): # follows on starboard side sec = seconds() + time while seconds() < sec: if onBlackBack(): d.driveTimed(-90, -20, 20) else: d.driveTimed(-20, -90, 20) msleep(10)
def tempInit(): c.startTime = seconds() tempServos() moveOutrigger(c.outriggerIn, 100) moveArm(c.armUp, 10) msleep(500) moveClaw(c.clawMid, 15) msleep(500) binGrabUp() waitForButton() c.startTime = seconds()
def data_log(message, filename="general.log"): dt = seconds() - c.START_TIME FORMAT = '%(dt)s\t%(message)s' logging.basicConfig(filename=filename, format=FORMAT) data = {'dt': dt} # print(str(data) + " " + message) logging.warning(message, extra=data)
def DEBUGwithWait(): freeze(c.LMOTOR) freeze(c.RMOTOR) ao() print 'Program stop for DEBUG\nSeconds: ', seconds() - c.startTime msleep(5000) exit(0)
def init(): u.move_servo(c.servoClaw, c.clawOpen, 100) u.move_servo(c.servoArm, c.armDown, 100) u.move_servo(c.outrigger, c.outriggerIn, 10) #u.move_servo(c.servoCow,c.cowMid,100) enable_servos() u.waitForButton() c.startTime = seconds()
def DEBUG_WITH_WAIT(): freeze(c.LMOTOR) freeze(c.RMOTOR) ao() msleep(5000) display('Program stop for DEBUG\nSeconds: {}'.format(seconds() - c.startTime)) exit(0)
def init(): c.startTime = seconds() if c.isPrime: print "Running Tater - Prime" else: print "Running Tater - Clone" print c.armFront testSensors() testServos() testMotors() moveOutrigger(c.outriggerIn, 25) testET() disable_servos() #wait4light() waitForButton() shut_down_in(119.9) c.startTime = seconds() enable_servos()
def main(): u._init() u.shake_down() u.wait_4_light() w.shut_down_in(110) start_time = w.seconds() a.move_out_startbox() a.find_burning_center() a.move_to_cubes() # use camera to move to cubes a.get_cubes_num(5) # get 5 cubes (all cubes) a.move_to_med() a.return_to_med() end_time = w.seconds() print "%s medical center" % ("Close", "Far")[c.burning_center] print "%s the cubes" % ("NOT," "YES")[c.can_see] print "Program took %f seconds" % (end_time - start_time)
def calibrate3(): AMOUNT = 6000 _clear_ticks() _drive(50, 50) startR = seconds() while _right_ticks() < AMOUNT: pass endR = seconds() - startR freeze_motors() msleep(5000) _clear_ticks() _drive(50, 50) startL = seconds() while _left_ticks() < AMOUNT: pass endL = seconds() - startL print "Value: " + str(endL / endR)
def drive_timed_straight(speed, time, clear=False): print "corrected driving for time" scale = 1.3 sum = 0 max_error = 0 max_sum = 0 total_time = 0 number_times = 0 last_time = seconds() if clear: _clear_ticks() start_time = seconds() while seconds() <= time + start_time: left = _left_ticks() right = _right_ticks() error = right - left sum += error if abs(sum) > abs(max_sum): max_sum = sum if abs(error) > max_error: max_error = abs(error) p_scale = error * 0.018 i_scale = sum * 0.001 adjustment = p_scale + i_scale scale = abs(adjustment) + 1 if adjustment == 0: _drive(speed, speed) elif adjustment > 0: right = True _drive(speed, int(speed / scale)) elif adjustment < 0: if right: now = seconds() total_time += now - last_time last_time = now number_times += 1 print total_time / number_times right = False _drive(int(speed / scale), speed) print "error %s, sum %s" % (error, sum)
def drive_with_gyro(speed, time): theta = 0 start_time = w.seconds() while w.seconds - start_time < (time / 1000.0): if speed > 0: c.leftMotor.moveAtVelocity( int(speed - speed * (1.920137e-16 + 0.000004470956 * theta))) c.rightMotor.moveAtVelocity( int(speed + speed * (1.920137e-16 + 0.000004470956 * theta))) else: c.leftMotor.moveAtVelocity( int(speed + speed * (1.920137e-16 + 0.000004470956 * theta))) c.rightMotor.moveAtVelocity( int(speed - speed * (1.920137e-16 + 0.000004470956 * theta))) w.msleep(10) theta += (w.gyro_z() - bias) * 10
def log(message): print(message) datetime = strftime("%Y-%m-%d %H:%M:%S") if c.LOGFILE == "": name = "{} {}".format(c.ROBOT_NAME, datetime) path = "/home/root/Documents" os.system("mkdir -p '{}'/logs".format(path)) os.system("mkdir -p '{}'/logs/{}".format(path, c.ROBOT_NAME)) c.LOGFILE = "{}/logs/{}/{}".format(path, c.ROBOT_NAME, name) open(c.LOGFILE, "w").close() dt = "{:.3f}".format(seconds() - c.START_TIME) FORMAT = '%(datetime)s %(dt)s: %(message)s' logging.basicConfig(filename=c.LOGFILE, format=FORMAT) data = {'dt': dt, 'datetime': datetime} # print(str(data) + " " + message) logging.warning(message, extra=data)
def upRamp(): x.rotate(25, 50) x.drive_speed(36, 100) x.drive_speed(6, -50) x.rotate(89, 50) x.drive_speed(30, 100) x.drive_speed(4, -50) x.rotate(79, 50) u.move_servo(c.servoArm, c.armBotguy,10) msleep(300) x.drive_speed(70, 75) u.move_servo(c.servoCow, c.cowDown, 10) msleep(300) x.rotate(-45, 50) print "Seconds elapsed: " + seconds() - c.startTime
def DEBUG(): freeze(c.LMOTOR) freeze(c.RMOTOR) print('Program stop for DEBUG\nSeconds: ', seconds() - c.startTime) ao() exit(0)
def drive_speed(inches, speed, accel=False): # Drives an exact distance in inches. print "driving exact distance" scale = 1.3 sum = 0 max_error = 0 max_sum = 0 total_time = 0 number_times = 0 error = 0 last_time = seconds() right = False if inches < 0: speed = -speed _clear_ticks() ticks = abs(INCHES_TO_TICKS * inches) remain = 0 if accel: for sp in range(1, speed, int(speed / 10)): drive_timed_straight(sp, .025) remain = int((_left_ticks() + _right_ticks()) / 2) _clear_ticks() while _right_ticks() <= ticks - remain: left = _left_ticks() right = _right_ticks() error = right - left sum += error if abs(sum) > abs(max_sum): max_sum = sum if abs(error) > max_error: max_error = abs(error) p_scale = error * 0.018 i_scale = sum * 0.001 adjustment = p_scale + i_scale scale = abs(adjustment) + 1 if adjustment == 0: _drive(speed, speed) elif adjustment > 0: right = True _drive(speed, int(speed / scale)) elif adjustment < 0: if right: now = seconds() total_time += now - last_time last_time = now number_times += 1 right = False _drive(int(speed / scale), speed) display("adjustment: {}".format(adjustment)) display("scale: ".format(scale)) display("max error: " + str(max_error)) display("max sum: " + str(max_sum)) display("current error " + str(error)) display("current sum " + str(sum)) freeze_motors() display("DONE") display("max error: " + str(max_error)) display("max sum: " + str(max_sum)) display("current error " + str(error)) display("current sum " + str(sum))
def calibrate2(inches=24, speed=50): global lAdjust global INCHES_TO_TICKS INCHES_TO_TICKS = 0 lAdjust = 1 if inches < 0: speed = -speed _clear_ticks() ticks = abs(INCHES_TO_TICKS * inches) prevAdjust = -1 while True: start = seconds() lStart = start rStart = start lTime = -1 rTime = -1 l = False r = False i = 0 _clear_ticks() while not l or not r: if l and not r: i = i + 1 elif r and not l: i = i - 1 if analog(2) > 1500: if not l: lTime = seconds() - lStart l = True print("LEFT") if analog(3) > 1500: if not r: rTime = seconds() - rStart r = True print("RIGHT") if _right_ticks() == _left_ticks(): _drive(speed, speed) if _right_ticks() > _left_ticks(): _drive(speed, int(speed / 1.3)) if _left_ticks() > _right_ticks(): _drive(int(speed / 1.3), speed) lAdjust = rTime / lTime if prevAdjust != -1: lAdjust = (lAdjust + prevAdjust) / 2 INCHES_TO_TICKS = ((_left_ticks() + _right_ticks()) / 2) / inches drive_timed(-50, -50, int(seconds() - start)) print("lTime: " + str(lTime) + "\trTime: " + str(rTime)) print("ROT: " + str(i)) print("ADJ: " + str(lAdjust)) print("I2T: " + str(INCHES_TO_TICKS) + "\n") if i == 0: break wait_for_button()
def DEBUG(): ao() print 'Program stop for DEBUG\nSeconds: ', seconds() - c.startTime exit(0)
def shutdown(): freeze(c.LMOTOR) freeze(c.RMOTOR) ao() display('Program stopped\nSeconds: {}'.format(seconds() - c.startTime)) exit(0)
def get_wait(): return seconds() < time
def set_wait(DELAY): global time time = seconds() + DELAY
def currentTime(): print 'Current time: ', seconds() - c.startTime
def setWait(DELAY): # Sets wait time in seconds before breaking a loop. global time time = seconds() + DELAY
def getWait( ): # Used to break a loop after using "setWait". An example would be: setWiat(10) | while true and getWait(): do something(). return seconds() < time