Merge remote-tracking branch 'origin/master' into master

mappingbot/constants.py

@@ -22,7 +22,7 @@ ULTRASOUND_TRIGGER_RIGHT = 26  # was 22
 # ----- Numerical Constants -----
 
 WHEEL_DIAMETER = 6.4  # Wheel diameter in cm
-WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER / 2 * PI  # wheel circumference in cm
+WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * PI  # wheel circumference in cm
 
 ENCODER_PPR = 40  # Encoder pulses per rotation (assuming both rising and falling edges)
 

mappingbot/driving.py

@@ -4,9 +4,9 @@ import time
 import math
 from encoder import Encoder
 #import PWMoutputFunction # what is this?
-#GPIO Mode(BOARD / BCM)
-from mappingbot.encoder import Encoder
-from mappingbot.constants import MOTOR_LEFT_FORWARD, MOTOR_RIGHT_FORWARD, MOTOR_RIGHT_BACKWARD, MOTOR_LEFT_BACKWARD, \
+
+from encoder import Encoder
+from constants import MOTOR_LEFT_FORWARD, MOTOR_RIGHT_FORWARD, MOTOR_RIGHT_BACKWARD, MOTOR_LEFT_BACKWARD, \
     ENCODER_LEFT, ENCODER_RIGHT, WHEEL_CIRCUMFERENCE, ENCODER_PPR
 
 # import PWMoutputFunction # what is this?
@@ -17,26 +17,29 @@ class Driving:
     def __init__(self):
         self.circumference = WHEEL_CIRCUMFERENCE
         self.setpointPulseCountLeft = 0
-        self.setpointPulseCuntRight = 0
+        self.setpointPulseCountRight = 0
         self.currentPulseCountLeft = 0
         self.currentPulseCountRight = 0
         self.directionLeftWheel = True  # true = forward
         self.directionRightWheel = True  # true = forward
         self.pulsesPerRotation = ENCODER_PPR
-        self.pulsesPer45DegreeCarRotation = 10
+        self.pulsesPerRadian = 2*ENCODER_PPR/(2*math.pi)
         self.speed = 0  # speed in PWM dutycycle, ranges from 0 to 100
         self.rotationspeed = 50  # speed in PWM dutycycle
         self.wheelDistance = 14.0
         self.x = 0
         self.y = 0
         self.orientation = 0
+        self.setpointX = 0
+        self.setpointY = 0
+        self.setpointOrientation = 0
         self.oldPCL = 0
         self.oldPCR = 0
         self.leftPWM = 0
         self.rightPWM = 0
 
         GPIO.setup(MOTOR_LEFT_FORWARD, GPIO.OUT)
-        GPIO.setup(MOTOR_RIGHT_FORWARD, GPIO.OUT)
+        GPIO.setup(MOTOR_LEFT_BACKWARD, GPIO.OUT)
         GPIO.setup(MOTOR_RIGHT_FORWARD, GPIO.OUT)
         GPIO.setup(MOTOR_RIGHT_BACKWARD, GPIO.OUT)
 
@@ -63,6 +66,9 @@ class Driving:
             return False
 
     def driveForward(self, distance, speed):
+        # update setpoint
+        self.setpointX += math.cos(self.setpointOrientation) * distance
+        self.setpointY += math.sin(self.setpointOrientation) * distance
         # reset pulse count
         self.encoderLeft.reset_pulse_count()
         self.encoderRight.reset_pulse_count()
@@ -77,6 +83,9 @@ class Driving:
         self.drive()
 
     def driveBackward(self, distance, speed):
+        # update setpoint
+        self.setpointX -= math.cos(self.setpointOrientation) * distance
+        self.setpointY -= math.sin(self.setpointOrientation) * distance
         # reset pulse count
         self.encoderLeft.reset_pulse_count()
         self.encoderRight.reset_pulse_count()
@@ -91,7 +100,9 @@ class Driving:
         self.drive()
 
     def turnLeft(self, numberOf45degreeRotations):
-        self.setpointOrientation += numberOf45degreeRotations * math.pi / 8        # reset pulse count
+        # update setpoint
+        self.setpointOrientation -= numberOf45degreeRotations * math.pi / 4
+        # reset pulse count
         self.encoderLeft.reset_pulse_count()
         self.encoderRight.reset_pulse_count()
         # set speed
@@ -100,12 +111,27 @@ class Driving:
         self.directionLeftWheel = False
         self.directionRightWheel = True
         # calculate setpoints
