def gotoXY(self, x, y, speed=30, block=True): ''' Move to absolute position (x,y). moves axes simultaneously. homeXY must be called first. Args: x: position of the x-axis (mm) y: position of the y-axis (mm) speed: the speed of the axes (mm/s) block (bool): block until move is complete ''' # start move to x position self.xMotor.on_to_position(SpeedNativeUnits(speed * self.xRatio), x * self.xRatio, brake=False, block=False) # start move to y position self.yMotor.on_to_position(SpeedNativeUnits(speed * self.yRatio), y * self.yRatio, brake=False, block=False) if block: # sleep until either move is complete or motors stall self.xMotor.wait_until_not_moving(self.defaultTimeout) self.yMotor.wait_until_not_moving(self.defaultTimeout)
def driveStraightForDistance(self, distance, speedLeft=None, speedRight=None, stopAtEnd=True): ''' Drive for given distance. Args: distance: distance to drive in mm speedLeft: speed in mm/s (left motor) speedRight: speed in mm/s (right motor) stopAtEnd (bool): brake at end of movement ''' if speedLeft == None or speedLeft == 0: speedLeft = self.defaultSpeed if speedRight == None or speedRight == 0: speedRight = self.defaultSpeed degPerSecLeft = 360 * speedLeft / (pi * self.wheel_diameter) degPerSecRight = 360 * speedRight / (pi * self.wheel_diameter) driveTime = StopWatch() driveTime.start() self.leftDriveMotor.on(SpeedNativeUnits(degPerSecLeft), block=False) self.rightDriveMotor.on(SpeedNativeUnits(degPerSecRight), block=False) while driveTime.value_ms <= abs(distance / speedLeft) * 1000: sleep(0.02) if stopAtEnd: self.leftDriveMotor.stop() self.rightDriveMotor.stop() sleep(0.01)
def test_units(self): clean_arena() populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')]) m = Motor() self.assertEqual(SpeedPercent(35).get_speed_pct(m), 35) self.assertEqual(SpeedDPS(300).get_speed_pct(m), 300 / 1050 * 100) self.assertEqual(SpeedNativeUnits(300).get_speed_pct(m), 300 / 1050 * 100) self.assertEqual(SpeedDPM(30000).get_speed_pct(m), (30000 / 60) / 1050 * 100) self.assertEqual(SpeedRPS(2).get_speed_pct(m), 360 * 2 / 1050 * 100) self.assertEqual(SpeedRPM(100).get_speed_pct(m), (360 * 100 / 60) / 1050 * 100)
def follow_line(self, kp, ki, kd, side, speed=SpeedPercent(50), target_light_intensity=None, follow_left_edge=True, white=60, off_line_count_max=20, sleep_time=0.01, follow_for=follow_for_forever, **kwargs): target_light = self.sLS_left other_light = self.sLS_right if target_light_intensity is None: time.sleep(1) target_light_intensity = self.get_light_intensitiy(side) buffed = target_light_intensity + 2.5 target_light_intensity_ls = other_light.reflected_light_intensity - 3 integral = 0.0 last_error = 0.0 derivative = 0.0 off_line_count = 0 speed_native_units = -speed.to_native_units(self.motor_left) MAX_SPEED = SpeedNativeUnits(self.max_speed / 2) switch_locked = False while follow_for(self, **kwargs): if args.light_mode == 'ambient': target = target_light.ambient_light_intensity other = other_light.ambient_light_intensity else: target = target_light.reflected_light_intensity other = other_light.reflected_light_intensity print("{:.3f}->{:.3f} | {:.3f}->{:.3f} | {:.3f} <-> {:.3f} | {}". format(target, target_light_intensity, other, target_light_intensity_ls, target_light_intensity, target_light_intensity_ls, follow_left_edge)) if other < buffed and not switch_locked: follow_left_edge = False target_light = self.sLS_right other_light = self.sLS_left _tmp = target_light_intensity target_light_intensity = target_light_intensity_ls target_light_intensity_ls = _tmp else: follow_left_edge = True target_light = self.sLS_left other_light = self.sLS_right #if reflected_light_intensity > buffed*2 and follow_left_edge is False and reflected_light_intensity_ls > target_light_intensity_ls*2: # follow_left_edge = True # switch_locked = False error = target_light_intensity - target integral = integral + error derivative = error - last_error last_error = error turn_native_units = -((kp * error) + (ki * integral) + (kd * derivative)) if not follow_left_edge: turn_native_units *= -1 #if error < last_error: # speed_native_units -= 2 #elif error > last_error: # speed_native_units += 5 right_speed = SpeedNativeUnits(speed_native_units - turn_native_units) left_speed = SpeedNativeUnits(speed_native_units + turn_native_units) #print(turn_native_units, speed_native_units) # DEBUG #print('Error: %s' % error) #print('reflected_light_intesity: : %s' % reflected_light_intensity) #print('integral: %s' % integral) #print('derivative: %s' % derivative) #print('last_error: %s' % last_error) #print('turn_native_units: %s' % turn_native_units) #print('left_speed: %s' % left_speed) #print('right_speed: %s' % right_speed) if left_speed > MAX_SPEED: print("%s: left_speed %s is greater than MAX_SPEED %s" % (self, left_speed, MAX_SPEED)) self.mMT.stop() raise LineFollowErrorTooFast( "The robot is moving too fast to follow the line") if right_speed > MAX_SPEED: print("%s: right_speed %s is greater than MAX_SPEED %s" % (self, right_speed, MAX_SPEED)) self.mMT.stop() raise LineFollowErrorTooFast( "The robot is moving too fast to follow the line") # Have we lost the line? if target >= white: off_line_count += 1 if off_line_count >= off_line_count_max: #self.mMT.stop() #raise LineFollowErrorLostLine("we lost the line") pass else: off_line_count = 0 if sleep_time: time.sleep(sleep_time) #self.mMT.on(left_speed, right_speed) self.mMT.stop()
def followLineDist(self, distance, sensor, sideToFollow, stopAtEnd, speed, gain_multiplier=1): ''' Drive forward following line. Uses direct control of drive motors and PD control. Args: distance: distance to follow line in mm sensor: colorsensor to use for line following sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT stopAtEnd (bool): Stop motors at end of line following speed: speed in mm/s gain_multiplier: multiplier for P and D gains ''' degPerSec = 360 * speed / (pi * self.wheel_diameter) propGain = 6.0 * gain_multiplier derivGain = 6 * gain_multiplier target = (self.blackThreshold + self.whiteThreshold) / 2 print("******* starting line following ********") # print("error,Pterm,Dterm") # initialize term for derivative calc previousError = target - avgReflection(sensor, 2) i = 0 driveTime = StopWatch() driveTime.start() while driveTime.value_ms <= abs(distance / speed) * 1000: # calc error and proportional term error = target - avgReflection(sensor, 2) Pterm = propGain * error # calc d(error)/d(t) and derivative term d_error_dt = error - previousError Dterm = derivGain * d_error_dt previousError = error if sideToFollow == LineEdge.RIGHT: self.leftDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm + Dterm), block=False) self.rightDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm - Dterm), block=False) # eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging if sideToFollow == LineEdge.LEFT: self.leftDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm - Dterm), block=False) self.rightDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm + Dterm), block=False) # eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging # eprint("line following complete") if stopAtEnd: self.leftDriveMotor.stop() self.rightDriveMotor.stop() sleep(0.01) # Play sound when we stop line following sound.beep()
def follow_line(self, kp, ki, kd, speed, target_light_intensity=None, follow_left_edge=True, white=60, off_line_count_max=20, sleep_time=0.01, follow_for=follow_for_forever, prox_treshold=50, **kwargs): """ PID line follower ``kp``, ``ki``, and ``kd`` are the PID constants. ``speed`` is the desired speed of the midpoint of the robot ``target_light_intensity`` is the reflected light intensity when the color sensor is on the edge of the line. If this is None we assume that the color sensor is on the edge of the line and will take a reading to set this variable. ``follow_left_edge`` determines if we follow the left or right edge of the line ``white`` is the reflected_light_intensity that is used to determine if we have lost the line ``off_line_count_max`` is how many consecutive times through the loop the reflected_light_intensity must be greater than ``white`` before we declare the line lost and raise an exception ``sleep_time`` is how many seconds we sleep on each pass through the loop. This is to give the robot a chance to react to the new motor settings. This should be something small such as 0.01 (10ms). ``follow_for`` is called to determine if we should keep following the line or stop. This function will be passed ``self`` (the current ``MoveTank`` object). Current supported options are: - ``follow_for_forever`` - ``follow_for_ms`` ``**kwargs`` will be passed to the ``follow_for`` function Example: .. code:: python from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms from ev3dev2.sensor.lego import ColorSensor tank = MoveTank(OUTPUT_A, OUTPUT_B) tank.cs = ColorSensor() try: # Follow the line for 4500ms tank.follow_line( kp=11.3, ki=0.05, kd=3.2, speed=SpeedPercent(30), follow_for=follow_for_ms, ms=4500 ) except Exception: tank.stop() raise """ assert self.cs, "ColorSensor must be defined" if target_light_intensity is None: target_light_intensity = self.cs.reflected_light_intensity integral = 0.0 last_error = 0.0 derivative = 0.0 off_line_count = 0 speed_native_units = speed.to_native_units(self.left_motor) print(speed_native_units) MAX_SPEED = SpeedNativeUnits(self.max_speed) button = Button() while follow_for(self, **kwargs): if "down" in button.buttons_pressed: self.stop() return reflected_light_intensity = self.cs.reflected_light_intensity error = reflected_light_intensity - target_light_intensity integral = integral + error derivative = error - last_error print("target: {:^20} light: {:^20}, deri: {:^5}, integral: {:^5}". format(target_light_intensity, reflected_light_intensity, derivative, integral)) if derivative > 6: #or abs(integral) > 100: self.stop() print("Wrong way") return # self.on_for_degrees(SpeedPercent(-10), SpeedPercent(10), 100) # integral = 0 # last_error = 0 # derivative = 0 # off_line_count = 0 # time.sleep(1) # continue if integral > 150: self.stop() print("Too high integral") integral = -100.0 # We are correcting heading too much to the left, make correction to right self.on_for_degrees(SpeedPercent(30), SpeedPercent(-30), 150) self.on_until_target(target_light_intensity) continue last_error = error turn_native_units = (kp * error) + (ki * integral) + (kd * derivative) if not follow_left_edge: turn_native_units *= -1 left_speed = SpeedNativeUnits(speed_native_units - turn_native_units) right_speed = SpeedNativeUnits(speed_native_units + turn_native_units) # Is distance to wall too close? #if self.us.proximity < prox_treshold: # print("Too close") # self.stop() # in_line = reflected_light_intensity >= white # turn_help = self.get_next_turn() # # If we are still in line, safe to turn straight away # if in_line: # self.on_for_degrees(turn_help[0], turn_help[1], turn_help[2]) # else: # self.on_for_seconds(SpeedPercent(-10), SpeedPercent(-10), 1) # self.on_for_degrees(turn_help[0], turn_help[1], turn_help[2]) # # Reset values for new straigth line # integral = 0 # last_error = 0 # derivative = 0 # off_line_count = 0 # time.sleep(2) # continue # Have we lost the line? if reflected_light_intensity >= white: off_line_count += 1 if off_line_count >= off_line_count_max: self.stop() print("Lost line") raise LineFollowErrorLostLine("we lost the line") else: off_line_count = 0 if sleep_time: time.sleep(sleep_time) self.on(left_speed, right_speed) self.stop()