I96 follow path (#128)

* Added randomToken() function * updated createUser and authUser * Fixed authUser not resolving if username or pass is not found * WIP withAuth modifier * Made initial schema with current api * Marked every API file with version 1 * WIP Initial schema v2 * Follow path forwards and backwards if threading didnt exist * Follow path forwards and backwards if threading didnt exist * Added Robot Schema * Added missing stuff to schema * Fix threading issue and reorganise code * Auth implementation with username check only. * FIxed bugs in auth after testing * Removed leftover auth code from previous implementation * Moved auth documentation to README.md * Removed unnecessary line * WIP Updating order endpoints to v2 * Updated most warehouse endpoints to v2 * Added more checks to addItem * Fixed typo * Fixed slight error in schema * Updated schema so location is a geopoint instead of a query * Move sideways count blue markers * try reversing to get back on line * I104 memory mongo (#107) * Added mongodb-memory-server dependency * Refactored db.js to use async/await * Fixed small typo in refactoring * Refactored model.js to use promises and fixed a few bugs along the way * Added fake in-memory mongodb option to the server * Added automatic initialization of the fake database with fake data. * npm run dev uses fake db by default * Added npm run dev-real option * move side to side * Removed unnecessary conditions on some endpoints. * Added error handling to schema * get back on line method * ignore blue after travelling sideways until a green marker is seen * renamed followLineServer to followLine * change run methods to have distance and reverse parameters * Added POST /warehouse to server * Added more example data to fakedb * Fixed a few bugs with the server * I41 (#113) * Added Firebase Functions to server * Moved Firebase project to root and reset changes made to server project * Added README.md and tidied firebase.json * Added Content to README.md * Changes to index.js * Final Updates to structure and README * Generating warehouse floor states from .csv file * Removed print * Path Finding Starting * Demo 1 (#87) * Added android project * Added travis config file * travis now tests the server at the correct directory * Added build status image to main README.md * I34 (#40) * Added travis config file * travis now tests the server at the correct directory * Added build status image to main README.md * Fixed server tests in travis * Added beta branch build status to README.md * I32 (#36) * Set up server npm project * Implemented basic TCP socket server * Added RobotConnection tests * Added README.md * Added note about Prettier. * I35 (#42) * added directory for robot software * added toddler * Removed root .idea * create MOTOR_FORWARD variable * I37 (#48) * Added Firebase Functions to server * Moved Firebase project to root and reset changes made to server project * Added README.md and tidied firebase.json * Added Content to README.md * Changes to index.js * Final Updates to structure and README * Removed the server directory * Removed the server directory (#50) * Added jest testing to firebase functions. * Jest temporarily passes without tests. * I51 (#52) * Removed the server directory * Added jest testing to firebase functions. * Jest temporarily passes without tests. * Fixed travis deployment not working * I51 (#53) * Removed the server directory * Added jest testing to firebase functions. * Jest temporarily passes without tests. * Fixed travis deployment not working * Potential fix for firebase deployment * Fixed firebase functions deployment on Travis * Added master firebase project to travis deploy * I55 (#58) * Removed firebase folder * Removed firebase functions deployment from travis.yaml * Random IntelliJ changes * I56 (#67) * move robot forward using both motors * add skeleton class for detecting line * create line follower object in toddler * try to fix but everything breaks * add detectLine * makes BOB sidestep * detect colour * can differentiate between black line and white paper * I45 (#57) * move robot forward using both motors * add skeleton class for detecting line * create line follower object in toddler * try to fix but everything breaks * add detectLine * makes BOB sidestep * detect colour * can differentiate between black line and white paper * move but can't stay on line * use 2 colour sensors * Merge branch 'i56' of https://github.com/Assis10t/assis10t into i56 Conflicts: ev3/detectLine.py * stops bob only turning right * follow line decently * make code nicer * I58 (#69) * Removed firebase folder * Removed firebase functions deployment from travis.yaml * Random IntelliJ changes * Made initial hello world server * Added more packages * Minor change to README * Minor change to README * Basic implementation of a CRUD server with mongodb * Added Bonjour Publisher and Client * added bonjour * Robot comms teset * WIP makefile * Re-added package-lock.json * Robot turns on when button pressed * Android can now find server via zeroconf * Added test mode for when disconnected from robot * Refactored ServerConnection * Updated server name in android * Added turn off on * Added Feedback * Added Off/On Switch * Final touches * I66 (#70) * start detect marker * add stop method * add runner on separate thread * bOb stops when detecting green line * beobe stops after 3 lines man * I71 (#72) * start detect marker * stop on blue * I60 Connected Android to server (#74) * Removed firebase folder * Removed firebase functions deployment from travis.yaml * Random IntelliJ changes * Made initial hello world server * Added more packages * Minor change to README * Minor change to README * Basic implementation of a CRUD server with mongodb * Added Bonjour Publisher and Client * added bonjour * Robot comms teset * WIP makefile * Re-added package-lock.json * Robot turns on when button pressed * Android can now find server via zeroconf * Added test mode for when disconnected from robot * Refactored ServerConnection * Updated server name in android * Added turn off on * Fixed minor things * App can display list of items * Re-added addItem and getItems * Fixed app not connecting to server * Added ability to refresh list of items * Implemented adding order on android * Adding order now removes the items in it from the database * I76 (#79) * start detect marker * stop on blue * added function for sideways movement * I78 Basic authentication (#83) * Login works on the server & mini website. * Implemented Android login. * Added zeroconf bypass * Added back button to LoginActivity * I81 Bypass zeroconf (#84) * Login works on the server & mini website. * Implemented Android login. * Added zeroconf bypass * addOrder now starts moving the robot (#85) * I58 (#86) * Removed firebase folder * Removed firebase functions deployment from travis.yaml * Random IntelliJ changes * Made initial hello world server * Added more packages * Minor change to README * Minor change to README * Basic implementation of a CRUD server with mongodb * Added Bonjour Publisher and Client * added bonjour * Robot comms teset * WIP makefile * Re-added package-lock.json * Robot turns on when button pressed * Android can now find server via zeroconf * Added test mode for when disconnected from robot * Refactored ServerConnection * Updated server name in android * Added turn off on * Added Feedback * Added Off/On Switch * Final touches * Updated Website * additional changes * Added Input Verification * Merge beta * Fixed merge issues and other bugs * Added Robot Pathfinding JS * Added More Pathfinding] * Get Next Job Added to Index.js * basic pathfinding * Added Order Updating * WIP * WIP * WIP changes for robot command * Added mongo_client * Mongo Client gets new orders * Updated To Server V2 * Added `npm run randomid` * Added some fields to the API schema (non-breaking changes) * Fixed merge issues and inconsistencies in fake_db.json * Making an order now correctly reduces the quantity of items instead of removing them. Also added GET /db for debugging. * Updated with Kieran's Stuff * Intergrated Kieran's Changes * cleaned up code * Demo 2 Fixes * make sideways movement work with current arena layout * if sideways instruction arrives after forwards/backwards bob will keep moving until he sees blue * add follow path * integrated get back on line * Updated model
Assis10t · Mar 2, 2019 · b238294 · b238294
1 parent 9d1a582
commit b238294
Show file tree

Hide file tree

Showing 14 changed files with 387 additions and 161 deletions.
diff --git a/.DS_Store b/.DS_Store
diff --git a/robot_software/__pycache__/bob_translation.cpython-36.pyc b/robot_software/__pycache__/bob_translation.cpython-36.pyc
diff --git a/robot_software/bobTranslation.py b/robot_software/bobTranslation.py
@@ -0,0 +1,13 @@
+# Takes the job object of the received JSON from the server, and returns a list
+# of tuples (direction, distance) for bob to execute.
+def extract(job):
+ bobIns = []
+ for j in job:
+ command = j["command"]
+ if(command == "move"):
+ bobIns.append((j["parameters"]["direction"], j["parameters"]["blocks"]))
+ elif(command == "lift"):
+ bobIns.append(("L", j["parameters"]["height"]))
+ elif(command == "grab"):
+ bobIns.append(("G", 0))
+ return bobIns
diff --git a/ev3/control.py → robot_software/control.py b/ev3/control.py → robot_software/control.py
@@ -18,6 +18,6 @@ def calculate_torque(lval, rval, DT, integral, previous_error):
  # u negative: too dark, turn left
  # u is torque (See IVR lecture on Control)
  u = (KP * error) + (KI * integral) + (KD * derivative)
- print("u:", u)
+ #print("u:", u)
  logging.info("PID torque: ", u)
- return u, integral, error
+ return u, integral, error
diff --git a/robot_software/detectMarking.py b/robot_software/detectMarking.py
@@ -0,0 +1,13 @@
+#! /usr/bin/env python3
+import ev3dev.ev3 as ev3
+import logging
+from time import sleep
+
+
+class DetectMarking:
+
+ # Constructor
+ def __init__(self):
+ self.btn = ev3.Button()
+ self.shut_down = False
+
diff --git a/robot_software/follow.py b/robot_software/follow.py
diff --git a/robot_software/followLine.py b/robot_software/followLine.py
@@ -0,0 +1,246 @@
+#! /usr/bin/env python3
+import ev3dev.ev3 as ev3
+import logging
+from time import sleep, time
+import control
+
+
+class FollowLine:
+ # From https://gist.github.com/CS2098/ecb3a078ed502c6a7d6e8d17dc095b48
+ MOTOR_SPEED = 700
+ KP = 10
+ KD = 0.5 # derivative gain medium
+ KI = 0 # integral gain lowest
+ DT = 50 # milliseconds - represents change in time since last sensor reading/
+
+ MARKING_NUMBER = 2 # number of consecutive colour readings to detect marking
+ MARKING_INTERVAL = 1 # time between marking checks in seconds
+ REVERSE = False
+
+ # Constructor
+ def __init__(self):
+ self.btn = ev3.Button()
+ self.shut_down = False
+
+ # colour sensors
+ self.csfl = ev3.ColorSensor('in1') # colour sensor front left
+ self.csfr = ev3.ColorSensor('in2') # colour sensor front right
+ self.csbl = ev3.ColorSensor('in3') # colour sensor back left
+ self.csbr = ev3.ColorSensor('in4') # colour sensor back right
+ assert self.csfl.connected
+ assert self.csfr.connected
+ assert self.csbl.connected
+ assert self.csbr.connected
+ self.csfl.mode = 'COL-REFLECT' # measure light intensity
+ self.csfr.mode = 'COL-REFLECT' # measure light intensity
+ self.csbl.mode = 'COL-COLOR' # measure colour
+ self.csbr.mode = 'COL-COLOR'
+ # motors
+ self.lm = ev3.LargeMotor('outA') # left motor
+ self.rm = ev3.LargeMotor('outC') # right motor
+ self.cm = ev3.LargeMotor('outD') # centre motor
+ assert self.lm.connected
+ assert self.rm.connected
+ assert self.cm.connected
+
+ self.blue = 2
+ self.green = 3
+
+ self.consecutive_colours = 0 # counter for consecutive colour readings
+ self.ignore_blue = False # when switching from sideways to forwards
+ self.ignore_green = False # when switching from fowards to sideways
+ #self.number_of_markers = 0 # at which marker it should stop
+
+ def detect_marking(self, colour_left, colour_right):
+ if (colour_right == 3 and colour_left == 3) or (colour_right == 2 and colour_left == 2): # 3 = green 2 = blue
+ self.consecutive_colours += 1
+ # print("CONSECUTIVE COLOURS: ", self.consecutive_colours)
+ if self.consecutive_colours > self.MARKING_NUMBER:
+ return colour_right
+ else:
+ self.consecutive_colours = 0
+ return -1
+
+ # limit motor speed to safe values: [-1000, 1000] deg/sec
+ def limit_speed(self, speed):
+ if speed > 1000:
+ return 1000
+ if speed < -1000:
+ return -1000
+ return speed
+
+ def correct_trajectory(self, number_of_markers, reverse):
+ integral = 0
+ previous_error = 0
+ marker_counter = 0
+ start_time = time()
+ time_off_line = 0
+ self.ignore_green = True
+
+ if reverse:
+ self.csfl.mode = 'COL-COLOR' # measure colour
+ self.csfr.mode = 'COL-COLOR' # measure colour
+ self.csbl.mode = 'COL-REFLECT' # measure light intensity
+ self.csbr.mode = 'COL-REFLECT' # measure light intensity
+ else:
+ self.csfl.mode = 'COL-REFLECT' # measure light intensity
+ self.csfr.mode = 'COL-REFLECT' # measure light intensity
+ self.csbl.mode = 'COL-COLOR' # measure colour
+ self.csbr.mode = 'COL-COLOR'
+
+ # Assign sensors to act as front or back
+ while not self.shut_down:
+ if reverse:
+ lval = self.csbr.value() # back right becomes front left
+ rval = self.csbl.value()
+ else:
+ lval = self.csfl.value()
+ rval = self.csfr.value()
+
+ # most likely off line, may need to recalibrate numbers later
+ time_off_line = self.get_back_on_line(lval, rval, time_off_line)
+
+ # Calculate torque using PID control
+ u, integral, previous_error = control.calculate_torque\
+ (lval, rval, self.DT, integral, previous_error)
+ # Set the speed of the motors
+ speed_left = self.limit_speed(self.MOTOR_SPEED + u)
+ speed_right = self.limit_speed(self.MOTOR_SPEED - u)
+
+ # run motors
+ if reverse:
+ self.lm.run_timed(time_sp=self.DT, speed_sp=speed_right)
+ self.rm.run_timed(time_sp=self.DT, speed_sp=speed_left)
+ else:
+ self.lm.run_timed(time_sp=self.DT, speed_sp=-speed_left)
+ self.rm.run_timed(time_sp=self.DT, speed_sp=-speed_right)
+ sleep(self.DT / 1000)
+
+ # print("u {}".format(u))
+ # print("lm {}\n".format(lm.speed_sp))
+ # print("rm {}".format(rm.speed_sp))
+ # print("PID:", lval, rval)
+
+ # Check markers
+ # Wait before checking for colour again
+ if time() - start_time > self.MARKING_INTERVAL:
+ if reverse:
+ colour_left = self.csfr.value()
+ colour_right = self.csfl.value()
+ else:
+ colour_left = self.csbl.value()
+ colour_right = self.csbr.value()
+
+ # returns 3 if green, 2 if blue
+ marker_colour = self.detect_marking(colour_left, colour_right)
+ if marker_colour == self.green:
+ # stop after given number of greens
+ self.ignore_blue = False
+ marker_counter += 1
+ ev3.Sound.beep()
+ start_time = time()
+ if marker_counter >= number_of_markers:
+ # self.stop()
+ return
+ elif marker_colour == self.blue and not self.ignore_blue:
+ # stop on blue marker
+ # self.stop()
+ # self.reverse = not self.reverse
+ return
+
+ def run_sideways(self, distance, reverse):
+ self.ignore_blue = False
+
+ # If previous instruction was forwards or backwards
+ # keep moving until a blue line is seen
+ if self.ignore_green:
+ self.correct_trajectory(99, self.REVERSE)
+
+ self.ignore_green = False
+
+ if reverse:
+ self.csfl.mode = 'COL-COLOR' # measure light intensity
+ self.csfr.mode = 'COL-REFLECT' # measure colour
+ self.csbl.mode = 'COL-COLOR' # measure light intensity
+ self.csbr.mode = 'COL-REFLECT' # measure colour
+ else:
+ self.csfl.mode = 'COL-REFLECT' # measure light intensity
+ self.csfr.mode = 'COL-COLOR' # measure colour
+ self.csbl.mode = 'COL-REFLECT' # measure light intensity
+ self.csbr.mode = 'COL-COLOR' # measure colour
+
+ marker_counter = 0
+ start_time = time()
+ while not self.shut_down:
+ if reverse:
+ self.cm.run_timed(time_sp=self.DT/2, speed_sp=-500)
+ else:
+ self.cm.run_timed(time_sp=self.DT/2, speed_sp=500)
+ sleep(self.DT / 2000)
+
+ if time() - start_time > self.MARKING_INTERVAL:
+ if reverse:
+ colour_left = self.csfl.value()
+ colour_right = self.csbl.value()
+ else:
+ colour_left = self.csbr.value()
+ colour_right = self.csfr.value()
+
+
+ # returns 3 if green, 2 if blue
+ marker_colour = self.detect_marking(colour_left, colour_right)
+ if marker_colour == self.blue:
+ # stop after given number of blues
+ marker_counter += 1
+ ev3.Sound.beep()
+ start_time = time()
+ if marker_counter >= distance:
+ # self.stop()
+ self.ignore_blue = True
+ return
+ elif marker_colour == self.green:
+ # stop on green marker
+ # self.stop()
+ # self.reverse = not self.reverse
+ return
+
+ # move forwards/backwards
+ def run_forwards(self, distance, reverse):
+ self.REVERSE = reverse
+ self.correct_trajectory(distance, reverse)
+
+ # when line is lost oscillate side to side until it is found
+ def get_back_on_line(self, lval, rval, time_off_line):
+ if lval > 90 and rval > 70:
+ if time_off_line == 0:
+ time_off_line = time()
+ # if off line for more than a second move side-to-side until line is found
+ print(time() - time_off_line)
+ if time() - time_off_line > 0.5:
+ correction_speed = 200
+ correction_time = 100
+ # can change thresholds
+ while lval > 70 and rval > 50:
+ self.cm.run_timed(time_sp=correction_time, speed_sp=correction_speed)
+ correction_speed *= -1
+ # increase the time to move in one direction to increased the search radius
+ correction_time += 100
+ sleep(correction_time / 1000) # milliseconds to seconds
+ lval = self.csfl.value()
+ rval = self.csfr.value()
+ time_off_line = 0
+ else:
+ time_off_line = 0
+ return time_off_line
+
+ def stop(self):
+ self.shut_down = True
+ self.rm.stop()
+ self.lm.stop()
+ ev3.Sound.speak("bruh").wait()
+
+
+# Main function
+if __name__ == "__main__":
+ robot = FollowLine()
+ robot.run_forwards(2, False)