-        self.setpointPulseCountLeft = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
-        self.setpointPulseCountRight = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
+        # self.setpointPulseCountLeft = abs(self.setpointOrientation-self.orientation) * self.pulsesPerRadian
+        # self.setpointPulseCountRight = abs(self.setpointOrientation-self.orientation) * self.pulsesPerRadian
+        # print(abs(self.setpointOrientation-self.orientation))
+        # self.setpointPulseCountLeft = 0.5 * self.pulsesPerRotation * abs(
+        #     self.setpointOrientation - self.orientation) * self.wheelDistance / self.circumference
+        # self.setpointPulseCountRight = 0.5 * self.pulsesPerRotation * abs(
+        #     self.setpointOrientation - self.orientation) * self.wheelDistance / self.circumference
+        # print("setpointL: " + str(self.setpointPulseCountLeft))
+        # print("alpha: " + str(abs(self.setpointOrientation - self.orientation)))
+        self.setpointPulseCountLeft = 0.5 * self.pulsesPerRotation * abs(
+            self.setpointOrientation - self.orientation) * self.wheelDistance / self.circumference
+        self.setpointPulseCountRight = 0.5 * self.pulsesPerRotation * abs(
+            self.setpointOrientation - self.orientation) * self.wheelDistance / self.circumference
+        print("setpoint: " + str(self.setpointOrientation))
+        #self.setpointPulseCountLeft = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
+        #self.setpointPulseCountRight = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
         self.drive()
 
     def turnRight(self, numberOf45degreeRotations):
-        self.setpointOrientation -= numberOf45degreeRotations * math.pi / 8
+        # update setpoint
+        self.setpointOrientation += numberOf45degreeRotations * math.pi / 4
         # reset pulse count
         self.encoderLeft.reset_pulse_count()
         self.encoderRight.reset_pulse_count()
@@ -115,28 +141,39 @@ class Driving:
         self.directionLeftWheel = True
         self.directionRightWheel = False
         # calculate setpoints
-        self.setpointPulseCountLeft = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
-        self.setpointPulseCountRight = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
+        # self.setpointPulseCountLeft = abs(self.setpointOrientation - self.orientation) * self.pulsesPerRadian
+        # self.setpointPulseCountRight = abs(self.setpointOrientation - self.orientation) * self.pulsesPerRadian
+        #self.orientation += diff * (self.circumference / self.pulsesPerRotation) / (
+        #            self.wheelDistance * 2 * math.pi) * 2 * math.pi
+        self.setpointPulseCountLeft = 0.5*self.pulsesPerRotation*abs(self.setpointOrientation - self.orientation)*self.wheelDistance/self.circumference
+        self.setpointPulseCountRight = 0.5*self.pulsesPerRotation*abs(self.setpointOrientation - self.orientation)*self.wheelDistance/self.circumference
+        print("setpoint: " + str(self.setpointOrientation))
+        # print("alpha: " + str(abs(self.setpointOrientation - self.orientation)))
+        # self.setpointPulseCountLeft = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
+        # self.setpointPulseCountRight = numberOf45degreeRotations * self.pulsesPer45DegreeCarRotation
         self.drive()
 
     def getPWM(self):
         self.currentPulseCountLeft = self.encoderLeft.get_pulse_count()
-        self.currentPulseCountRight= self.encoderRight.get_pulse_count()
-        #print("pulseCnt: " + str(self.currentPulseCountLeft) + "    setpoint: " + str(self.setpointPulseCountLeft))
+        self.currentPulseCountRight = self.encoderRight.get_pulse_count()
+        # print(str(self.currentPulseCountLeft) + "   " + str(self.currentPulseCountRight))
+        # print("pulseCnt: " + str(self.currentPulseCountLeft) + "    setpoint: " + str(self.setpointPulseCountLeft))
         orientationError = self.setpointOrientation - self.orientation
         if self.directionLeftWheel == self.directionRightWheel:
             gainOrientation = 10
         else:
             gainOrientation = 0
-        print(orientationError)
+        gainOrientation = 0
+        #print(orientationError)
         self.currentPulseCountRight = self.encoderRight.get_pulse_count()
         # print("pulseCnt: " + str(self.currentPulseCountLeft) + "    setpoint: " + str(self.setpointPulseCountLeft))
 
+
         if self.currentPulseCountLeft >= self.setpointPulseCountLeft:
             self.leftPWM = 0
-        elif self.currentPulseCountLeft >= self.setpointPulseCountLeft - 5:
+        elif self.currentPulseCountLeft >= self.setpointPulseCountLeft - 10:
             self.leftPWM = self.speed * 0.5
-        elif self.currentPulseCountLeft >= self.setpointPulseCountLeft - 2:
+        elif self.currentPulseCountLeft >= self.setpointPulseCountLeft - 5:
             self.leftPWM = self.speed * 0.1
         else:
             self.leftPWM = self.speed - gainOrientation * orientationError
@@ -144,18 +181,31 @@ class Driving:
         # if self.setpointPulseCountRight >= self.currentPulseCountRight:
         if self.currentPulseCountRight >= self.setpointPulseCountRight:
             self.rightPWM = 0
-        elif self.currentPulseCountRight >= self.setpointPulseCountRight - 5:
+        elif self.currentPulseCountRight >= self.setpointPulseCountRight - 10:
             self.rightPWM = self.speed * 0.5
-        elif self.currentPulseCountRight >= self.setpointPulseCountRight - 2:
+        elif self.currentPulseCountRight >= self.setpointPulseCountRight - 5:
             self.rightPWM = self.speed * 0.1
         else:
             self.rightPWM = self.speed + gainOrientation * orientationError
 
