Lunar-Rover-2017-Robotics/Motor_controller.py at master · MANSEDS/Lunar-Rover-2017-Robotics · 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
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
# MANSEDS Lunar Rover -- Motor Controller
# Author: Ethan Ramsay


# Import dependencies
import RPi.GPIO as GPIO
import time
import Adafruit_PCA9685
import logging


# Logging config
logging.basicConfig(filename='motor.log', level=logging.DEBUG)


# System variables
wheel_diameter = 0.12 # m
pi = 3.14159
max_rpm = 26
max_ang_vel = max_rpm * pi / 30 # rad/s
motor = 0


# GPIO setup/functions/cleanup
GPIO.setmode(GPIO.BCM)


def GPIO_set(pwm, dc):
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(pwm, GPIO.OUT)
    motor = GPIO.PWM(pwm, 50)
    motor.start(dc)


def GPIO_clear(motor):
    motor.stop()
    GPIO.cleanup()


# Adafruit setup
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)
logging.debug('Set Adafruit pwm freq to 60')


# Calculate angular velocity for desired velocity
def calc_des_ang_vel(v):
    des_ang_vel = 2 * v / wheel_diameter # rad/s
    if des_ang_vel > max_ang_vel:
        if od:
            des_ang_vel = max_ang_vel
            print("Desired velocity exceeds maximum velocity, velocity set to maximum due to" + \
                                "overdrive, extended use of overdrive is not recommended")
        else:
            des_ang_vel = max_ang_vel * 0.9
    elif des_ang_vel >= 0.9 * max_ang_vel:
        if not od:
            des_ang_vel = 0.9 * max_ang_vel
    logging.debug("Calculated required angular velocity for desired velocity: %s", des_ang_vel)
    return des_ang_vel


def calc_dc(dc_min, dc_max, des_ang_vel):
    dc_range = dc_max - dc_min
    inter = dc_range * des_ang_vel / max_ang_vel
    dc = dc_min + inter
    logging.debug("Calculated required duty cycle for desired angular velocity: %s", dc)
    return dc


def calc_pl(pl_min, pl_max, des_ang_vel):
    pl_range = pl_max - pl_min
    inter = pl_range * des_ang_vel / max_ang_vel
    pl = pl_min + inter
    logging.debug("Calculated required pulse length for desired angular velocity: %s", pl)
    pl = int(pl)
    return pl


if __name__ == "__main__":

    import argparse

    # Arguments
    parser = argparse.ArgumentParser()
    parser.add_argument("v", help="Velocity (m/s)")
    parser.add_argument("t", help="Operation time (s)")

    # Arguments for direct GPIO control from Pi
    parser.add_argument("hi", help="High Pin No. (BOARD)")
    parser.add_argument("lo", help="Low Pin No. (BOARD)")
    # parser.add_argument("dc_min", help="Minimum Duty Cycle")
    # parser.add_argument("dc_max", help="Maximum Duty Cycle")
    # parser.add_argument("pwm", help="PWM Pin No. (BOARD)")

    # Arguments for Adafruit PWM hat control
    parser.add_argument("pl_min", help="Minimum Pulse Length")
    parser.add_argument("pl_max", help="Maximum Pulse Length")
    parser.add_argument("channel", help="Channel No. (Adafruit PWM Hat)")

    # Optional arguments
    parser.add_argument("--od", help="Overdrive Enable", action="store_true")

    # Parse arguments
    args = parser.parse_args()
    v = float(args.v)
    t = float(args.t)
    hi = int(args.hi)
    lo = int(args.lo)
    # dc_min = float(args.dc_min)
    # dc_max = float(args.dc_max)
    # pwm = int(args.pwm)
    pl_min = float(args.pl_min)
    pl_max = float(args.pl_max)
    channel = int(args.channel)
    od = args.od
    if args.od:
        print("Overdrive enabled, not recommended for extended durations")
    logging.debug("Arguments parsed: v=%s, t=%s, hi=%s, lo=%s, pl_min=%s, pl_max=%s, " + \
                            "channel=%s od=%s", v, t, hi, lo, pl_min, pl_max, channel, od)


    # Calculate angular velocity for desired velocity
    des_ang_vel = calc_des_ang_vel(v)
    print("Desired angular velocity is: "+str(des_ang_vel))


    # Calculate interpolated duty cycle
    # dc = calc_dc(dc_min, dc_max, des_ang_vel)
    # print("Required duty cycle is: "+str(dc))


    # Calculate interpolated pulse length
    pl = calc_pl(pl_min, pl_max, des_ang_vel)
    print("Required pulse length is: "+str(pl))


    # Setup Motor HiLo pins
    GPIO.setup(hi, GPIO.OUT)
    GPIO.output(hi, 1)
    GPIO.setup(lo, GPIO.OUT)
    GPIO.output(lo, 0)


    # Activate motor through GPIO
    # GPIO_set(pwm, dc)
    # time.sleep(t)
    # GPIO_clear()


    # Activate motor through Adafruit PWM hat
    pwm.set_pwm(channel, 0, pl)
    time.sleep(t)
    pwm.set_pwm(channel, 0, 0)


else:
    des_ang_vel = calc_des_ang_vel(v)
    dc = calc_dc(dc_min, dc_max, des_ang_vel)
    pl = calc_pl(pl_min, pl_max, des_ang_vel)
    print(dc)