Lunar-Rover-2017-Robotics/Scoop_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
# MANSEDS Lunar Rover -- Scoop Controller
# Author: Ethan Ramsay

# Abbreviations:
# dc = duty cycle
# pl = pulse length
# lin_act | lin = linear actuator
# e = extension
# a = angle
# pwm = pulse width modulation pin no.
# chan = pulse width modulation channel


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


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

# System variables
lin_act = 0
servos = []
lin_pwm = 0
servo_pwm = [0, 0]
lin_dc_range = [0, 100]
servo_dc_range = [[0, 100], [0, 100]]
lin_chan = 12
servo_chan = [13, 14]
lin_pl_range = [0, 4095]
servo_pl_range = [0, 4095]


# GPIO setup function
def GPIO_set(dc):
    for i in range(0,2,1):
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(servo_pwm[i], GPIO.OUT)
        servos.append(GPIO.PWM(servo_pwm[i], 60))
    GPIO.setup(lin_pwm, GPIO.OUT)
    lin_act = GPIO.PWM(lin_pwm, 60)


def GPIO_clear(lin_act):
    for servo in servos:
        servo.stop()
    lin_act.stop()
    GPIO.cleanup()


# Adafruit setup
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(60)


# Signal functions
def calc_dc(dc_min, dc_max, magnitude, maximum):
    dc_range = dc_max - dc_min
    inter = dc_range * magnitude / maximum
    dc = dc_min + inter
    logging.debug("Calculated required duty cycle for desired extension: %s", dc)
    return dc


def calc_pl(pl_min, pl_max, magnitude, maximum):
    pl_range = pl_max - pl_min
    inter = pl_range * magnitude / maximum
    pl = pl_min + inter
    logging.debug("Calculated required pulse length for desired extension: %s", pl)
    return pl


# Command functions
def scoop():
    pass


def adjust_pos(axis, direction):
    if axis == "hor":
        if direction == "forward":
            pass
        elif direction == "backwards":
            pass
    elif axis == "ver":
        if direction == "up":
            pass
        elif direction == "down":
            pass


if __name__ = "__main__":
    # Arguments
    parser = argparse.ArgumentParser()

    # Command arguments
    g = parser.add_mututally_exclusive_group(required=True)
    gh = g.add_mututally_exclusive_group()
    gv = g.add_mututally_exclusive_group()
    g.add_argument("-s", "--scoop", help="Scoop up", action="store_true")
    g.add_argument("-e", "--empty", help="Empty scoop", action="store_true")
    gh.add_argument("-u", "--up", help="Position scoop vertically up", action="store_true")
    g.add_argument("-d", "--down", help="Position scoop vertically up", action="store_true")
    gv.add_argument("-f", "--forward", action="store_true")
    g.add_argument("-b", "", help="--backwards", action="store_true")

    # Optional arguments

    # Parse arguments
    args = parser.parse_args()

    # Such empties