-        # 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
+        if self.directionLeftWheel != self.directionRightWheel:
+            if abs(self.setpointOrientation - self.orientation) < 0.01:
+                self.leftPWM = 0
+                self.rightPWM = 0
+
+            elif abs(self.setpointOrientation - self.orientation) < 0.3:
+                self.leftPWM *= 0.4
+                self.rightPWM *= 0.4
+                print(self.leftPWM)
+
+
+
+
+        # # 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
 
@@ -166,8 +216,6 @@ class Driving:
         leftDC, rightDC, dirLeft, dirRight = motor.getPWM()
         diffPCL = self.currentPulseCountLeft - self.oldPCL
         diffPCR = self.currentPulseCountRight - self.oldPCR
-        #print(diffPCL)
-        #print(diffPCR)
         if dirLeft:
             self.leftPinF.ChangeDutyCycle(leftDC)
             self.leftPinB.ChangeDutyCycle(0)
@@ -209,6 +257,9 @@ class Driving:
         self.x += math.cos(self.orientation) * move * (self.circumference / self.pulsesPerRotation)
         self.y += math.sin(self.orientation) * move * (self.circumference / self.pulsesPerRotation)
         self.orientation += diff*(self.circumference / self.pulsesPerRotation)/(self.wheelDistance * 2 * math.pi)*2*math.pi
+        print("angle: " + str(self.orientation))
+        print("alpha: " + str(abs(self.setpointOrientation - self.orientation)))
+
         #self.orientation += diff
         #print(diff)
         # print("x: " + str(self.x))
@@ -218,16 +269,20 @@ class Driving:
         #print("pcr: " + str(self.currentPulseCountLeft) + "old: " + str(self.oldPCL) + "diff: " + str(self.currentPulseCountLeft - self.oldPCL))
         #print("LeftDC: " + str(leftDC) + "  RightDC: " + str(rightDC))
         if not (leftDC == 0 and rightDC == 0):
-            time.sleep(0.1)
+            time.sleep(0.005)
             self.drive()
         else:
+            print("next")
             self.oldPCR = 0
             self.oldPCL = 0
             self.currentPulseCountLeft = 0
             self.currentPulseCountRight = 0
-            print("x: " + str(self.x))
-            print("y: " + str(self.y))
-            print("angle: " + str(self.orientation))
+            # print("x: " + str(self.x))
+            # print("y: " + str(self.y))
+            # print("angle: " + str(self.orientation))
+            # print("SetX: " + str(self.setpointX))
+            # print("SetY: " + str(self.setpointY))
+            # print("SetAngle: " + str(self.setpointOrientation))
         #     #self.x += 0.5*(self.currentPulseCountRight+self.currentPulseCountLeft)*(self.circumference/self.pulsesPerRotation)
         #
         #     diff = abs(self.currentPulseCountRight - self.currentPulseCountLeft)
@@ -252,16 +307,16 @@ try:
     # leftPinB.start(0)
     # rightPinF.start(0)
     # rightPinB.start(0)
-    motor.driveForward(30, 50)
-    motor.turnRight(2)
-    motor.driveForward(30, 50)
-    motor.turnLeft(2)
-    motor.driveBackward(30, 50)
-    motor.turnLeft(2)
-    motor.driveForward(30, 50)
+    # motor.driveForward(30, 50)
+    # motor.turnRight(2)
+    # motor.driveForward(30, 50)
+    # motor.turnLeft(2)
+    # motor.driveBackward(30, 50)
+    # motor.turnLeft(2)
+    # motor.driveForward(30, 50)
     motor.turnRight(2)
 
-
+    motor.turnLeft(2)
     # motor.drive()
 
     # while True:

mappingbot/encoder.py

@@ -2,7 +2,7 @@ import RPi.GPIO as GPIO
 import time
 import math
 
-from mappingbot.constants import ENCODER_PPR, WHEEL_CIRCUMFERENCE
+from constants import ENCODER_PPR, WHEEL_CIRCUMFERENCE
 
 GPIO.setmode(GPIO.BCM)
 

mappingbot/storage.py

 import numpy as np
 from pathlib import Path
-from mappingbot.constants import DATA_FOLDER, DATA_FILENAME
+from constants import DATA_FOLDER, DATA_FILENAME
 
 
 class DataStorage:

mappingbot/ultrasound.py

@@ -5,9 +5,9 @@ import time
 from pathlib import Path
 import numpy as np
 
-from mappingbot.constants import ULTRASOUND_ECHO_FORWARD, ULTRASOUND_ECHO_LEFT, ULTRASOUND_ECHO_RIGHT, \
+from constants import ULTRASOUND_ECHO_FORWARD, ULTRASOUND_ECHO_LEFT, ULTRASOUND_ECHO_RIGHT, \
     ULTRASOUND_TRIGGER_FORWARD, ULTRASOUND_TRIGGER_LEFT, ULTRASOUND_TRIGGER_RIGHT, SONIC_SPEED
-from mappingbot.storage import DataStorage
+from storage import DataStorage
 
 
 class UltrasoundManager: