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 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 turn_off_pom_plow(): ''' turns off the pom plow ''' wallaby.off(POMPLOW_PORT)
def stop(): '''stops the robot''' off(LEFT_MOTOR_PORT) off(RIGHT_MOTOR_PORT)