From 56c1d074fd6e98fb473831ba60519b94f25516fd Mon Sep 17 00:00:00 2001 From: cd6 Date: Tue, 29 Jan 2019 14:59:28 +0000 Subject: [PATCH 1/8] move robot forward using both motors --- robot_software/toddler.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/robot_software/toddler.py b/robot_software/toddler.py index ea0d718..3b5a1c3 100644 --- a/robot_software/toddler.py +++ b/robot_software/toddler.py @@ -5,7 +5,9 @@ import cv2 class Toddler: - MOTOR_FORWARD = 1 + MOTOR_SIDE = 1 + MOTOR_FRONT_LEFT = 2 + MOTOR_FRONT_RIGHT = 3 def __init__(self, IO): print('[Toddler] I am toddler playing in a sandbox') @@ -17,9 +19,11 @@ def __init__(self, IO): def control(self): print('{}\t{}'.format(self.getSensors(), self.getInputs())) - self.mc.setMotor(self.MOTOR_FORWARD, 1) + self.mc.setMotor(self.MOTOR_FRONT_LEFT, 1) + self.mc.setMotor(self.MOTOR_FRONT_RIGHT, 1) time.sleep(0.5) + def vision(self): image = self.camera.getFrame() self.camera.imshow('Camera', image) From 5ba79189e64ff2cc8fb432901ac1ac99f63bf0ea Mon Sep 17 00:00:00 2001 From: cd6 Date: Tue, 29 Jan 2019 16:15:06 +0000 Subject: [PATCH 2/8] add skeleton class for detecting line --- robot_software/lineFollower.py | 31 +++++++++++++++++++++++++++++++ robot_software/toddler.py | 1 + 2 files changed, 32 insertions(+) create mode 100644 robot_software/lineFollower.py diff --git a/robot_software/lineFollower.py b/robot_software/lineFollower.py new file mode 100644 index 0000000..40c8d62 --- /dev/null +++ b/robot_software/lineFollower.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +import time +import numpy +import cv2 + +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.left_sensor = IO.interface_kit.getSensors[0] + self.right_sensor = IO.interface_kit.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) \ No newline at end of file diff --git a/robot_software/toddler.py b/robot_software/toddler.py index 3b5a1c3..5749e02 100644 --- a/robot_software/toddler.py +++ b/robot_software/toddler.py @@ -29,3 +29,4 @@ def vision(self): self.camera.imshow('Camera', image) time.sleep(0.05) + From 211d53fa5d389d9c155e19ca194a32308e75eb5c Mon Sep 17 00:00:00 2001 From: cd6 Date: Wed, 30 Jan 2019 15:47:15 +0000 Subject: [PATCH 3/8] create line follower object in toddler --- robot_software/toddler.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/robot_software/toddler.py b/robot_software/toddler.py index 5749e02..58d77ae 100644 --- a/robot_software/toddler.py +++ b/robot_software/toddler.py @@ -3,8 +3,10 @@ import time import numpy import cv2 +from lineFollower import LineFollower class Toddler: + MOTOR_SIDE = 1 MOTOR_FRONT_LEFT = 2 MOTOR_FRONT_RIGHT = 3 @@ -16,17 +18,16 @@ def __init__(self, IO): self.getSensors = IO.interface_kit.getSensors self.mc = IO.motor_control self.sc = IO.servo_control + follower = LineFollower(IO) def control(self): print('{}\t{}'.format(self.getSensors(), self.getInputs())) - self.mc.setMotor(self.MOTOR_FRONT_LEFT, 1) - self.mc.setMotor(self.MOTOR_FRONT_RIGHT, 1) + # self.mc.setMotor(self.MOTOR_FRONT_LEFT, 1) + # self.mc.setMotor(self.MOTOR_FRONT_RIGHT, 1) + follower.motor_speed() time.sleep(0.5) - def vision(self): image = self.camera.getFrame() self.camera.imshow('Camera', image) - time.sleep(0.05) - - + time.sleep(0.05) \ No newline at end of file From 3096bcff88df582819b9ac9719af31b8ec4e0ffc Mon Sep 17 00:00:00 2001 From: cd6 Date: Thu, 31 Jan 2019 13:57:38 +0000 Subject: [PATCH 4/8] try to fix but everything breaks --- robot_software/lineFollower.py | 15 ++++++++------- robot_software/toddler.py | 4 ++-- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/robot_software/lineFollower.py b/robot_software/lineFollower.py index 40c8d62..0ca161d 100644 --- a/robot_software/lineFollower.py +++ b/robot_software/lineFollower.py @@ -4,28 +4,29 @@ import numpy import cv2 -class LineFollower +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.left_sensor = IO.interface_kit.getSensors[0] - self.right_sensor = IO.interface_kit.getSensors[1] + 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 + # 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 + # 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) \ No newline at end of file + #print(r, l) diff --git a/robot_software/toddler.py b/robot_software/toddler.py index 58d77ae..9d968b3 100644 --- a/robot_software/toddler.py +++ b/robot_software/toddler.py @@ -18,13 +18,13 @@ def __init__(self, IO): self.getSensors = IO.interface_kit.getSensors self.mc = IO.motor_control self.sc = IO.servo_control - follower = LineFollower(IO) + self.follower = LineFollower(IO) def control(self): print('{}\t{}'.format(self.getSensors(), self.getInputs())) # self.mc.setMotor(self.MOTOR_FRONT_LEFT, 1) # self.mc.setMotor(self.MOTOR_FRONT_RIGHT, 1) - follower.motor_speed() + self.follower.motor_speed() time.sleep(0.5) def vision(self): From e969ac5e107c3018e99feb30386cbf5a02906268 Mon Sep 17 00:00:00 2001 From: Alexander Rader Date: Fri, 1 Feb 2019 13:12:41 +0000 Subject: [PATCH 5/8] add detectLine --- ev3/detectLine.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 ev3/detectLine.py diff --git a/ev3/detectLine.py b/ev3/detectLine.py new file mode 100644 index 0000000..e3d7ae7 --- /dev/null +++ b/ev3/detectLine.py @@ -0,0 +1,20 @@ +#! /usr/bin/env python3 +import ev3dev.ev3 as ev3 + +class detectLine: + # Constructor + def __init__(self): + self.btn = ev3.Button() + self.shut_down = False + + def run(self): + cs = ev3.ColorSensor() + assert cs.connected + + cs.mode = 'COL-REFLECT' # measure light intensity + + # motors + flm = ev3.LargeMotor('outA') # forward-left motor + frm = ev3.LargeMotor('outB') # forward-right motor + sm = ev3.LargeMotor('outC') # sidemotor + sm.run_timed(speed_sp=300, time_sp=1000) \ No newline at end of file From 5d2b372603456b9c52c00988a7e6763f42be03e3 Mon Sep 17 00:00:00 2001 From: Alexander Rader Date: Fri, 1 Feb 2019 13:56:02 +0000 Subject: [PATCH 6/8] makes BOB sidestep --- ev3/detectLine.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/ev3/detectLine.py b/ev3/detectLine.py index e3d7ae7..4838bc3 100644 --- a/ev3/detectLine.py +++ b/ev3/detectLine.py @@ -8,13 +8,19 @@ def __init__(self): self.shut_down = False def run(self): - cs = ev3.ColorSensor() - assert cs.connected + #cs = ev3.ColorSensor() + #assert cs.connected - cs.mode = 'COL-REFLECT' # measure light intensity + #cs.mode = 'COL-REFLECT' # measure light intensity # motors - flm = ev3.LargeMotor('outA') # forward-left motor - frm = ev3.LargeMotor('outB') # forward-right motor + flm = ev3.LargeMotor('outA') # front-left motor + frm = ev3.LargeMotor('outB') # front-right motor sm = ev3.LargeMotor('outC') # sidemotor - sm.run_timed(speed_sp=300, time_sp=1000) \ No newline at end of file + sm.run_timed(speed_sp=300, time_sp=1000) + + +# Main function +if __name__ == "__main__": + robot = detectLine() + robot.run() \ No newline at end of file From 370e5a87847a92de099eacd57cee50dc3b4295ec Mon Sep 17 00:00:00 2001 From: Alexander Rader Date: Fri, 1 Feb 2019 14:27:35 +0000 Subject: [PATCH 7/8] detect colour --- ev3/detectLine.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ev3/detectLine.py b/ev3/detectLine.py index 4838bc3..4413d87 100644 --- a/ev3/detectLine.py +++ b/ev3/detectLine.py @@ -8,16 +8,20 @@ def __init__(self): self.shut_down = False def run(self): - #cs = ev3.ColorSensor() - #assert cs.connected + cs = ev3.ColorSensor() + assert cs.connected #cs.mode = 'COL-REFLECT' # measure light intensity + cs.mode = 'COL-COLOR' # measure colour + while(True): + yeet = cs.value() + print(yeet) # motors flm = ev3.LargeMotor('outA') # front-left motor frm = ev3.LargeMotor('outB') # front-right motor sm = ev3.LargeMotor('outC') # sidemotor - sm.run_timed(speed_sp=300, time_sp=1000) + #sm.run_timed(speed_sp=300, time_sp=1000) # Main function From bf609ca49890ea7b3010ced70732f0299e71a152 Mon Sep 17 00:00:00 2001 From: cd6 Date: Fri, 1 Feb 2019 16:13:47 +0000 Subject: [PATCH 8/8] can differentiate between black line and white paper --- ev3/detectLine.py | 47 +++++++++++++++++++++++++++++++++++++---------- 1 file changed, 37 insertions(+), 10 deletions(-) diff --git a/ev3/detectLine.py b/ev3/detectLine.py index 4413d87..e70a61c 100644 --- a/ev3/detectLine.py +++ b/ev3/detectLine.py @@ -1,7 +1,8 @@ #! /usr/bin/env python3 import ev3dev.ev3 as ev3 -class detectLine: + +class DetectLine: # Constructor def __init__(self): self.btn = ev3.Button() @@ -11,20 +12,46 @@ def run(self): cs = ev3.ColorSensor() assert cs.connected - #cs.mode = 'COL-REFLECT' # measure light intensity - cs.mode = 'COL-COLOR' # measure colour - while(True): - yeet = cs.value() - print(yeet) + cs.mode = 'COL-REFLECT' # measure light intensity + #cs.mode = 'COL-COLOR' # measure colour # motors - flm = ev3.LargeMotor('outA') # front-left motor - frm = ev3.LargeMotor('outB') # front-right motor - sm = ev3.LargeMotor('outC') # sidemotor + flm = ev3.LargeMotor('outA') # front-left motor + frm = ev3.LargeMotor('outB') # front-right motor + sm = ev3.LargeMotor('outC') # side motor #sm.run_timed(speed_sp=300, time_sp=1000) + speed = 360 / 4 # deg/sec, [-1000, 1000] + dt = 500 # milliseconds + # stop_action = "coast" + + # PID tuning + Kp = 1 # proportional gain + Ki = 0 # integral gain + Kd = 0 # derivative gain + + integral = 0 + previous_error = 0 + + # initial measurement when using one sensor to callibrate + target_value = cs.value() + + # Start the main loop + while not self.shut_down: + # Calculate steering using PID algorithm + error = target_value - cs.value() + integral += (error * dt) + derivative = (error - previous_error) / dt + + # u zero: on target, drive forward + # u positive: too bright, turn right + # u negative: too dark, turn left + + u = (Kp * error) + (Ki * integral) + (Kd * derivative) + print(u) + # Main function if __name__ == "__main__": - robot = detectLine() + robot = DetectLine() robot.run() \ No newline at end of file