diff --git a/.DS_Store b/.DS_Store index 208aae8..c9e757b 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/robot_software/__pycache__/bob_translation.cpython-36.pyc b/robot_software/__pycache__/bob_translation.cpython-36.pyc new file mode 100644 index 0000000..3754979 Binary files /dev/null and b/robot_software/__pycache__/bob_translation.cpython-36.pyc differ diff --git a/robot_software/bobTranslation.py b/robot_software/bobTranslation.py new file mode 100644 index 0000000..d42fa83 --- /dev/null +++ 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 \ No newline at end of file diff --git a/ev3/control.py b/robot_software/control.py similarity index 92% rename from ev3/control.py rename to robot_software/control.py index 58394ae..235b643 100644 --- a/ev3/control.py +++ b/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 \ No newline at end of file + return u, integral, error diff --git a/robot_software/detectMarking.py b/robot_software/detectMarking.py new file mode 100644 index 0000000..04d43e9 --- /dev/null +++ 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 deleted file mode 100644 index b2dbc7d..0000000 --- a/robot_software/follow.py +++ /dev/null @@ -1,101 +0,0 @@ -#! /usr/bin/env python3 -import ev3dev.ev3 as ev3 -import logging -from time import sleep - - -class FollowLine: - # From https://gist.github.com/CS2098/ecb3a078ed502c6a7d6e8d17dc095b48 - MOTOR_SPEED = 700 - KP = 20 - KD = 0.1 # derivative gain medium - KI = 0 # integral gain lowest - - DT = 50 # milliseconds - represents change in time since last sensor reading/movement - - # Constructor - def __init__(self): - self.btn = ev3.Button() - self.shut_down = False - - def onLine(self, sensor_value, position): - if position == 'left': - return sensor_value < 30 - if position == 'right': - return sensor_value < 40 - logging.error("onLine: wrong position value for sensor") - return False - - def correctTrajectory(self, csfl, csfr, lm, rm): - integral = 0 - previous_error = 0 - - while not self.shut_down: - lval = csfl.value() - rval = csfr.value() - error = lval - rval - 10 - logging.info("PID error: ", error) - integral += (error * self.DT) - derivative = (error - previous_error) / self.DT - - # u zero: on target, drive forward - # u positive: too bright, turn right - # u negative: too dark, turn left - # u is torque (See IVR lecture on Control) - u = (self.KP * error) + (self.KI * integral) + (self.KD* derivative) - - # limit u to safe values: [-1000, 1000] deg/sec - if self.MOTOR_SPEED + abs(u) > 1000: # reduce u if speed and torque are too high - if u >= 0: - u = 1000 - self.MOTOR_SPEED - else: - u = self.MOTOR_SPEED - 1000 - - # run motors - lm.run_timed(time_sp=self.DT, speed_sp=-(self.MOTOR_SPEED + u)) - rm.run_timed(time_sp=self.DT, speed_sp=-(self.MOTOR_SPEED - u)) - 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) - - previous_error = error - - def run(self): - - # colour sensors - csfl = ev3.ColorSensor('in1') # colour sensor front left - csfr = ev3.ColorSensor('in2') # colour sensor front right - assert csfl.connected - assert csfr.connected - csfl.mode = 'COL-REFLECT' # measure light intensity - csfr.mode = 'COL-REFLECT' # measure light intensity - - # motors - lm = ev3.LargeMotor('outA') # left motor - rm = ev3.LargeMotor('outC') # right motor - assert lm.connected - assert rm.connected - self.correctTrajectory(csfl, csfr, lm, rm) - - """ - lm.run_forever(speed_sp=-self.MOTOR_SPEED) - rm.run_forever(speed_sp=-self.MOTOR_SPEED) - while not self.shut_down: - print(csfl.value(), csfr.value()) - if self.onLine(csfl.value(), 'left') or self.onLine(csfr.value(), 'right'): # left sensor is on the line - lm.stop() - rm.stop() - self.correctTrajectory(csfl, csfr, lm, rm) - """ - rm.stop() - lm.stop() - - -# Main function -if __name__ == "__main__": - robot = FollowLine() - robot.run() diff --git a/robot_software/followLine.py b/robot_software/followLine.py new file mode 100644 index 0000000..e227baa --- /dev/null +++ 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) diff --git a/robot_software/followPath.py b/robot_software/followPath.py new file mode 100644 index 0000000..8ce61aa --- /dev/null +++ b/robot_software/followPath.py @@ -0,0 +1,54 @@ +#! /usr/bin/env python3 +import ev3dev.ev3 as ev3 +from followLine import FollowLine +from threading import Thread + + +class FollowPath: + # Follow a path given by the server + + # Constructor + def __init__(self): + self.shut_down = False + self.runner = None + + def go(self, path): + line_follower = FollowLine() + for direction, distance in path: + print(direction, distance) + # direction move in forwards axis or side axis + if direction == 'forward': + line_follower.run_forwards(distance, False) + elif direction == 'backward': + line_follower.run_forwards(distance, True) + elif direction == 'left': + line_follower.run_sideways(distance, False) + elif direction == 'right': + line_follower.run_sideways(distance, True) + elif direction == 'G': + ev3.Sound.speak("Grab").wait() + else: + ev3.Sound.speak("Wrong command given. What does", direction, "mean?").wait() + line_follower.stop() + + # TODO: possibly move start and stop to FollowPath or move correct trajectory to a separate file instead + def start(self, path): + self.shut_down = False + if len(path) == 0: + ev3.Sound.speak("No instructions given").wait() + else: + self.runner = Thread(target=self.go, args=(path,), name='go') + self.runner.start() + + +# Main function +if __name__ == "__main__": + path_follower = FollowPath() + # X = forwards + # Y = sideways + # G = grab + current_path = [('left', 1), ('forward', 2), ('backward', 2), ('left',2), ('forward', 4), ('right', 2), + ('backward', 4), ('right', 1)] + #current_path = [('Y', 1), ('X', 2), ('G', 0), ('X', -1), ('G', 0), ('X', 3), ('Y', 1), ('G', 0), ('X', -4), + # ('Y', -1)] + path_follower.start(current_path) diff --git a/robot_software/lineFollower.py b/robot_software/lineFollower.py deleted file mode 100644 index c84d7c1..0000000 --- a/robot_software/lineFollower.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env python - -import time - -class LineFollower: - - MOTOR_FRONT_LEFT = 2 - MOTOR_FRONT_RIGHT = 3 - - def __init__(self, IO): # can change this to pass in the individual sensors and motors instead of entire IO - # get the sensors from the phidget board - self.getSensors = IO.interface_kit.getSensors - self.left_sensor = self.getSensors()[0] - self.right_sensor = self.getSensors()[1] - self.mc = IO.motor_control - - def motor_speed(self): - while True: - # Stage 1 - if self.left_sensor == 0 and self.right_sensor == 0: # change ranges to match sensor values - # continue straight - self.mc.setMotor(self.MOTOR_FRONT_LEFT, 1) - self.mc.setMotor(self.MOTOR_FRONT_RIGHT, 1) - # Stage 2 - if self.left_sensor == 0 and self.right_sensor == 1: - # adjust - self.mc.setMotor(self.MOTOR_FRONT_RIGHT, -1) - if self.left_sensor == 1 and self.right_sensor == 0: - self.mc.setMotor(self.MOTOR_FRONT_LEFT, -1) - #print(r, l) diff --git a/robot_software/mongo_client.py b/robot_software/mongo_client.py index b8a94ba..f2844fc 100644 --- a/robot_software/mongo_client.py +++ b/robot_software/mongo_client.py @@ -6,7 +6,7 @@ ev3_package_check = True try: import ev3dev.ev3 as ev3 - from followLineServer import FollowLine + from followLine import FollowLine print("ev3 modules imported") except: traceback.print_exc() @@ -30,6 +30,10 @@ print("threading imported") from zeroconf import ServiceBrowser, Zeroconf print("zeroconf imported") +from bobTranslation import extract +print("bobTranslation imported") +from followPath import FollowPath +print("followPath imported") #remove me last_json = {} @@ -44,15 +48,17 @@ def polling(ip_addr, port, run_robot,username): headers = {'username':username} r = requests.get("http://{}:{}/robotjob".format(ip_addr,port),headers=headers) path = json.loads(r.text) - if (path["job"] == []): + if path["job"] == []: print("No order") else: print(path['job']) - #TODO: pass to robot + path_tuples = extract(path['job']['instruction_set']) + print("Path tuples: ", path_tuples) + robot_boy = FollowPath() + robot_boy.start(path_tuples) except: - url = "http://{}:{}/getmovement".format(ip_addr,port) - print("GET request: {} failed at {}".format(url,datetime.datetime.now())) + traceback.print_exc() time.sleep(5) diff --git a/server/index.js b/server/index.js index b18cf39..b8528b7 100644 --- a/server/index.js +++ b/server/index.js @@ -207,7 +207,7 @@ app.post('/register', (req, res, next) => { .then(user => { if (req.body.type == 'robot') { model - .addRobot(user.username, 0, 0) + .addRobot(user.username, 4, 0) .then(res.json({ success: true, user })) .catch(next) } else { diff --git a/server/model.js b/server/model.js index d2e8e86..c0c6d8b 100644 --- a/server/model.js +++ b/server/model.js @@ -168,8 +168,8 @@ const factory = db => ({ home_x: home_x, home_y: home_y, location: { - x: 0, - y: 0, + x: home_x, + y: home_y, z: 0 } }, @@ -188,7 +188,7 @@ const factory = db => ({ rej(err) } else { db() - .collection('warehouse') + .collection('warehouses') .find({}) .toArray((err, warehouse) => { db() diff --git a/server/package-lock.json b/server/package-lock.json index a586192..9a6cfb2 100644 --- a/server/package-lock.json +++ b/server/package-lock.json @@ -1928,7 +1928,8 @@ "ansi-regex": { "version": "2.1.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "aproba": { "version": "1.2.0", @@ -1949,12 +1950,14 @@ "balanced-match": { "version": "1.0.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "brace-expansion": { "version": "1.1.11", "bundled": true, "dev": true, + "optional": true, "requires": { "balanced-match": "^1.0.0", "concat-map": "0.0.1" @@ -1969,17 +1972,20 @@ "code-point-at": { "version": "1.1.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "concat-map": { "version": "0.0.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "console-control-strings": { "version": "1.1.0", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "core-util-is": { "version": "1.0.2", @@ -2096,7 +2102,8 @@ "inherits": { "version": "2.0.3", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "ini": { "version": "1.3.5", @@ -2108,6 +2115,7 @@ "version": "1.0.0", "bundled": true, "dev": true, + "optional": true, "requires": { "number-is-nan": "^1.0.0" } @@ -2122,6 +2130,7 @@ "version": "3.0.4", "bundled": true, "dev": true, + "optional": true, "requires": { "brace-expansion": "^1.1.7" } @@ -2129,12 +2138,14 @@ "minimist": { "version": "0.0.8", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "minipass": { "version": "2.3.5", "bundled": true, "dev": true, + "optional": true, "requires": { "safe-buffer": "^5.1.2", "yallist": "^3.0.0" @@ -2153,6 +2164,7 @@ "version": "0.5.1", "bundled": true, "dev": true, + "optional": true, "requires": { "minimist": "0.0.8" } @@ -2233,7 +2245,8 @@ "number-is-nan": { "version": "1.0.1", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "object-assign": { "version": "4.1.1", @@ -2245,6 +2258,7 @@ "version": "1.4.0", "bundled": true, "dev": true, + "optional": true, "requires": { "wrappy": "1" } @@ -2330,7 +2344,8 @@ "safe-buffer": { "version": "5.1.2", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "safer-buffer": { "version": "2.1.2", @@ -2366,6 +2381,7 @@ "version": "1.0.2", "bundled": true, "dev": true, + "optional": true, "requires": { "code-point-at": "^1.0.0", "is-fullwidth-code-point": "^1.0.0", @@ -2385,6 +2401,7 @@ "version": "3.0.1", "bundled": true, "dev": true, + "optional": true, "requires": { "ansi-regex": "^2.0.0" } @@ -2428,12 +2445,14 @@ "wrappy": { "version": "1.0.2", "bundled": true, - "dev": true + "dev": true, + "optional": true }, "yallist": { "version": "3.0.3", "bundled": true, - "dev": true + "dev": true, + "optional": true } } }, diff --git a/server/robot-pathfinding.js b/server/robot-pathfinding.js index 730ddce..a2efb62 100644 --- a/server/robot-pathfinding.js +++ b/server/robot-pathfinding.js @@ -17,17 +17,22 @@ makeWarehouse = (width, height) => { // add shelves for (var s = 0; s < number_of_shelves; s++) { for (var i = 1; i < width - 1; i++) { - walkable_grid[s * 2 + 1][i] = 1 + walkable_grid[s * 2][i] = 1 } } + console.log(walkable_grid) return walkable_grid } pathfind_to_point = (current_pos, end_pos, warehouse_grid) => { var pf_grid = new PF.Grid(warehouse_grid) var finder = new PF.AStarFinder() + var path = finder.findPath(current_pos[0], current_pos[1], end_pos[0], end_pos[1], pf_grid) path = PF.Util.compressPath(path) + console.log(current_pos) + console.log(end_pos) + console.log(path) return path } generate_drop_instruction = () => { @@ -37,7 +42,8 @@ generate_drop_instruction = () => { } generate_grab_instruction = () => { return { - command: 'grab' + command: 'grab', + parameters: {} } } @@ -55,24 +61,24 @@ generate_movement_instruction = (start, end) => { var num_blocks = end[0] - start[0] var direction = '' if (num_blocks < 0) { - direction = 'left' + direction = 'forward' num_blocks = num_blocks * -1 } else { - direction = 'right' + direction = 'backward' } } else { var num_blocks = end[1] - start[1] var direction = '' if (num_blocks < 0) { - direction = 'backwards' + direction = 'right' num_blocks = num_blocks * -1 } else { - direction = 'forwards' + direction = 'left' } } return { command: 'move', - parameter: { + parameters: { blocks: num_blocks, direction: direction } @@ -92,7 +98,7 @@ convert_order_to_job = (order, robot, warehouse_grid) => { var robot_xy = [robot_pos['x'], robot_pos['y'], robot_pos['z']] for (var i = 0; i < item_list.length; i++) { var current_item = item_list[i]['position'] - var item_xy = [current_item['x'], current_item['y'] * 2, robot_pos['z']] + var item_xy = [current_item['x'], current_item['y'], robot_pos['z']] var path = pathfind_to_point(robot_xy, item_xy, warehouse_grid) if (path == [] || path == undefined) {