def turn_one_wheel(v, angle, motor, g=None, c=None): """ """ L = io.motA R = io.motB servo = io.servo gyro = io.gyro col = io.col if angle > 0: direction = 1 # set the polairty switch for the wheels elif angle < 0: direction = -1 else: # 0 degrees = no work to be done return print(direction) # -------------- ROBOT --------------------- if motor == 'ROBOT': desired_angle = gyro.value() + angle logging.info( 'Turning the robot to desired {} degrees'.format(desired_angle)) turn_control = Controller(.01, 0, 0, desired_angle, history=10) while True: signal, err = turn_control.control_signal(gyro.value()) if abs(err) <= 5 or io.btn.backspace: # tolerance L.stop() R.stop() L.duty_cycle_sp = v R.duty_cycle_sp = v return g, c else: # if too small or too large, just use the default v if abs(v + signal) >= 100 or abs(v + signal) <= 20: signal = 0 if direction == 1: L.run_direct(duty_cycle_sp=direction * abs(v + signal)) elif direction == -1: R.run_direct(duty_cycle_sp=-1 * direction * abs(v + signal)) logging.info( 'GYRO = {},\tcontrol = {},\t err={}, \tL = {}, \tR = {}'. format(gyro.value(), signal, err, L.duty_cycle_sp, R.duty_cycle_sp)) try: g.set_val(gyro.value()) c.set_val(col.value()) except AttributeError: pass else: raise NameError('motor should be "ROBOT" or "SERVO"')
def turn_on_spot(v, angle, motor, g=None, c=None): """ Turn the robot or servo motor on the spot by the angle. :param v - the duty_cycle_sp at which the motor should travel at :param motor - either 'SERVO' or 'MOTOR' :param angle - If the angle is negative, it will turn CCW else CW. It sets the goal state of the robot or servo as the sum of its current heading (gyro.value()) and the angle. """ L = io.motA R = io.motB servo = io.servo gyro = io.gyro col = io.col if angle > 0: direction = 1 # set the polairty switch for the wheels elif angle < 0: direction = -1 else: # 0 degrees = no work to be done return print(direction) # -------------- ROBOT --------------------- if motor == 'ROBOT': desired_angle = gyro.value() + angle logging.info( 'Turning the robot to desired {} degrees'.format(desired_angle)) turn_control = Controller(.01, 0, 0, desired_angle, history=10) while True: signal, err = turn_control.control_signal(gyro.value()) signal, err = turn_control.control_signal(gyro.value()) if abs(err) <= 5 or io.btn.backspace: # tolerance L.stop() R.stop() L.duty_cycle_sp = v R.duty_cycle_sp = v return else: # if too small or too large, just use the default v if abs(v + signal) >= 100 or abs(v + signal) <= 20: signal = 0 L.run_direct(duty_cycle_sp=direction * abs(v + signal)) R.run_direct(duty_cycle_sp=-1 * direction * abs(v + signal)) logging.info( 'GYRO = {},\tcontrol = {},\t err={}, \tL = {}, \tR = {}'. format(gyro.value(), signal, err, L.duty_cycle_sp, R.duty_cycle_sp)) try: g.set_val(gyro.value()) c.set_val(col.value()) except AttributeError: pass # -------------- SERVO --------------------- elif motor == 'SERVO': turn_control = Controller(.001, 0, .5, servo.position + angle, history=10) # servo.duty_cycle_sp = servo.duty_cycle_sp * direction ev3.Sound.speak('Turning servo {} degrees'.format(angle)).wait() logging.info('Turning servo {} degrees'.format(angle)) while True: signal, err = turn_control.control_signal(servo.position) if abs(err) <= 4 or io.btn.backspace: # tolerance servo.stop() servo.speed_sp = v servo.duty_cycle_sp = servo.duty_cycle_sp * \ direction # return to the original number return if (v + abs(signal) >= 100): signal = 0 signal = (direction) * (v + abs(signal)) servo.run_direct(duty_cycle_sp=signal) logging.info( 'POS = {},\tcontrol = {},\t err={}, \tspd = {}'.format( servo.position, signal, err, servo.speed_sp)) try: g.set_val(gyro.value()) except AttributeError: pass else: raise NameError('motor should be "ROBOT" or "SERVO"')