Python driver update

README.md

@@ -1,8 +1,11 @@
 # We Robot Eurobot 2020 Autonomus Robot
 
+## Architecture
+
 The architecture is divided in 3 distincs parts:
 
 - the `driver` code, created in Python, this is the code that drive the robot, main logic of the navigation
 - the `dash` code, created in React, this is the front-end, user interface of this whole system. This interface can be handy to configure the robot, simulate the robot and assist the creation of the robot driver code by providing a pretty graphic simulation of the table.
 - the `server` code, written in Node.js, which supervise the driver process, recover it in case of a crash and connect the dashboard to the driver throught a websocket connexion. 
 
+

dash/src/App.js

@@ -41,6 +41,7 @@ export default class App extends React.Component {
     this.onCanvasClick = this.onCanvasClick.bind(this);
     this.toggleMesure = this.toggleMesure.bind(this)
     this.forward = this.forward.bind(this)
+    this.resetPosition = this.resetPosition.bind(this)
     this.stop = this.stop.bind(this)
     this.crossGroup = null
     this.mesurePoints = []
@@ -98,9 +99,12 @@ export default class App extends React.Component {
           <Button variant="contained" color="primary" onClick={this.goto} style={{marginTop: '1em'}}>
             Go TO
           </Button>
-          <Button variant="contained" color="primary" onMouseDown={this.forward} onMouseUp={this.stop} style={{marginTop: '1em'}}>
-            Forward
+          <Button variant="contained" color="secondary" onClick={this.resetPosition} style={{marginTop: '1em'}}>
+            Reset
           </Button>
+          {/* <Button variant="contained" color="primary" onMouseDown={this.forward} onMouseUp={this.stop} style={{marginTop: '1em'}}>
+            Forward
+          </Button> */}
           <Button variant="contained" color="primary" onClick={this.stop} style={{marginTop: '1em'}}>
             Stop
           </Button>
@@ -131,6 +135,13 @@ export default class App extends React.Component {
     }))
   }
 
