def task_time(t, mot, velo, f):
global thread_end
global thread_started
global threads_num
global result
w.cmpc(mot)
w.motor(mot, velo)
# lock.acquire()
# threads_num += 1
# lock.release()
thread_started = True
print("wait {}s ...".format(t))
while t > 0: # wait till expected time
print(t),
sleep(1)
t -= 1
w.freeze(mot)
print("\nstopping ...")
thread_end = True
# lock.acquire()
# threads_num -= 1
# lock.release()
result = "motor {}\nruntime: {}\nmotor ticks: {}\nturns: {}".format(
mot, runtime, w.gmpc(mot), f.get_result())
def task_rotate(turns, mot, velo, f):
global thread_end
global thread_started
global threads_num
global result
w.cmpc(mot)
w.motor(mot, velo)
# lock.acquire()
# threads_num += 1
# lock.release()
thread_started = True
print("wait {} turns ...".format(turns))
#data = []
last_val = f.get_result()
while last_val < turns: # wait till expected turns
val = f.get_result()
if val > last_val:
print("%2.1f\t%i" % (last_val, w.gmpc(mot)))
last_val = val
w.freeze(mot)
print("\nstopping ...")
thread_end = True
# lock.acquire()
# threads_num -= 1
# lock.release()
result = "motor {}\nturns: {}\nmotor ticks: {}".format(
mot, f.get_result(), w.gmpc(mot))
def drive(left, right):
if c.isPrime:
# PRIME motor settings
motor(c.LMOTOR, left)
motor(c.RMOTOR, right)
else:
# CLONE motor settings
motor(c.LMOTOR, left)
motor(c.RMOTOR, right)
def driveMotor(left, right, time): w.motor(c.leftMotor, int(left*c.motorScale)) w.motor(c.rightMotor, right) if time == 0: w.msleep(time) w.off(c.leftMotor) w.off(c.rightMotor) def forward(speed, time): driveMotor(speed, speed, time) def backward(speed, time): driveMotor(speed*-1, speed*-1, time) def spinLeft(speed, time): driveMotor(speed*-1, speed, time) def spinRight(speed, time): driveMotor(speed, speed*-1, time) def veerLeft(speed, time, o): driveMotor((speed*-1)-o, speed, time) def veerRight(speed, time, o): driveMotor(speed, (speed*-1)-o, time) def pivotRight(speed, time): driveMotor(0, speed, time) def pivotLeft(speed, time): driveMotor(speed, 0, time) def radiusTurn(speed, radius, time): driveMotor(speed, int((radius / (radius+5.0)*speed), time)) def driveUntilBlack(speed): while w.analog(c.largeTopHat) < c.LARGE_TOPHAT_LINE: driveMotor(speed, speed, 1) def lineFollowUntilTape(): while w.analog(c.smallTopHat) < c.SMALL_TOPHAT_LINE: if w.analog(c.largeTopHat) < c.LARGE_TOPHAT_LINE: veerLeft(50, 1, 10) elif w.analog(c.largeTopHat) > c.LARGE_TOPHAT_LINE: veerRight(50, 1, 10)
def new_get_poms_timed(speed, time):
for x in range(3):
get_poms_wiggle(speed, time / 3)
motor(c.DATEMOTOR1, 0)
motor(c.DATEMOTOR2, 0)
u.move_servo(c.servoDateWheel, c.wheelIn + 300, 20)
u.move_servo(c.servoDateWheel, c.wheelIn, 20)
pivot_right(1, 60)
pivot_left(.5, 60)
mpp.drive_speed(-.25, 60)
motor(c.LMOTOR, 0)
motor(c.RMOTOR, 0)
def task_rotate(turns, mot, velo, sens):
global result
w.cmpc(mot)
w.motor(mot, velo)
print("wait {} turns ...".format(turns))
sens.update()
last_val = sens.get_result()
while last_val < turns: # wait till expected turns
sens.update()
val = sens.get_result()
if val > last_val:
print(last_val),
last_val = val
w.freeze(mot)
print("\nstopping ...")
result = "motor {}\nturns: {}\nmotor ticks: {}".format(
mot, sens.get_result(), w.gmpc(mot))
def moveMotor(motor, power, dist): # basically just mtp()
w.cmpc(motor)
if dist < 0:
w.motor(motor, -power)
else:
w.motor(motor, power)
while abs(w.gmpc(motor)) < abs(dist):
print w.gmpc(motor)
continue
w.motor(motor, 0) # freeze motor, then shut off
w.off(motor)
def moveDegree(motor, power,
degree): # set the motor to a degree, like a servo
w.cmpc(motor)
if degree < 0:
w.motor(motor, -power)
else:
w.motor(motor, power)
goal = c.MOTOR_DEG2TICK(degree)
while abs(w.gmpc(motor)) < abs(goal):
print w.gmpc(motor)
continue
c.distance_traveled += w.gmpc(motor) # set to actual position, not goal
print "Distance Traveled: %d" % c.distance_traveled,
print "Goal: %d" % goal
print "done!"
w.motor(motor, 0) # freeze motor, then shut off
w.off(motor)
def _drive(left, right): # Moves the robot using motor commands.
motor(LMOTOR, left)
motor(RMOTOR, right)
def turn_on_pom_plow():
'''
turns on the pom plow
'''
wallaby.motor(POMPLOW_PORT)
def driveMotorTimed(motorport, speed, time):
motor(motorport, speed)
msleep(time)
def driveMotor(motorport, speed):
motor(motorport, speed)
def get_poms_wiggle(speed, time):
motor(c.DATEMOTOR2, speed - 20)
motor(c.DATEMOTOR1, speed)
for x in range(3): #6
u.move_servo_timed(c.servoDateWheel, c.wheelIn + 45, time / 6)
u.move_servo_timed(c.servoDateWheel, c.wheelIn - 5, time / 6)
motor(c.DATEMOTOR1, 0)
motor(c.DATEMOTOR2, 0)
motor(c.LMOTOR, 0)
motor(c.RMOTOR, 0)
def get_poms_timed(speed, time):
motor(c.DATEMOTOR, speed)
msleep(time)
motor(c.DATEMOTOR, 0)
motor(c.LMOTOR, 0)
motor(c.RMOTOR, 0)
def drive_date_motor(speed, time):
motor(c.DATEMOTOR, speed)
msleep(time)
motor(c.DATEMOTOR, 0)
def drive(left, right):
motor(c.LMOTOR, left)
motor(c.RMOTOR, right)
def move_forwards(power):
'''move forwards at the given power'''
motor(LEFT_MOTOR_PORT, power)
motor(RIGHT_MOTOR_PORT, power)
def turn_right(power):
'''turns right at the given power'''
motor(LEFT_MOTOR_PORT, power)
motor(RIGHT_MOTOR_PORT, -power)
def move_backwards(power):
'''move backwards at the given power'''
motor(LEFT_MOTOR_PORT, -power)
motor(RIGHT_MOTOR_PORT, -power)