def flip(self):
"""
Motors will sometimes stall if you call on_to_position() multiple
times back to back on the same motor. To avoid this we call a 50ms
sleep in flipper_hold_cube() and after each on_to_position() below.
We have to sleep after the 2nd on_to_position() because sometimes
flip() is called back to back.
"""
log.info("flip()")
if self.shutdown:
return
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# Grab the cube and pull back
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 0
self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
sleep(0.05)
# At this point the cube is at an angle, push it forward to
# drop it back down in the turntable
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
MindCuber.hold_cube_pos)
sleep(0.05)
transformation = [2, 4, 1, 3, 0, 5]
self.apply_transformation(transformation)
def Step4():
LeftAction.on_for_degrees(-50,1400,True,False)
TankPair.on_for_degrees(SpeedDPS(-250),SpeedDPS(-250),30,True,True)
TankPair.on_for_degrees(SpeedDPS(0),SpeedDPS(-250),170,True,True)
TankPair.on_for_degrees(SpeedDPS(-250),SpeedDPS(-250),260,True,True)
TankPair.on_for_degrees(SpeedDPS(-250),SpeedDPS(0),430,True,True)
TankPair.on_for_degrees(SpeedDPS(-250),SpeedDPS(-250),10,True,True)
LeftAction.on_for_degrees(100,200,True,True)
LeftAction.on_for_degrees(100,1200,True,False)
def equilibrar():
ini.motorRight.reset()
g = ini.Gyro.rate # Velocidad Angular del Giroscopio (grados/s)
time.sleep(0.002)
g = g + ini.Gyro.rate
ini.dtheta = g / 2.0 - ini.offset_gyro # Velocidad Angular (grados/s)
ini.offset_gyro = ini.offset_gyro * 0.999 + (
0.001 * (ini.dtheta + ini.offset_gyro)) # Actualizamos el offset
ini.theta = ini.theta + ini.dtheta * ini.dt # Angulo (grados)
ini.theta = ini.theta * 0.999 - ini.theta * 0.001
inout.eprint('\r' + "Angulo: " + str(ini.theta))
inout.eprint("Velocidad Angular: " + str(ini.dtheta))
ini.n = ini.n + 1
if ini.n == ini.n_max:
ini.n = 0
ini.xdes = 0
ini.x = ini.motorLeft.position + ini.motorRight.position # Posición (deg)
ini.n_ant = ini.n + 1
if ini.n_ant == ini.n_max:
ini.n_ant = 0
ini.encoder[ini.n] = ini.x # Posicion (rotaciones)
average = 0
for i in range(ini.n_max):
average = average + ini.encoder[i]
inout.eprint("ini.encoder[" + str(i) + "] = " + str(ini.encoder[i]))
average = average / ini.n_max
inout.eprint("average = " + str(average))
ini.dx = average / ini.dt #Posicion
# contralador PID
k1 = 26.5
k2 = 1.56
k3 = 0.15
k4 = 0.13
ini.e = (k1 * ini.theta + k2 * ini.dtheta + k3 * ini.x + k4 * ini.dx)
ini.de_dt = (ini.e - ini.e_prev) / ini.dt
inout.eprint("Rotacion del motor = " + str(ini.x))
inout.eprint("Velocidad del motor = " + str(ini.dx))
ini.iedt = ini.iedt + ini.e * ini.dt
ini.e_prev = ini.e
inout.eprint("p = " + str(ini.e))
ini.rot_prev = ini.rotacion
if ini.e > 1050:
ini.e = 1050
elif ini.e < -1050:
ini.e = -1050
v = int(ini.e / ini.motorLeft.max_speed * 100)
#ini.motorLeft.on(speed = ini.e)
ini.motorLeft.on(speed=SpeedDPS(ini.e))
ini.motorRight.on(speed=SpeedDPS(ini.e))
inout.eprint("speed = " + str(ini.motorLeft.speed))
inout.eprint("speed = " + str(ini.e))
def Step2():
while RightSensor.color != 6:
TankPair.on(SpeedDPS(0), SpeedDPS(-50))
while RightSensor.color != 1:
TankPair.on(SpeedDPS(-50), SpeedDPS(-50))
while LeftSensor.color != 1:
TankPair.on(SpeedDPS(-50), SpeedDPS(0))
LeftWheel.off()
RightWheel.off
TankPair.on_for_degrees(SpeedDPS(0), SpeedDPS(-100), 75, True, True)
TankPair.on_for_degrees(SpeedDPS(-250), SpeedDPS(-250), 250, True, True)
def turn(self, direc): # Half - works
self.drive.on_for_degrees(SpeedDPS(225), SpeedDPS(225), 223)
if direc == "L" or direc == "l":
self.steer.on_for_degrees(steering=-100,
speed=SpeedDPS(720),
degrees=400)
elif direc == "R" or direc == "r":
self.steer.on_for_degrees(steering=100,
speed=SpeedDPS(720),
degrees=720)
self.steer.off()
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, False)
self.rightMotor.on_for_degrees(SpeedDPS(speedDegreePerSecond),
degreesToRun, brake, block)
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
if speedCmPerSecond < 0:
raise Exception('speed cannot be negative')
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Run - Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftLargeMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, False)
self.rightLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, block)
def LineFollowing(Degree):
DegreeSum = 0
AngleOld = 360 * LeftWheel.position / LeftWheel.count_per_rot
while DegreeSum < Degree:
if LeftSensor.color == ColorSensor.COLOR_WHITE:
RightWheel.on(SpeedDPS(230))
LeftWheel.on(SpeedDPS(80))
else:
LeftWheel.on(SpeedDPS(230))
RightWheel.on(SpeedDPS(80))
#print("LeftSensor - color:{0}", LeftSensor.color_name, file=sys.stderr)
AngleNew = 360 * LeftWheel.position / LeftWheel.count_per_rot
DegreeSum = DegreeSum + AngleNew - AngleOld
AngleOld = AngleNew
LeftWheel.off()
RightWheel.off()
def LineFollowing(FastSpeed, SlowSpeed, Degree):
DegreeSum = 0
AngleOld = 360 * RightWheel.position / RightWheel.count_per_rot
while DegreeSum < Degree:
if RightSensor.color == ColorSensor.COLOR_WHITE:
LeftWheel.on(SpeedDPS(FastSpeed))
RightWheel.on(SpeedDPS(SlowSpeed))
else:
RightWheel.on(SpeedDPS(FastSpeed))
LeftWheel.on(SpeedDPS(SlowSpeed))
AngleNew = 360 * RightWheel.position / RightWheel.count_per_rot
DegreeSum = DegreeSum + abs(AngleNew - AngleOld)
AngleOld = AngleNew
#print(RightSensor.reflected_light_intensity,LeftSensor.reflected_light_intensity,file=sys.stderr)
LeftWheel.off()
RightWheel.off()
def colorarm_corner(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_corner(%d)" % square_index)
position_target = -580
if square_index == 1:
position_target -= 10
elif square_index == 3:
position_target -= 30
elif square_index == 5:
position_target -= 20
elif square_index == 7:
pass
else:
raise ScanError(
"colorarm_corner was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'),
('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(
SpeedPercent(35).to_native_units(m), 35 / 100 * m.max_speed)
self.assertEqual(SpeedDPS(300).to_native_units(m), 300)
self.assertEqual(SpeedNativeUnits(300).to_native_units(m), 300)
self.assertEqual(SpeedDPM(30000).to_native_units(m), (30000 / 60))
self.assertEqual(SpeedRPS(2).to_native_units(m), 360 * 2)
self.assertEqual(SpeedRPM(100).to_native_units(m), (360 * 100 / 60))
self.assertEqual(DistanceMillimeters(42).mm, 42)
self.assertEqual(DistanceCentimeters(42).mm, 420)
self.assertEqual(DistanceDecimeters(42).mm, 4200)
self.assertEqual(DistanceMeters(42).mm, 42000)
self.assertAlmostEqual(DistanceInches(42).mm, 1066.8)
self.assertAlmostEqual(DistanceFeet(42).mm, 12801.6)
self.assertAlmostEqual(DistanceYards(42).mm, 38404.8)
self.assertEqual(DistanceStuds(42).mm, 336)
def colorarm_edge(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_edge(%d)" % square_index)
position_target = -640
if square_index == 2:
position_target -= 20
elif square_index == 4:
position_target -= 40
elif square_index == 6:
position_target -= 20
elif square_index == 8:
pass
else:
raise ScanError(
"colorarm_edge was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def flipper_away(self, speed=300):
"""
Move the flipper arm out of the way
"""
log.info("flipper_away()")
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed), 0)
def dti(self,
speed,
n,
startCounting=False,
sectionCache=0): # Drive to nth intersection
kp = 1.1
ki = 0
kd = 0
integral = 0
perror = error = 0
inters = 0
piderror = 0
while not self.btn.any(
): # Remember to try stuff twice, this is a keyboard interrupt
lv = self.LLight.reflected_light_intensity
rv = self.RLight.reflected_light_intensity
error = rv - lv
integral += integral + error
derivative = lv - perror
piderror = (error * kp) + (integral * ki) + (derivative * kd)
if speed + abs(piderror) > 100:
if piderror >= 0:
piderror = 100 - speed
else:
piderror = speed - 100
self.drive.on(left_speed=speed - piderror,
right_speed=speed + piderror)
sleep(0.01)
perror = error
# Drive up to nth intersection
# These values are subject to change depending on outside factors, CHANGE ON COMPETITION DAY
if (lv <= 50 and rv <= 55) or (lv <= 50 and rv >= 55) or (
lv >= 50 and rv <= 55): # Currently at an intersection
inters += 1
if (startCounting == True):
sectionCache += 1
if inters == n: # Currently at nth intersection
self.drive.off()
return
self.drive.off()
self.drive.on_for_seconds(SpeedDPS(115), SpeedDPS(115), 1)
print("Left Value: {}, Right Value: {}, P error: {}, Inters: {}".
format(lv, rv, piderror, inters),
file=sys.stderr)
def init_motors(self):
for x in (self.flipper, self.turntable, self.colorarm):
x.reset()
log.info("Initialize flipper %s" % self.flipper)
self.flipper.on(SpeedDPS(-50), block=True)
self.flipper.off()
self.flipper.reset()
log.info("Initialize colorarm %s" % self.colorarm)
self.colorarm.on(SpeedDPS(500), block=True)
self.colorarm.off()
self.colorarm.reset()
log.info("Initialize turntable %s" % self.turntable)
self.turntable.off()
self.turntable.reset()
def rotate_cube_blocked(self, direction, nb):
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# OVERROTATE depends on lot on MindCuber.rotate_speed
current_pos = self.turntable.position
OVERROTATE = 18
final_pos = int(135 * round(
(current_pos + (270 * direction * nb)) / 135.0))
temp_pos = int(final_pos + (OVERROTATE * direction))
log.info(
"rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s"
% (direction, nb, current_pos, temp_pos, final_pos))
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
temp_pos)
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
final_pos)
def flipper_hold_cube(self, speed=300):
current_position = self.flipper.position
# Push it forward so the cube is always in the same position
# when we start the flip
if (current_position <= MindCuber.hold_cube_pos - 10
or current_position >= MindCuber.hold_cube_pos + 10):
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed),
MindCuber.hold_cube_pos)
sleep(0.05)
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 test_tank_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
drive = MoveTank(OUTPUT_A, OUTPUT_B)
drive.on_for_rotations(SpeedDPS(400), SpeedDPM(10000), 10)
self.assertEqual(drive.left_motor.position, 0)
self.assertEqual(drive.left_motor.position_sp, 10 * 360)
self.assertEqual(drive.left_motor.speed_sp, 400)
self.assertEqual(drive.right_motor.position, 0)
self.assertAlmostEqual(drive.right_motor.position_sp, 10 * 360 * ((10000 / 60) / 400), delta=7)
self.assertAlmostEqual(drive.right_motor.speed_sp, 10000 / 60, delta=1)
def test_steering_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
drive = MoveSteering(OUTPUT_A, OUTPUT_B)
drive.on_for_rotations(25, SpeedDPS(400), 10)
self.assertEqual(drive.left_motor.position, 0)
self.assertEqual(drive.left_motor.position_sp, 10 * 360)
self.assertEqual(drive.left_motor.speed_sp, 400)
self.assertEqual(drive.right_motor.position, 0)
self.assertEqual(drive.right_motor.position_sp, 5 * 360)
self.assertEqual(drive.right_motor.speed_sp, 200)
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'),
('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(
SpeedPercent(35).to_native_units(m), 35 / 100 * m.max_speed)
self.assertEqual(SpeedDPS(300).to_native_units(m), 300)
self.assertEqual(SpeedNativeUnits(300).to_native_units(m), 300)
self.assertEqual(SpeedDPM(30000).to_native_units(m), (30000 / 60))
self.assertEqual(SpeedRPS(2).to_native_units(m), 360 * 2)
self.assertEqual(SpeedRPM(100).to_native_units(m), (360 * 100 / 60))
def rotate_cube(self, direction, nb):
current_pos = self.turntable.position
final_pos = 135 * round(
(self.turntable.position + (270 * direction * nb)) / 135.0)
log.info(
"rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" %
(direction, nb, current_pos, final_pos))
if self.flipper.position > 35:
self.flipper_away()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
final_pos)
if nb >= 1:
for i in range(nb):
if direction > 0:
transformation = [0, 1, 5, 2, 3, 4]
else:
transformation = [0, 1, 3, 4, 5, 2]
self.apply_transformation(transformation)
def large_motor():
lm = LargeMotor()
'''
This will run the large motor at 50% of its
rated maximum speed of 1050 deg/s.
50% x 1050 = 525 deg/s
'''
lm.on_for_seconds(50, seconds=3)
sleep(1)
'''
speed and seconds are both POSITIONAL
arguments which means
you don't have to include the parameter names as
long as you put the arguments in this order
(speed then seconds) so this is the same as
the previous command:
'''
lm.on_for_seconds(50, 3)
sleep(1)
'''
This will run at 500 degrees per second (DPS).
You should be able to hear that the motor runs a
little slower than before.
'''
lm.on_for_seconds(SpeedDPS(500), seconds=3)
sleep(1)
# 36000 degrees per minute (DPM) (rarely useful!)
lm.on_for_seconds(SpeedDPM(36000), seconds=3)
sleep(1)
# 2 rotations per second (RPS)
lm.on_for_seconds(SpeedRPS(2), seconds=3)
sleep(1)
# 100 rotations per minute(RPM)
lm.on_for_seconds(SpeedRPM(100), seconds=3)
def correction(self, dps=180):
"""
Do a correction for any deviation after travelling between squares
The implementation below assumes that we didn't deviate too far between black squares
This means we are still *roughly* orthogonal to the square
We also assume that the squares are perfectly square, to within other errors
The implementation is to find the angle left and right until we are off the square,
take the midpoint and correct this amount.
This should put us on a roughly equal but opposite deviation compared to the last trip, canceling the next problem
:param dps: The speed to turn in degrees per second
:return: None
"""
# Stop the old thread to start a new one with better parameters
self.black_square_sensor.stop_reading()
#Currently on a black square, move until on a white square
self.black_square_sensor.start_reading(count=2, init_val=0, interval=0.1, wait_time=0.2)
#While we are not back on the white keep turning
start = time.time()
self.tank.on(0, SpeedDPS(dps))
while not self.black_square_sensor.above_threshold():
continue
end = time.time()
left_angle = dps*(end-start)
#We now know angular deviation to left, reset by moving back
self.tank.on_for_degrees(0, SpeedDPS(-dps), left_angle)
#Do the same for the right angle
self.black_square_sensor.start_reading(count=2, init_val=0, interval=0.1, wait_time=0.2)
start = time.time()
self.tank.on(SpeedDPS(dps), 0)
while not self.black_square_sensor.above_threshold():
continue
end = time.time()
right_angle = dps * (end - start)
self.tank.on_for_degrees(SpeedDPS(-dps), 0, right_angle)
# Now we have both left and right angles, lets average then move to corrected bearing
angle_correction = (left_angle - right_angle) / 2
angle_correction = 0.65*(90/math.pi)*math.atan(math.radians(angle_correction))
self.tank.on_for_degrees(SpeedDPS(-dps), SpeedDPS(dps), angle_correction)
dt = 0.0
tachospeed = 0.0 # degrs Per Sec
ev3dev2_speed = 0
d_deg_avg = 0.0
tachospeed_avg = 0.0
start_pos = 0
end_pos = 0
start_time = 0.0
end_time = 0.0
m = LargeMotor(OUTPUT_A)
time.sleep(0.1)
m.reset()
time.sleep(0.1)
start_time = time.time()
m.on(SpeedDPS(800))
for i in range(1600):
t1 = time.time()
old_pos = encoder_pos
time.sleep(0.05)
encoder_pos = m.position
dt = time.time() - t1
d_deg = encoder_pos - old_pos
tachospeed = d_deg / dt
d_deg_avg = ((d_deg_avg + d_deg) / (2))
tachospeed_avg = ((tachospeed_avg + tachospeed) / (2))
if abs(d_deg) > 100:
print("d_deg = ", d_deg, "; d_deg_avg", d_deg_avg, "; tachospeed_avg",
tachospeed_avg)
print("speed_d = ", m.speed_d) # powerup default is 7500
start_time = time.time()
""" ### quick test
m.position = 0
print(m.position) """
for j in range(250, 351, 5):
m.speed_p = 15000
m.speed_i = j
m.speed_d = 7500
tachospeed_min = 999999.99
tachospeed_max = 0.0
start_pos = 0
start_time = time.time()
m.on(SpeedDPS(goal_DPS))
for i in range(100):
t1 = time.time()
old_pos = encoder_pos
time.sleep(0.05)
encoder_pos = m.position
dt = time.time() - t1
d_deg = encoder_pos - old_pos
tachospeed = d_deg / dt
d_deg_avg = ((d_deg_avg + d_deg) / (2))
tachospeed_avg = ((tachospeed_avg + tachospeed) / (2))
if tachospeed < tachospeed_min:
tachospeed_min = tachospeed
if tachospeed > tachospeed_max:
tachospeed_max = tachospeed
# if abs(d_deg) > 100:
#
import time
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, SpeedDPS, MoveTank
from ev3dev2.sensor import INPUT_1
print("Please attach a motor to BAM1")
time.sleep(3)
start_pos = 0
end_pos = 0
start_time = 0.0
end_time = 0.0
m = LargeMotor(OUTPUT_A)
time.sleep(0.1)
m.reset()
time.sleep(0.1)
start_time = time.time()
for i in range(1, 11):
m.on_to_position(SpeedDPS(600), (i * 360), brake=False,
block=True) ### this method FAILS TO BLOCK
# m.on_for_degrees(SpeedDPS(600), (i * 360), brake=False, block=True) ### this method FAILS TO BLOCK
# time.sleep(.1)
m.wait_while('running')
m.off(brake=True)
print("target pos = ", (i * 360), "; actual pos = ", m.position)
m.reset()
time.sleep(.1)
start_pos = 0
end_pos = 0
start_time = 0.0
end_time = 0.0
m = LargeMotor(OUTPUT_A)
time.sleep(0.1)
m.reset()
time.sleep(0.1)
start_time = time.time()
m.stop_action = 'brake'
# m.ramp_up_sp(5000) #### unsupported (on PiStorms only?)
# m._ramp_down_sp(5000) #### unsupported (on PiStorms only?)
m.on(SpeedDPS(0))
t1 = time.time()
for i in range(0, 910, 10):
# print("START --------> m.on...", round((time.time() - t1), 4))
print(i, " Increment SpeedDPS --------> m.on(SpeedDPS(i)) ",
round((time.time() - t1), 4))
m.on(SpeedDPS(i))
#m.on_to_position(SpeedDPS(100), (i * 360), brake=False, block=True) ### this method FAILS TO BLOCK
#m.run_to_rel_pos(position_sp=(i * 360), speed_sp=600, stop_action="hold") ### no claim of blocking
#m.on_for_degrees(SpeedDPS(900), (i * 360), brake=True, block=True) ### this method FAILS TO BLOCK
# m.on_for_seconds(speed=i, seconds=8, brake=False)
# print("END --------> m.on...")
# #time.sleep(2)
# print("stop_action is ", m.stop_action, round((time.time() - t1), 4))
# print("state is ", m.state, round((time.time() - t1), 4))
def moveToAngle(self, targetAngleRadians, dryrun=False):
'''
Description:meant to move link to position (in radians) at a certain fixed speed
param:targetAngleRadians:desc:target angle in radians
param:targetAngleRadians:type:float
# param:stop_action:desc:what to do when finished ('coast', 'brake', 'hold'
# choices of ev3dev2.motor.LargeMotor.STOP_ACTION_COAST
# ev3dev2.motor.LargeMotor.STOP_ACTION_BRAKE
# ev3dev2.motor.LargeMotor.STOP_ACTION_HOLD
param:stop_action:type:str
param:dryrun:desc:if true, won't run, if false, will do.
param:dryrun:type:bool
'''
LOGGER_LINK_MTA_INSTANCE = LOGGER_LINK_MTA.format(self.linkID)
try:
if targetAngleRadians < self.angleLimit[
0] or targetAngleRadians > self.angleLimit[1]:
raise OutOfBoundAngle(
'Setpoint does not fit within acceptable bounds',
targetAngle=targetAngleRadians)
with self.lock:
# 0. Check for urgent stop
if self.urgentStop:
raise MotorUrgentStopException(self.motor.description)
# 1. Estimate time to target:
currentAngleDegrees = self.armAngleDegX
targetAngleDegrees = degrees(targetAngleRadians)
eta = abs(targetAngleDegrees - currentAngleDegrees) / abs(
self.motorMinSpeedDPS) * self.gearRatio
# 2. Initialize variables
idx = 0
startTime = time.time()
initialMotorPosition = self.motor.position
maxLoopTimeFactor = 4
logEvery = 1
avgStepDuration = 0
if dryrun:
log.warning(
LOGGER_LINK_MTA_INSTANCE,
"Step {0} - Not moving motor {1} : dry run".format(
idx, self.motor.kwargs['address']))
return
angleError = abs(targetAngleDegrees - self.armAngleDegX)
thetaDotPrev = 0
while angleError > self.wiggleDeg:
loopStartTime = time.time()
# Check for silly and urgent conditions
if self.urgentStop:
raise MotorUrgentStopException(self.motor.description)
if self.armAngleRadX < self.angleLimit[0]:
log.critical(
LOGGER_LINK_MTA_INSTANCE,
f'Current angle {self.armAngleDegX} lower than angleLimit[0] {degrees(self.angleLimit[0])}'
)
raise OutOfBoundAngle(
'While moving arm link, we reached the lower angle limit.',
self.armAngleDegX)
if self.armAngleRadX > self.angleLimit[1]:
log.critical(
LOGGER_LINK_MTA_INSTANCE,
f'Current angle {self.armAngleDegX} higher than angleLimit[1] {degrees(self.angleLimit[0])}'
)
raise OutOfBoundAngle(
'While moving arm link, we reached the upper angle limit.',
self.armAngleDegX)
if loopStartTime - startTime > maxLoopTimeFactor * eta:
raise ControlerRunningTooLongException(
f'Took over {maxLoopTimeFactor} times original anticipated ETA (of {eta:.2f}s ==> {maxLoopTimeFactor} * {eta:.2f}s = {maxLoopTimeFactor*eta:.2f}s).'
)
if loopStartTime - startTime > 3 and self.motor.position == initialMotorPosition:
raise ControlerRunningTooLongException(
'After 3 seconds, the motor is still in the same initial position'
)
if abs(self.motor.speed - self.motor.max_dps) < 50:
raise MotorRunningFastException(self.motor.description)
# Calculate motor speed for step
delta = angleError
thetaDot = min(
((self.motorMaxSpeedDPS - self.motorMinSpeedDPS) /
self.maxDelta) * delta + self.motorMinSpeedDPS,
self.motorMaxSpeedDPS)
# Calculate direction of motor rotation
thetaDot = int(
-thetaDot
) if self.armAngleDegX > targetAngleDegrees else int(
thetaDot)
# Validate if motor speed needs to change
if thetaDotPrev != thetaDot:
log.debug(
LOGGER_LINK_MTA_INSTANCE,
f'Step {idx} - Updating motor {self.motor.address}\'s speed from {thetaDotPrev} to {thetaDot} dps.'
)
self.motor.on(SpeedDPS(thetaDot),
brake=False,
block=False)
# Note current angle error and last motor speed
thetaDotPrev = thetaDot
angleError = abs(targetAngleDegrees - self.armAngleDegX)
stepDuration = time.time() - loopStartTime
avgStepDuration += stepDuration
if idx % logEvery == 0:
avgStepDuration /= logEvery
infoLogString = f'Step {idx+1} - Avg. Loop : {avgStepDuration:.3f}s => Position/Target/Error = ' \
f'{self.armAngleDegX:.2f}/{targetAngleDegrees:.2f}/{angleError:.2f}'
log.debug(LOGGER_LINK_MTA_INSTANCE, infoLogString)
avgStepDuration = 0
idx += 1
# Stop the motor at this point - and hold position
log.debug(
LOGGER_LINK_MTA_INSTANCE,
f'Final position = {self.armAngleDegX:.2f} deg - set point is {targetAngleDegrees:.2f} deg'
)
except OutOfBoundAngle as error:
log.error(
LOGGER_LINK_MTA_INSTANCE,
f'Asked for an angle for this link out of bounds - asked : {error.targetAngle}, limits = [{degrees(self.angleLimit[0]):.2f}, {degrees(self.angleLimit[1]):.2f}]. Reason = {error.reason}'
)
except MotorRunningFastException as error:
log.error(
LOGGER_LINK_MTA_INSTANCE,
"Motor {} running too fast ({}) - exiting and stopping motor".
format(error.motorName, error.speed))
raise
except ControlerRunningTooLongException as error:
log.error(
LOGGER_LINK_MTA_INSTANCE,
"MoveToAngle routine running for too long. Something is likely wrong."
)
log.error(LOGGER_LINK_MTA_INSTANCE,
"Detailed Error Message = {}".format(error.reason))
except MotorUrgentStopException as error:
log.error(LOGGER_LINK_MTA_INSTANCE,
"Urgent Motor Stop called. {}".format(error.motorName))
self.resetLink()
except:
log.error(LOGGER_LINK_MTA_INSTANCE,
'Error : {}'.format(traceback.print_exc()))
raise
finally:
self.motor.stop(stop_action=self.stopAction)
return
#!/usr/bin/env python3 from ev3dev2.motor import MediumMotor, OUTPUT_A from ev3dev2.motor import SpeedDPS, SpeedRPS, SpeedRPM from time import sleep medium_motor = MediumMotor(OUTPUT_A) # We'll make the motor turn 180 degrees # at speed 50 (780 dps for the medium motor) medium_motor.on_for_degrees(speed=50, degrees=180) # then wait one second sleep(1) # then – 180 degrees at 500 dps medium_motor.on_for_degrees(speed=SpeedDPS(500), degrees=-180) sleep(1) # then 0.5 rotations at speed 50 (780 dps) medium_motor.on_for_rotations(speed=50, rotations=0.5) sleep(1) # then – 0.5 rotations at 1 rotation per second (360 dps) medium_motor.on_for_rotations(speed=SpeedRPS(1), rotations=-0.5) sleep(1) medium_motor.on_for_seconds(speed=SpeedRPM(60), seconds=5) sleep(1) medium_motor.on(speed=60) sleep(2) medium_motor.off()
