127 lines
4.3 KiB
Python
127 lines
4.3 KiB
Python
import time
|
|
|
|
# Library imports
|
|
from vex import *
|
|
|
|
class Pierre:
|
|
def __init__(self) -> None:
|
|
self.brain = Brain()
|
|
self.brain_inertial = Inertial()
|
|
self.left_drive_smart = Motor(
|
|
Ports.PORT1,
|
|
GearSetting.RATIO_2_1,
|
|
False)
|
|
self.right_drive_smart = Motor(
|
|
Ports.PORT6,
|
|
GearSetting.RATIO_2_1,
|
|
True)
|
|
self.drivetrain = SmartDrive(
|
|
lm=self.left_drive_smart,
|
|
rm=self.right_drive_smart,
|
|
g=self.brain_inertial,
|
|
wheelTravel=200,
|
|
trackWidth=292,
|
|
wheelBase=127,
|
|
units=DistanceUnits.MM,
|
|
externalGearRatio=2.0,
|
|
)
|
|
# touchled_2 = Touchled(Ports.PORT2)
|
|
# optical_3 = Optical(Ports.PORT3)
|
|
self.color_sensor = ColorSensor(Ports.PORT3)
|
|
# distance_7 = Distance(Ports.PORT7)
|
|
# bumper_8 = Bumper(Ports.PORT8)
|
|
self.velocity = 50
|
|
self.brain.buttonLeft.pressed(self.velocity_change, (-10,))
|
|
self.brain.buttonRight.pressed(self.velocity_change, (10,))
|
|
self.brain.buttonCheck.pressed(self.forward_routine)
|
|
|
|
def action_loop(self):
|
|
while True:
|
|
#brain.screen.set_cursor(1, 1)
|
|
#brain.screen.print("Hue " + str(color_3.hue()))
|
|
#brain.screen.set_cursor(2, 1)
|
|
#brain.screen.print("Brt " + str(color_3.brightness()))
|
|
#brain.screen.set_cursor(3, 1)
|
|
#brain.screen.print("Obj " + str(color_3.is_near_object()))
|
|
time.sleep(.05)
|
|
|
|
def calibrate_drivetrain(self):
|
|
# Calibrate the Drivetrain Inertial
|
|
sleep(200, MSEC)
|
|
self.brain.screen.print("Calibrating")
|
|
self.brain.screen.next_row()
|
|
self.brain.screen.print("Inertial")
|
|
self.brain_inertial.calibrate()
|
|
while self.brain_inertial.is_calibrating():
|
|
sleep(25, MSEC)
|
|
self.brain.screen.clear_screen()
|
|
self.brain.screen.set_cursor(1, 1)
|
|
self.brain.screen.print("Pierre is ready.")
|
|
|
|
def forward_routine(self) -> None:
|
|
self.brain.screen.set_cursor(1, 1)
|
|
self.brain.screen.print("driving routine")
|
|
self.play_confirm_sound()
|
|
|
|
self.drivetrain.set_drive_velocity(self.velocity, VelocityUnits.PERCENT)
|
|
self.drivetrain.set_turn_velocity(self.velocity, VelocityUnits.PERCENT)
|
|
for _ in range(4):
|
|
self.brain.screen.clear_line(2)
|
|
self.brain.screen.set_cursor(2, 1)
|
|
self.brain.screen.print("straight")
|
|
self.drivetrain.drive_for(DirectionType.FORWARD, 20.0, DistanceUnits.IN, wait=True)
|
|
|
|
self.play_pause_sound()
|
|
self.brain.screen.clear_line(3)
|
|
self.brain.screen.set_cursor(3, 1)
|
|
self.brain.screen.print("turn")
|
|
self.drivetrain.turn_for(
|
|
direction=TurnType.LEFT,
|
|
angle=90,
|
|
wait=True)
|
|
|
|
self.brain.screen.set_cursor(2, 1)
|
|
self.brain.screen.print("Complete.")
|
|
self.play_complete_sound()
|
|
|
|
def play_confirm_sound(self) -> None:
|
|
self.brain.play_note(1, 1, 100)
|
|
self.brain.play_note(1, 2, 100)
|
|
self.brain.play_note(1, 3, 100)
|
|
self.brain.play_note(1, 6, 100)
|
|
self.brain.play_note(1, 4, 100)
|
|
|
|
def play_complete_sound(self) -> None:
|
|
self.brain.play_note(1, 5, 100)
|
|
time.sleep(0.2)
|
|
self.brain.play_note(1, 5, 400)
|
|
|
|
def play_pause_sound(self) -> None:
|
|
self.brain.play_note(1, 7, 100)
|
|
time.sleep(0.2)
|
|
self.brain.play_note(1, 6, 200)
|
|
time.sleep(0.2)
|
|
self.brain.play_note(1, 5, 300)
|
|
|
|
def velocity_change(self, delta: int) -> None:
|
|
self.velocity -= delta
|
|
if self.velocity < 10:
|
|
self.velocity = 10
|
|
elif self.velocity > 100:
|
|
self.velocity = 100
|
|
self.brain.screen.set_cursor(1, 1)
|
|
self.brain.screen.print("Velocity: " + str(self.velocity) + "%")
|
|
|
|
|
|
|
|
def main() -> None:
|
|
# Brain should be defined by default
|
|
pierre = Pierre()
|
|
pierre.calibrate_drivetrain()
|
|
pierre.action_loop()
|
|
|
|
#drivetrain.drive_for(FORWARD, 10)
|
|
#drivetrain.drive_for(REVERSE, 10)
|
|
#color_3.set_light(LedStateType.ON)
|
|
if __name__ == "__main__":
|
|
main() |