+  resetPosition() {
+    console.log('Reset position')
+    this.socket.send(JSON.stringify({
+        cmd: 'reset_pos'
+    }))
+  }
+
   goto() {
     console.log('toggle goto')
     this.socket.send(JSON.stringify({

driver/app.py

@@ -21,14 +21,13 @@
 from sys import exit
 from os import _exit
 from math import pi
-from adafruit_crickit import crickit
-from src.InputManager import *
+#from src.InputManager import *
 from src.PositionWatcher import PositionWatcher
 from src.WebsocketManager import WebsocketManager
 
 
 websocketManager = WebsocketManager()
-websocketManager.start()
+#websocketManager.start()
 
 positionWatcher = PositionWatcher(websocketManager)
 positionWatcher.start()
@@ -44,17 +43,22 @@
 #     print('Eel enabled!')
 #     positionWatcher.enableEel()
 
+
 def main():
+    robot.stop()
+    sleep(1)
+    robot.stop()
     print('Initialization')
-    while True:
-        time.sleep(1)
+    robot.goTo(0, 25, 5, True)
+    print('> exit')
 
 try:
     main()
 
 except KeyboardInterrupt:
     print('Interrupted')
-    robot.stopMotors()
+    robot.cancelOperations()
+    robot.stop()
     try:
         exit(0)
     except SystemExit:

driver/runMotors.py

@@ -0,0 +1,28 @@
+import time
+import sys
+from sys import exit
+from os import _exit
+from adafruit_crickit import crickit
+
+motor1, motor2 = crickit.dc_motor_1, crickit.dc_motor_2
+
+def main():
+    print("main")
+    motor1.throttle = -1
+    motor2.throttle = 1
+    while True:
+        time.sleep(1)
+
+try:
+    main()
+
+except KeyboardInterrupt:
+    print('Interrupted')
+    motor1.throttle = 0
+    motor2.throttle = 0
+
+    try:
+        exit(0)
+    except SystemExit:
+        _exit(0)
+

driver/src/InputManager.py

@@ -1,8 +1,7 @@
 from threading import Thread
 from time import sleep
 import json
-from src.Motor import Motor
-from adafruit_crickit import crickit
+from src.Motors import Motors
 
 class InputExecutor(Thread):
     def __init__(self, robot, params):

driver/src/Motors.py

@@ -0,0 +1,75 @@
+from gpiozero import DigitalOutputDevice
+from gpiozero import PWMOutputDevice
+
+class Motors:
+
+    def __init__(self):
+        #///////////////// Define Motor Driver GPIO Pins /////////////////
+        # Motor A, Left Side GPIO CONSTANTS
+        PWM_DRIVE_LEFT = 12     # ENA - H-Bridge enable pin
+        FORWARD_LEFT_PIN = 8   # INA1 - Forward Drive
+        REVERSE_LEFT_PIN = 7   # INA2 - Reverse Drive
+        # Motor B, Right Side GPIO CONSTANTS
+        PWM_DRIVE_RIGHT = 18    # ENB - H-Bridge enable pin
+        FORWARD_RIGHT_PIN = 24  # INB1 - Forward Drive
+        REVERSE_RIGHT_PIN = 23   # INB2 - Reverse Drive
+
+        STANDBY_PIN = 25
+
+        self.driveLeft = PWMOutputDevice(PWM_DRIVE_LEFT, True, 0, 1000)
+        self.driveRight = PWMOutputDevice(PWM_DRIVE_RIGHT, True, 0, 1000)
+
+        self.forwardLeft = DigitalOutputDevice(FORWARD_LEFT_PIN)
+        self.reverseLeft = DigitalOutputDevice(REVERSE_LEFT_PIN)
+
+        self.forwardRight = DigitalOutputDevice(REVERSE_RIGHT_PIN)
+        self.reverseRight = DigitalOutputDevice(FORWARD_RIGHT_PIN)
+
+        # self.driveLeft = PWMOutputDevice(PWM_DRIVE_RIGHT, True, 0, 1000)
+        # self.driveRight = PWMOutputDevice(PWM_DRIVE_LEFT, True, 0, 1000)
+
+        # self.forwardLeft = DigitalOutputDevice(FORWARD_RIGHT_PIN)
+        # self.reverseLeft = DigitalOutputDevice(REVERSE_RIGHT_PIN)
+
+        # self.forwardRight = DigitalOutputDevice(FORWARD_LEFT_PIN)
+        # self.reverseRight = DigitalOutputDevice(REVERSE_LEFT_PIN)
+
+        self.standBy = DigitalOutputDevice(STANDBY_PIN) 
+        self.standBy.value = True
+
+
+    def setLeft(self, value):
+        if value < 0:
+            value = -value
+            self.reverseLeft.value = True
+            self.forwardLeft.value = False
+        else:
+            self.reverseLeft.value = False
+            self.forwardLeft.value = True
+        print('Left', value)
+        self.driveLeft.value = value
+
+    def setRight(self, value):
+        if value < 0:
+            value = -value
+            self.reverseRight.value = True
+            self.forwardRight.value = False
+        else:
+            self.reverseRight.value = False
+            self.forwardRight.value = True
+        print('Right', value)
+        self.driveRight.value = value
+
+    def setBoth(self, value):
+        self.setLeft(value)
+        self.setRight(value)
+
+    def stop(self):
+        print('MOTORS: Stop Called')
+        self.reverseLeft.value = False
+        self.forwardLeft.value = False
+        self.forwardRight.value = False
+        self.reverseRight.value = False
+        self.driveRight.value = 0
+        self.driveLeft.value = 0
+

driver/src/PositionWatcher.py

@@ -6,9 +6,10 @@
 class PositionWatcher:
     perimeter = 205
     axialDistance = 233.5
-    theta = pi/2
-    x = 932
-    y = 98
+    initialTheta = pi/2
+    theta = 0
+    x = 0
+    y = 0
 
     # left
     phaseA = DigitalInputDevice(15, True)
@@ -39,6 +40,7 @@ class PositionWatcher:
 
     def __init__(self, websocketManager):
         self.websocketManager = websocketManager
+        self.theta = self.initialTheta
 
     def watchTicks(self):
         while self.enabled:
@@ -112,3 +114,8 @@ def getOrientationDeg(self):
 
     def setOnPositionChangedHandler(self, handler):
         self.onPositionChangedHandler = handler
+
+    def reset(self):
+        self.x = self.y = self.leftTicks = self.rightTicks = 0
+        self.theta = self.initialTheta
+        self.leftState = self.leftOldState = self.rightState = self.rightOldState = self.oldTicks = (0, 0)

driver/src/Robot.py

@@ -1,19 +1,20 @@
-from adafruit_crickit import crickit
 from math import *
 from time import sleep
 from gpiozero import DigitalInputDevice
+from .Motors import Motors
 
 class Robot:
-    leftMotor, rightMotor = crickit.dc_motor_1, crickit.dc_motor_2
     positionWatcher = None
     xR = yR = orientation = erreurPre = differenceErreurs = 0
     sommeErreurs = 0
     leftEndSwitch = DigitalInputDevice(13, True)
     rightEndSwitch = DigitalInputDevice(21, True)
     running = False
+    motors = None
 
     def __init__(self, positionWatcher):
         self.positionWatcher = positionWatcher
+        self.motors = Motors()
 
     def fetch(self):
         self.xR = self.positionWatcher.getPos()[0]
@@ -27,12 +28,14 @@ def goToOrientation(self, targetTheta):
             self.fetch()
             deltaTheta = targetTheta - self.orientation
             if abs(deltaTheta) > pi:
-                deltaTheta = (2*pi - abs(deltaTheta)) * (-deltaTheta / abs(deltaTheta))
+                deltaTheta = (2*pi - abs(deltaTheta)) * - \
+                deltaTheta / abs(deltaTheta)
             if abs(deltaTheta) > seuilOrientation:
-                self.setBoth(0.8 * deltaTheta/abs(deltaTheta) + (0.2/pi/(deltaTheta)))
+                self.motors.setBoth(0.8 * deltaTheta/abs(deltaTheta) + (0.2/pi/(deltaTheta)))
             else:
                 self.running = False
-                self.stopMotors()
+                self.motors.stop()
+
 
     def goToPath(self, path, threehold):
         inc = 0
@@ -44,23 +47,18 @@ def goToPath(self, path, threehold):
             self.goTo(point[0], point[1], t, mustStop)
             inc += 1
         print('ALL COMPLETED')
-
-    def setLeft(self, pwm):
-        self.leftMotor.throttle = pwm
-
-    def setRight(self, pwm):
-        self.rightMotor.throttle = pwm
 
-    def setBoth(self, pwm):
-        self.setLeft(-pwm)
-        self.setRight(pwm)
+    # def setBoth(self, pwm):
+    #     self.setLeft(-pwm)
+    #     self.setRight(pwm)
 
     def goTo(self, targetX, targetY, threehold, mustStop= False, backward = False):
+        print('> GOTO: ', {"x":targetX, "y":targetY, "threehold": threehold, "mustStop": mustStop, "backward": backward})
         self.fetch()
         a = 0
-        p, i, d = 190, 4, 550
+        p, i, d = 160, 2, 500
         #p, i, d = 190, 2, 400
-        vitesseC, vitesseR = 0.7 , 0.4
+        vitesseC, vitesseR = 0.7 , 0.5
         self.running = True
         self.sommeErreurs = 0
         distanceCibleI = sqrt((targetX-self.xR)*(targetX-self.xR)+(targetY-self.yR)*(targetY-self.yR))
@@ -86,7 +84,6 @@ def goTo(self, targetX, targetY, threehold, mustStop= False, backward = False):
 
             while abs(erreurOrientation) > pi:
                 erreurOrientation += (-2*pi) * (erreurOrientation/abs(erreurOrientation))
-
             cmd = (erreurOrientation*p) + (self.sommeErreurs*i) + (self.differenceErreurs*d)
             cmdD += cmd
             cmdG -= cmd
@@ -117,26 +114,19 @@ def goTo(self, targetX, targetY, threehold, mustStop= False, backward = False):
 
             try:
                 if (not backward):
-                    self.leftMotor.throttle = cmdG
-                    self.rightMotor.throttle = cmdD
+                    self.motors.setLeft(cmdG)
+                    self.motors.setRight(cmdD)
                 else:
-                    self.leftMotor.throttle = -cmdD
-                    self.rightMotor.throttle = -cmdG
+                    self.motors.setLeft(-cmdD)
+                    self.motors.setRight(-cmdG)
             except ValueError:
                 print('_____________________ERREUR________________________')
                 print(-cmdG, cmdD)
             if distanceCible < threehold:
                 self.running = False
 
-            print('arrivé !  position:', (self.xR, self.yR, degrees(self.orientation)))
-
-        print('exit of the loop')
-        if mustStop:
-            self.stopMotors()
-            self.goToOrientation(pi/2)
-
-    def stopMotors(self):
-        self.leftMotor.throttle = self.rightMotor.throttle = 0
+        print('arrivé !  position:', (self.xR, self.yR, degrees(self.orientation)))
+        if mustStop: self.motors.stop()
 
     def cancelOperations(self):
         self.running = False
@@ -153,11 +143,14 @@ def logStateLoop(self):
             self.fetch()
             print(self.xR, self.yR, degrees(self.orientation))
             sleep(0.1)
+
+    def stop(self):
+        self.motors.stop()
 
     def goUntilTouched(self):
-        self.leftMotor.throttle = -0.5
-        self.rightMotor.throttle = -0.5
+        self.motors.setLeft(-0.5)
+        self.motors.setRight(-0.5)
         while not(self.leftEndSwitch.value and self.rightEndSwitch.value):
             sleep(0)
-        self.stopMotors()
+        self.motors.stop()