PYPER/src/DrivingSystem.py at master · TimHanewich/PYPER · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
import settings
import machine
import MovementCommand
import time

class DrivingSystem:

    def __init__(self) -> None:

        # drive train
        self.safety:machine.Pin = machine.Pin(settings.gp_safety, machine.Pin.OUT)
        self.safety.off() # start with safety on
        self.i1:machine.PWM = machine.PWM(machine.Pin(settings.gp_i1, machine.Pin.OUT))
        self.i1.freq(50)
        self.i1.duty_u16(0) # start at 0%
        self.i2:machine.PWM = machine.PWM(machine.Pin(settings.gp_i2, machine.Pin.OUT))
        self.i2.freq(50)
        self.i2.duty_u16(0) # start at 0%

        # steering system
        self.steer_pwm:machine.PWM = None


    ### STEERING SYSTEM BELOW ###


    def enable_steer(self) -> None:
        self.steer_pwm = machine.PWM(machine.Pin(settings.gp_steering, machine.Pin.OUT))
        self.steer_pwm.freq(50)
        self.steer(0.0) # to go center

    def disable_steer(self) -> None:
        if self.steer_pwm != None:
            self.steer_pwm.deinit() # turn off the PWM
            self.steer_pwm = None

    def steer(self, angle:float) -> None:
        if angle < -1.0 or angle > 1.0:
            raise Exception("Steer angle '" + str(angle) + "' outside of range -1.0 to 1.0")
        if self.steer_pwm != None:
            ns:int = int(round(1500000 + (((2000000 - 1000000) * angle) / 2), 0))
            self.steer_pwm.duty_ns(ns)


    #### DRIVE TRAIN SYSTEM BELOW #####


    def enable_drive(self) -> None:
        self.safety.on()
        self.drive(0.0)

    def disable_drive(self) -> None:
        self.safety.off()
        self.drive(0.0)

    def drive(self, power:float) -> None:
        duty:int = abs(int(round(65025 * power, 0)))
        duty = max(min(duty, 65025), 0)
        if power >= 0.0:
            self.i1.duty_u16(0)
            self.i2.duty_u16(duty)
        else:
            self.i1.duty_u16(duty)
            self.i2.duty_u16(0)


    ########### HIGHER LEVEL ##############

    def execute(self, mc:MovementCommand.MovementCommand, stop_at_end:bool = True) -> None:
        self.steer(mc.steer)
        self.drive(mc.drive)
        time.sleep(mc.duration)
        if stop_at_end:
            self.drive(0.0) # stop driving