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
