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

ev3/detectLine.py

@@ -0,0 +1,57 @@
+#! /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
+ #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') # 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.run()

robot_software/lineFollower.py

@@ -0,0 +1,32 @@
+#!/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.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)

robot_software/toddler.py

@@ -3,9 +3,13 @@
 import time
 import numpy
 import cv2
+from lineFollower import LineFollower
 
 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')
@@ -14,14 +18,16 @@ def __init__(self, IO):
  self.getSensors = IO.interface_kit.getSensors
  self.mc = IO.motor_control
  self.sc = IO.servo_control
+ self.follower = LineFollower(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)
+ self.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)