enviro-mapper/drivetrain.py at master · kmgovind/enviro-mapper · 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
162
163
164
165
166
167
168
169
170
171
#!/usr/bin/env python3
from signal import pause
from time import sleep

import constants as k
import numpy as np
import RPi.GPIO as GPIO

posR = 0
posL = 0
lastencodedR = 0
lastencodedL = 0
rrv = 0
rfw = 0
lrv = 0
lfw = 0


def setup_dt():
    global rrv, rfw, lrv, lfw

# Setup Pins
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BCM)  # using the board's numbering scheme

    # Pins
    GPIO.setup(k.R_MOTOR_ACTV, GPIO.OUT)
    GPIO.setup(k.R_MOTOR_FW, GPIO.OUT)
    GPIO.setup(k.R_MOTOR_RV, GPIO.OUT)
    GPIO.setup(k.R_MOTOR_ENC, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    GPIO.setup(k.R_MOTOR_VAL, GPIO.IN)
    GPIO.setup(k.L_MOTOR_ACTV, GPIO.OUT)
    GPIO.setup(k.L_MOTOR_FW, GPIO.OUT)
    GPIO.setup(k.L_MOTOR_RV, GPIO.OUT)
    GPIO.setup(k.L_MOTOR_ENC, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    GPIO.setup(k.L_MOTOR_VAL, GPIO.IN)

    # Enable PWM
    rrv = GPIO.PWM(k.R_MOTOR_RV, k.PWM_FREQ)
    rfw = GPIO.PWM(k.R_MOTOR_FW, k.PWM_FREQ)
    lrv = GPIO.PWM(k.L_MOTOR_RV, k.PWM_FREQ)
    lfw = GPIO.PWM(k.L_MOTOR_FW, k.PWM_FREQ)

    # Encoder Interrupts
    GPIO.add_event_detect(k.R_MOTOR_ENC, GPIO.RISING, callback=updateR)
    GPIO.add_event_detect(k.L_MOTOR_ENC, GPIO.RISING, callback=updateL)


def drive_forward(cm):
    global posR, posL
    posR = 0
    posL = 0
    # 0.225 * 2.54 is cm per step of encoder
    distcomp = np.floor(cm/(0.225 * 2.54))

    # Activate Motors
    GPIO.output(k.R_MOTOR_ACTV, True)
    GPIO.output(k.L_MOTOR_ACTV, True)

    while ((posR < distcomp) or (posL < distcomp)):

        # Compute necessary powers
        moveratioR = ((distcomp - posR)/distcomp) * 100
        moveratioL = ((distcomp - posL)/distcomp) * k.LR_BIAS_forward * 100
        # spdR = np.interp(moveratioR, [0, 105], [70, 200])
        # spdL = np.interp(moveratioL, [0, 105], [70, 200])
        # np.clip(spdR, 50, 200, out=spdR)
        # np.clip(spdL, 50, 200, out=spdL)

        # Turn off reverse
        GPIO.output(k.R_MOTOR_RV, False)
        GPIO.output(k.L_MOTOR_RV, False)

        # Drive forward pins
        if (moveratioL > 100):
            moveratioL = 100
        if (moveratioL < 25):
            moveratioL = 25
        if (moveratioR > 100):
            moveratioR = 100
        if (moveratioR < 25):
            moveratioR = 25
        rfw.start(moveratioR)
        lfw.start(moveratioL)

    rfw.stop()
    lfw.stop()

    # Deactivate Motors
    GPIO.output(k.R_MOTOR_ACTV, False)
    GPIO.output(k.L_MOTOR_ACTV, False)

def turn_to_readings(degrees,wall_tracker):
    degrees_to_measure = list(range(0,degrees,k.TURN_GRADIENT))
    degrees_to_measure.append(degrees)
    lastDegree = 0
    for i in degrees_to_measure:
        degreeChange = i - lastDegree
        turn_to(degreeChange)
        wall_tracker.take_reading_turn(degreeChange)
        lastDegree = i

def turn_to(degrees):

    global posR, posL
    posR = 0
    posL = 0
    degree = (((degrees/360) * (17.687 * 2.54))/(0.225 * 2.54))
    degreesteps = np.floor(degree)

    # Activate Motors
    GPIO.output(k.R_MOTOR_ACTV, True)
    GPIO.output(k.L_MOTOR_ACTV, True)

    while ((posR < degreesteps) or (posL < degreesteps)):
        moveratioR = ((degreesteps - posR)/degreesteps) * 100
        moveratioL = ((degreesteps - posL)/degreesteps) * k.LR_BIAS * 100
        if (moveratioL > 85):
            moveratioL = 85
        if (moveratioL < 30):
            moveratioL = 30
        if (moveratioR > 85):
            moveratioR = 85
        if (moveratioR < 30):
            moveratioR = 30
        GPIO.output(k.R_MOTOR_RV, False)
        GPIO.output(k.L_MOTOR_FW, False)
        rfw.start(moveratioR)
        lrv.start(moveratioL)

    rfw.stop()
    lrv.stop()

    # Deactivate Motors
    GPIO.output(k.R_MOTOR_ACTV, False)
    GPIO.output(k.L_MOTOR_ACTV, False)


def brake(seconds):
    # Deactivate Motors
    GPIO.output(k.R_MOTOR_ACTV, False)
    GPIO.output(k.L_MOTOR_ACTV, False)

    #Stop Motor Control Singal
    GPIO.output(k.R_MOTOR_RV, False)
    GPIO.output(k.R_MOTOR_FW, False)
    GPIO.output(k.L_MOTOR_RV, False)
    GPIO.output(k.L_MOTOR_FW, False)
    sleep(seconds)


def close_pins():
    GPIO.cleanup()


def updateR(dummy):
    global lastencodedR, posR
    encodedR = GPIO.input(k.R_MOTOR_ENC)
    # encodedR = GPIO.input(k.R_MOTOR_VAL)
    if (encodedR != lastencodedR):
        posR += 1
    lastencodedR = encodedR


def updateL(dummy):
    global lastencodedL, posL
    encodedL = GPIO.input(k.L_MOTOR_ENC)
    # encodedL = GPIO.input(k.L_MOTOR_VAL)
    if (encodedL != lastencodedL):
        posL += 1
    lastencodedL = encodedL