def __init__(self, client, display):
self.minVoltage = PowerSupply().min_voltage
self.maxVoltage = PowerSupply().max_voltage
self.medVoltage = self.maxVoltage * 0.5
self.exitVoltage = self.minVoltage + (self.minVoltage * 0.05)
self.action = Actions()
self.client = client
self.display = display
print("Starting battery thread")
# Execute battery thread
batteryThread = Thread(target=self.chargeStatus, args=())
batteryThread.start()
def __init__(self, speed):
self.minVoltage = PowerSupply().min_voltage
self.maxVoltage = PowerSupply().max_voltage
self.medVoltage = self.maxVoltage * 0.5
self.exitVoltage = self.minVoltage + (self.minVoltage * 0.05)
self.action = Actions(speed)
# Get current time HH:MM:SS
now = datetime.now()
time = now.strftime("%H:%M:%S")
print(str(time) + "\tExecuting Battery Thread\n")
# Execute battery thread
batteryThread = Thread(target=self.chargeStatus, args=())
batteryThread.start()
def battery_check():
'''Warns us of low battery with a beep and a sound msg.'''
voltage = PowerSupply().measured_volts
if voltage < LOW_BATTERY_VOLTAGE_THRESHOLD:
play_beep(400.0, 2.0) # Hz; seconds
Sound().speak('Warning! Battery running low!')
elif voltage < MIDDLE_VOLTAGE_THRESHOLD:
play_beep(length=2.0)
Sound().speak('Battery at midway point.')
print(voltage)
def __init__(self):
self.exit = True
self.callback_exit = True
# Connect sensors and buttons.
self.btn = Button()
self.ir = InfraredSensor()
self.ts = TouchSensor()
self.power = PowerSupply()
self.tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
print('EV3 Node init starting')
rospy.init_node('ev3_robot', anonymous=True, log_level=rospy.DEBUG)
print('EV3 Node init complete')
rospy.Subscriber('ev3/active_mode', String, self.active_mode_callback, queue_size=1)
self.power_init()
print('READY!')
#!/usr/bin/env python3
#
# template for ev3dev2-based code
#
from ev3dev2.power import PowerSupply
import time
b1=PowerSupply()
while(True):
print("measured_voltage: ", b1.measured_voltage)
print("measured_volts: ", b1.measured_volts)
#print("measured_current: ", b1.measured_current) ### (not supported on Pistorms?)
#print("measured_amps: ", b1.measured_amps) ### (not supported on Pistorms?)
#print("technology: ", b1.technology) ### (not supported on Pistorms?)
print("type: ", b1.type)
time.sleep(15)
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):
print(message)
def getCurrentVoltage(self):
return PowerSupply().measured_voltage * 10
def __init__(self, sonar, sensor_color):
self._sonar = UltrasonicSensor(sonar)
self._color = ColorSensor(sensor_color)
self._bateria = PowerSupply()
def __init__(self,
gain_gyro_angle=1700,
gain_gyro_rate=120,
gain_motor_angle=7,
gain_motor_angular_speed=9,
gain_motor_angle_error_accumulated=3,
power_voltage_nominal=8.0,
pwr_friction_offset_nom=3,
timing_loop_msec=30,
motor_angle_history_length=5,
gyro_drift_compensation_factor=0.05,
left_motor_port=OUTPUT_D,
right_motor_port=OUTPUT_A,
debug=False):
"""Create GyroBalancer."""
# Gain parameters
self.gain_gyro_angle = gain_gyro_angle
self.gain_gyro_rate = gain_gyro_rate
self.gain_motor_angle = gain_motor_angle
self.gain_motor_angular_speed = gain_motor_angular_speed
self.gain_motor_angle_error_accumulated =\
gain_motor_angle_error_accumulated
# Power parameters
self.power_voltage_nominal = power_voltage_nominal
self.pwr_friction_offset_nom = pwr_friction_offset_nom
# Timing parameters
self.timing_loop_msec = timing_loop_msec
self.motor_angle_history_length = motor_angle_history_length
self.gyro_drift_compensation_factor = gyro_drift_compensation_factor
# Power supply setup
self.power_supply = PowerSupply()
# Gyro Sensor setup
self.gyro = GyroSensor()
#self.gyro.mode = self.gyro.MODE_GYRO_RATE
self.gyro.mode = self.gyro.MODE_GYRO_G_A
# Touch Sensor setup
self.touch = TouchSensor()
# IR Buttons setup
# self.remote = InfraredSensor()
# self.remote.mode = self.remote.MODE_IR_REMOTE
# Configure the motors
self.motor_left = LargeMotor(left_motor_port)
self.motor_right = LargeMotor(right_motor_port)
# Sound setup
self.sound = Sound()
# Open sensor and motor files
self.gyro_angle_file = open(self.gyro._path + "/value0", "rb")
self.gyro_rate_file = open(self.gyro._path + "/value1", "rb")
self.touch_file = open(self.touch._path + "/value0", "rb")
self.encoder_left_file = open(self.motor_left._path + "/position",
"rb")
self.encoder_right_file = open(self.motor_right._path + "/position",
"rb")
self.dc_left_file = open(self.motor_left._path + "/duty_cycle_sp", "w")
self.dc_right_file = open(self.motor_right._path + "/duty_cycle_sp",
"w")
# Drive queue
self.drive_queue = queue.Queue()
# Stop event for balance thread
self.stop_balance = threading.Event()
# Debugging
self.debug = debug
# Handlers for SIGINT and SIGTERM
signal.signal(signal.SIGINT, self.signal_int_handler)
signal.signal(signal.SIGTERM, self.signal_term_handler)
def __init__(self,
robot_config_file,
initRobotConfig,
timeStep,
odometryTimer=0.1,
defaultLogging=30):
"""
Creates the main bot class.
"""
if not os.path.exists(robot_config_file):
raise ValueError(f'Cannot find configuration file '
f'"{robot_config_file}"')
with open(robot_config_file, 'r') as f:
self.robotConfiguration = json.load(f)
# Adjust JSON just read
self.__adjustConfigDict(self.robotConfiguration)
# Exception Queue for inter threads exception communications
self.exceptionQueue = queue.Queue()
self.mqttMoveQueue = queue.Queue()
self.baseMovementLock = asyncio.locks.Lock()
self.timeStep = timeStep
self.powerSupply = PowerSupply()
# region sensor Init
self.sensors = {}
for sensor in self.robotConfiguration["sensors"]:
sensorDict = self.robotConfiguration["sensors"][sensor]
legoPort = LegoPort(sensorDict["input"])
# Applies to both I2C and SDK controled sensors
if legoPort.mode != sensorDict["mode"]:
legoPort.mode = sensorDict["mode"]
# Only applies to sensors controled by the python SDK
if "set_device" in sensorDict:
legoPort.set_device = sensorDict["set_device"]
time.sleep(1)
address = sensorDict["input"]
self.sensors[sensor] = sensorDict["sensorType"](address=address)
# endregion
# base Init
self.base = DiffDriveBase(self.robotConfiguration["base"],
initRobotConfig, odometryTimer)
# arm/endeffector Init
self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"])
# Other matrix init
self.M0e = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"]) # noqa F501
self.Toe_grip = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"]) # noqa F501
self.Tbc = mr.RpToTrans(
R=np.array(
self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"])
self.Tcb = mr.TransInv(self.Tbc)
# Reset all motors at init
self.urgentStop = False
log.info(LOGGER_DDR_MAIN, 'Done main robot init.')
