ME348E-Robot-Code-Base-Rpi/motorControl.py at main · hypanim/ME348E-Robot-Code-Base-Rpi · 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
import time

class motorControl:
    def __init__(self, sendTarget, targetStep1, targetStep2, targetStep3, targetStep4, currentStep1, currentStep2, currentStep3, currentStep4):
        self.sendTarget = sendTarget
        self.targetStep1 = targetStep1
        self.targetStep2 = targetStep2
        self.targetStep3 = targetStep3
        self.targetStep4 = targetStep4
        self.currentStep1 = currentStep1
        self.currentStep2 = currentStep2
        self.currentStep3 = currentStep3
        self.currentStep4 = currentStep4

    def readCurrentSteps(self):
        reading = [0, 0, 0, 0]
        reading[0] = self.currentStep1.value
        reading[1] = self.currentStep2.value
        reading[2] = self.currentStep3.value
        reading[3] = self.currentStep4.value
        return reading

    def moving(self):
        time.sleep(0.1)
        if (self.currentStep1.value != 0):
            return True
        if (self.currentStep2.value != 0):
            return True
        if (self.currentStep3.value != 0):
            return True
        if (self.currentStep4.value != 0):
            return True
        return False

    def writeTargetSteps(self, targetList):
        self.sendTarget.value = True
        self.targetStep1.value = targetList[0]
        self.targetStep2.value = targetList[1]
        self.targetStep3.value = targetList[2]
        self.targetStep4.value = targetList[3]

    def moveForward(self, targetDistance):
        self.sendTarget.value = True
        self.targetStep1.value = targetDistance
        self.targetStep2.value = -targetDistance
        self.targetStep3.value = -targetDistance
        self.targetStep4.value = targetDistance

    def moveRight(self, targetDistance):
        self.sendTarget.value = True
        self.targetStep1.value = -targetDistance
        self.targetStep2.value = -targetDistance
        self.targetStep3.value = targetDistance
        self.targetStep4.value = targetDistance

    def rotate(self, targetDistance):
        self.sendTarget.value = True
        self.targetStep1.value = targetDistance
        self.targetStep2.value = targetDistance
        self.targetStep3.value = targetDistance
        self.targetStep4.value = targetDistance