def main():
'''The main function of our program'''
# set the console just how we want it
#reset_console()
#set_cursor(OFF)
#set_font('Lat15-Terminus24x12')
# print something to the screen of the device
#print('Hello World!')
# print something to the output panel in VS Code
debug_print('Hello VS Code!')
# wait a bit so you have time to look at the display before the program
# exits
time.sleep(1)
motors = LargeMotor(OUTPUT_C)
#motors.position_sp = 20
motors.run_to_abs_pos(position_sp=20)
#while True:
# debug_print(motors.position_sp)
# sleep(1)
# motors.run_to_abs_pos(20,50)
#motors.run_to_abs_pos()
shovel = moveShovel.Shovel()
drive = drive_gyro.Drive_gyro(shovel)
drive.test()
class Pen:
def __init__(self, port_motor_move, port_motor_pen):
# Motor to move pen sideways
self.motor_move = LargeMotor(port_motor_move)
self.motor_move.reset()
self.motor_move.speed_sp = 400
self.motor_move.stop_action = 'brake'
self.motor_move_positions = (0, -60, -210, -350, -500, -640)
# Motor te move pen up or down
self.motor_pen = LargeMotor(port_motor_pen)
self.motor_pen.stop_action = 'hold'
# TouchSensor to indicate pen is up
self.ts_pen = TouchSensor('pistorms:BBS2')
# Move pen up
self.motor_pen.run_forever(speed_sp=-200)
self.ts_pen.wait_for_pressed()
self.motor_pen.stop()
@try_except
def pen_up(self):
''' Move pen up.'''
self.motor_pen.run_to_abs_pos(speed_sp=400, position_sp=-20)
wait_while_motors_running(self.motor_pen)
@try_except
def pen_down(self):
''' Move pen down.'''
self.motor_pen.run_to_abs_pos(speed_sp=400, position_sp=20)
wait_while_motors_running(self.motor_pen)
@try_except
def move_to_abs_pos(self, pos):
''' Move pen sidways to absolute position [0,..,5].'''
self.motor_move.run_to_abs_pos(
position_sp=self.motor_move_positions[pos])
