Skip to content
Snippets Groups Projects
Select Git revision
  • master default protected
  • motor_branch
  • webserver_branch
  • app_branch
  • controller_branch
  • mergestuff
  • ultrasound_storage_branch
  • speedsensor_branch
  • ultrasound_branch
9 results
Blame Permalink
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
driving.py 4.51 KiB
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 






# Libraries
#import RPi.GPIO as GPIO
import time
from encoder import Encoder
#import PWMoutputFunction

# GPIO Mode (BOARD / BCM)
#GPIO.setmode(GPIO.BCM)
#print(GPIO.VERSION)

class Driving:
    def __init__ (self):
        self.circumference = 21.0
        self.setpointPulseCountLeft =0
        self.setpointPulseCuntRight =0
        self.currentPulseCountLeft =0
        self.currentPulseCountRight=0
        self.directionLeftWheel = True #true = forward
        self.directionRightWheel = True #true = forward
        self.pulsesPerRotation = 40
        self.pulsesPer45DegreeCarRotation =10
        self.speed = 0 #speed in PWM percentage
        self.rotationspeed = 0.2 #speed in PWM percentage
        self.leftPWM = 0
        self.rightPWM =0
        self.encoderLeft = Encoder(name="encoder1", encoder_pin=23)
        self.encoderRight = Encoder(name="encoder1", encoder_pin=24)

    def stop(self):
        self.speed= 0
    
    def isCarStopped(self)
        if self.speed ==0:
            return True
        else
            return False

    def driveForward(self, distance, speed):
        #reset pulse count
        self.encoderLeft.reset_pulse_count()
        self.encoderRight.reset_pulse_count()
        #set speed
        self.speed = speed
        #set wheel direction
        self.directionLeftWheel = True
        self.directionRightWheel = True
        #calculate setpoint
        self.setpointPulseCountLeft = int(distance/self.circumference * self.pulsesPerRotation)
        self.setpointPulseCountRight = int(distance/self.circumference * self.pulsesPerRotation)
        

    
    def turnLeft(self,numberOf45degreeRotations):
        #reset pulse count
        self.encoderLeft.reset_pulse_count()
        self.encoderRight.reset_pulse_count()
        #set speed
        self.speed = self.rotationspeed
        #set direction wheels
        self.directionLeftWheel = False
        self.directionRightWheel = True
        #calculate setpoints
        self.setpointPulseCountLeft = numberOf45degreeRotations*self.pulsesPer45DegreeCarRotation
        self.setpointPulseCountRight = numberOf45degreeRotations*self.pulsesPer45DegreeCarRotation

    def turnRight(self,numberOf45degreeRotations):
        #reset pulse count
        self.encoderLeft.reset_pulse_count()
        self.encoderRight.reset_pulse_count()
        #set speed
        self.speed = self.rotationspeed
        #set wheel direction
        self.directionLeftWheel = True
        self.directionRightWheel = False
        #calculate setpoints
        self.setpointPulseCountLeft = numberOf45degreeRotations*self.pulsesPer45DegreeCarRotation
        self.setpointPulseCountRight = numberOf45degreeRotations*self.pulsesPer45DegreeCarRotation
    
    
    def getPWM(self):
        self.currentPulseCountLeft = self.encoderLeft.get_pulse_count()
        self.currentPulseCountRight= self.encoderRight.get_pulse_count()
        
        if  self.currentPulseCountLeft >= self.setpointPulseCountLeft:
            self.leftPWM = 0
        elif self.currentPulseCountLeft >= self.setpointPulseCountLeft-5:
            self.leftPWM = self.speed*0.5
        elif self.currentPulseCountLeft >= self.setpointPulseCountLeft-2:
            self.leftPWM = self.speed*0.1
        else:
            self.leftPWM = self.speed

        #if self.setpointPulseCountRight >= self.currentPulseCountRight:
        if self.currentPulseCountRight >= self.setpointPulseCountRight:
            self.rightPWM = 0
        elif self.currentPulseCountRight >= self.setpointPulseCountRight-5:
            self.rightPWM = self.speed*0.5
        elif self.currentPulseCountRight >= self.setpointPulseCountRight-2:
            self.rightPWM = self.speed*0.1
        else:
            self.rightPWM = self.speed

        # safty control, left and right PWM should always be the same. If not, choose the smaller one
        if self.leftPWM > self.rightPWM:
            self.leftPWM = self.rightPWM
        if self.rightPWM > self.leftPWM:
            self.rightPWM = self.leftPWM 

        return self.leftPWM, self.rightPWM, self.directionLeftWheel, self.directionRightWheel


try:
    motor = Driving()
    motor.__init__()
    leftPin = GPIO.PWM(LEFTMOTOR, 50)
    leftPin.start(0)
    motor.driveForward(30, 1)
    while True:
        leftDC, rightDC, dir1, dir2 = motor.getPWM()
        leftPin.ChangeDutyCycle(leftDC)
        print(leftDC)
        time.sleep(0.01)


except KeyboardInterrupt:  # Exit by pressing CTRL + C
    leftPin.stop()
    GPIO.cleanup()
    print("Measurement stopped by User")
finally:
    leftPin.stop()
    GPIO.cleanup()