[], dtype=np.int32) # list to hold 'r' - IR distance r (in arb units)
polarCoordsIRmean = np.array(
[], dtype=np.int32) # list to hold processed data IR-mean
polarCoordsIRstd = np.array(
[], dtype=np.int32) # list to hold processed data IR-stdev
# setup motors
mL = LargeMotor(OUTPUT_A)
time.sleep(0.5)
mL.reset()
time.sleep(2)
mLspd = 0
mL.stop_action = 'coast'
# mL.polarity = 'inversed'
mL.position = 0
mR = LargeMotor(OUTPUT_B)
time.sleep(0.5)
mR.reset()
time.sleep(2)
mRspd = 0
mR.stop_action = 'coast'
# mR.polarity = 'inversed'
mR.position = 0
spd = 0 # set this value to -30 to +90; use to set outer wheel/motor speed; default to mL for driving straight
turnRatio = 0.0 # set this value to -0.5 to +0.5; subtract from 1 to set turn ratio; multiply * spd to set inside motor speed
def keyboardInput(name):
import time, tty, sys
from ev3dev2.motor import LargeMotor, OUTPUT_D, SpeedPercent, SpeedDPS, MoveTank
from ev3dev2.sensor import INPUT_1
tty.setcbreak(sys.stdin)
mShuttle = LargeMotor(OUTPUT_D)
time.sleep(0.5)
mShuttle.reset()
time.sleep(2)
mShuttle.duty_cycle_sp = 0
mShuttleDtyMax = 22
mShuttle.stop_action = 'coast'
mShuttle.polarity = 'inversed'
mShuttle.position = 0
prev_mShuttle_pos = 0
# Turn motor on with run_direct and duty_cycle_sp of '0'
mShuttle.run_direct()
print ("Ready for keyboard commands...")
while (True):
x = ord(sys.stdin.read(1))
if x == 120: # x key pushed - exit
mShuttle.duty_cycle_sp = 0
mShuttle.off(brake=False)
break
if x == 32: # space key pushed
tty.setcbreak(sys.stdin)
mL = LargeMotor(OUTPUT_A)
time.sleep(0.5)
mL.reset(
) # avoid using; on pistorms problem when 'reset' is followed by motot 'on' at low speed
################ 'reset' and 'position = 0' result in pos of 0, and motor controller behaves poorly
################ when pos != 226 when starting the motor
time.sleep(2)
mLspd = 0
mL.stop_action = 'coast'
# mL.position = 226 ### FAILED work around for pistorms 'reset' followed by motot 'on' at low speed issue
####################### ### FAIL; can't write anything but '0' to position; the problem is at the driver level
mL.position = 0 ### avoid using on pistorms; motor controller behaves poorly when pos != 226 when starting the motor
mR = LargeMotor(OUTPUT_B)
time.sleep(0.5)
mR.reset()
time.sleep(2)
mRspd = 0
mR.stop_action = 'coast'
mR.position = 0
print("Ready for keyboard commands...")
while (doExit == False):
x = ord(sys.stdin.read(1))
if x == 120: # x key pushed - exit
mLspd = 0
leds = Leds() remote_leds = remote_led.Leds() sound = Sound() waist_motor = LargeMotor(OUTPUT_A) shoulder_control1 = LargeMotor(OUTPUT_B) shoulder_control2 = LargeMotor(OUTPUT_C) shoulder_motor = MoveTank(OUTPUT_B,OUTPUT_C) elbow_motor = LargeMotor(OUTPUT_D) roll_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_A) pitch_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_B) spin_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_C) grabber_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_D) waist_motor.position = 0 shoulder_control1.position = 0 shoulder_control2.position = 0 shoulder_motor.position = 0 elbow_motor.position = 0 roll_motor.position = 0 pitch_motor.position = 0 spin_motor.position = 0 grabber_motor.position = 0 waist_ratio = 7.5 shoulder_ratio = 7.5 elbow_ratio = 5 roll_ratio = 7 pitch_ratio = 5 spin_ratio = 7
def main():
Sound.speak("").wait()
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("outD")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
MB.position = 0
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
# change this to whatever speed what you want. V
left_wheel_speed = -300
right_wheel_speed = -300
# if Gyro value is the same as the starting value, go straigt. if more turn right. if less turn left. V
# FIX THIS VALUE!!!!!!!!!! V
while MB.position > -0.2:
if GY.value() != 0:
if GY.value() > starting_value:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate:
gyro_adjust = gyro_adjust + 1 # If gyro value has worsened despite the correction then change the adjust variable for next time
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate:
gyro_adjust = gyro_adjust + 1
else:
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 1
# wait for the block to drop. V
sleep(5)
# pulling away from the crane. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 0.25)
# Unlocking attachment. V
MD.on_for_degrees(SpeedPercent(50), 360)
# pulling away from unlocked attachment. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1)
# gyro 90 degree spin turn
while GY.value() < 91:
MB.run_forever(speed_sp=300)
MC.run_forever(speed_sp=-300)
# drive into home
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 4)
# some keyboard control setup and vars
tty.setcbreak(sys.stdin)
x = 0 # set here to make global
# setup motor
mL = LargeMotor(OUTPUT_A)
time.sleep(0.5)
mL.reset()
time.sleep(2)
mL.duty_cycle_sp = 0
mLDtyMax = 16
mL.stop_action = 'coast'
mL.polarity = 'inversed'
mL.position = 0
StallDetected = False
mLlastPos = 0
i = 0
# Turn motor on with run_direct and duty_cycle_sp of '0'
mL.run_direct()
def keyboardInput(name):
while (True):
global x # Declare x as global to force use of global 'x' in this function/thread
x = ord(sys.stdin.read(1))
time.sleep(0.05)
import importlib
tts_module = importlib.import_module('tts.ev3')
STUD_MM = 8
# TODO: Add code here
speed = 20
sound = Sound()
tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
steering_drive = MoveSteering(OUTPUT_A, OUTPUT_B)
mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3EducationSetRim, 16 * STUD_MM)
btn = Button()
arm = MediumMotor()
lightswitch = LargeMotor(OUTPUT_D)
lightswitch.position = 0
arm.position = 0
supply = PowerSupply()
mdiff.odometry_start()
scriptname = "/hardware/ev3.py"
debug = True
stationary_mode = False
movements = []
# drive in a turn for 5 rotations of the outer motor
# the first two parameters can be unit classes or percentages.
def debug_log(message):
global debug
if (debug):
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()
m.stop_action = 'coast'
m.stop()
m.position = 0
time.sleep(0.1)
start_time = time.time()
#m.on(SpeedDPS(800))
while True:
for i in range(6000):
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))
import time, tty, sys from ev3dev2.motor import LargeMotor, OUTPUT_C, OUTPUT_D, SpeedPercent, SpeedDPS, MoveTank from ev3dev2.sensor import INPUT_1 tty.setcbreak(sys.stdin) mLift = LargeMotor(OUTPUT_C) time.sleep(0.5) mLift.reset() time.sleep(2) mLift.duty_cycle_sp = 0 mLiftDtyMax = 36 mLift.stop_action = 'coast' mLift.polarity = 'inversed' mLift.position = 0 mGrab = LargeMotor(OUTPUT_D) time.sleep(0.5) mGrab.reset() time.sleep(2) mGrab.duty_cycle_sp = 0 mGrabDtyMax = 36 mGrab.stop_action = 'coast' mGrab.polarity = 'inversed' mGrab.position = 0 # Turn motors on with run_direct and duty_cycle_sp of '0' mLift.run_direct() mGrab.run_direct()
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, MoveSteering, OUTPUT_B, OUTPUT_C, SpeedDPS
from ev3dev2.sensor.lego import InfraredSensor
from sys import stderr
from time import sleep
motorB = LargeMotor(OUTPUT_B)
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
ir = InfraredSensor()
wf = 1
motorB.position = 0
steer_pair.on(steering=0, speed=SpeedDPS(265))
while motorB.position * 0.377 < 370:
if ir.proximity * 0.7 < 16.5:
motorB.position = 0
print(motorB.position * 0.377, ir.proximity * 0.7, file=stderr)
sleep(0.1)
steer = 28
rots = -1.8
steer_pair.on_for_rotations(steering=0, speed=25, rotations=0.5 * wf)
steer_pair.on_for_rotations(steering=steer, speed=15, rotations=rots * wf)
steer_pair.on_for_rotations(steering=-steer, speed=15, rotations=rots * wf)
steer_pair.on_for_rotations(steering=0, speed=25, rotations=0.7 * wf)
