diff options
Diffstat (limited to 'robot/problems/introduction/mindstorms_widgets_OLD.py')
| -rw-r--r-- | robot/problems/introduction/mindstorms_widgets_OLD.py | 398 |
1 files changed, 398 insertions, 0 deletions
diff --git a/robot/problems/introduction/mindstorms_widgets_OLD.py b/robot/problems/introduction/mindstorms_widgets_OLD.py new file mode 100644 index 0000000..4151133 --- /dev/null +++ b/robot/problems/introduction/mindstorms_widgets_OLD.py @@ -0,0 +1,398 @@ +#!/usr/bin/python + +import sys, time +from ev3dev import * +# The mindstorms_widgets class is a wrapper for the python-ev3dev +# language binding. It implements interfaces that correspond directly +# to the action and sensor programming blocks (widgets) from the +# mindstorms LabVIEW software. This allows leverage from the amazing +# documentation already provided by Lego. Some exceptions are: +# * It doesn't auto-detect the devices. You need to call +# connect_motor or connect_sensor first for each device you use +# * Only two large motors and one medium motor are supported. +# This simplifies the interfaces. The motors are referenced by +# the following names in the interfaces: +# 'medium' motor, default Port A +# 'right' large motor, default Port B +# 'left' large motor, default Port C +# * degrees and rotations inputs must always be positive +# use negative power to go backward +# * Rather than having a separate unregulated_motor widget, regulated is an +# an optional input parameter (default = True) +# * motor_rotation_measure, 'current_power' returns both the duty_cycle_sp +# (the actual power level being applied to the motor) and speed_sp +# * sound includes an option to give a sentence and have the robot speak + +# Known issues/limitations +# * The degrees, rotations, speed and invert values provided as inputs do +# not map directly to the values programmed to the sysfs attributes i.e. +# if you don't use mindstorm_widgets methods exclusively to control the +# motors you will need to explicitly initialize all the relevant sysfs +# attributes each time you program the motor from outside mindstorms_widgets +# * led_flash +# * no error handling/recovery +# * volume for tones doesn't work very well (seems to have only 2 levels) +# * when reading the heading and proximity in beacon mode, ir_sensor_measure +# returns good values at first but continuing to poll it seems to result +# in inconsistent values +# * ultrasonic and gyro sensors are not implemented + +# Mindstorms uses the following coefficients to compute speed_sp +# based on a power input in range 0-100 i.e. speed_sp = x * power +# medium motor: x = 16.4 +# large motor : x = 10.3 +# e.g. power of 10 sets medium motor speed_sp to 160 (deg/sec) + +MEDIUM_MOTOR_SPEED_COEFFICIENT = 16.4 +LARGE_MOTOR_SPEED_COEFFICIENT = 10.3 + +class mindstorms_widgets: + def __init__(self): + self.motor = { + 'medium':None, + 'left' :None, + 'right' :None + } + + def _set_motor_attribs( self, size, motors, powers, + invert, regulated, brake_at_end ): + # For regulated speed, speed_sp is tacho counts per + # second which for lego EV3 motors is also degrees per second. + # To provide a mindstorms 0-100 power value for regulated + # speed we need to convert it. + speed_x = LARGE_MOTOR_SPEED_COEFFICIENT + if size == 'medium': + speed_x = MEDIUM_MOTOR_SPEED_COEFFICIENT + + for m,p in zip ( motors, powers ): + # When using run_forever() the sign of speed_sp or duty_cycle_sp + # determines the direction the motor runs. When using + # run_to_rel_pos() the sign of position_sp determines the + # direction and the sign of speed_sp/duty_cycle_sp is ignored by + # the driver. + + # In either case the polarity reverses the direction. + + # When using move_steering, providing a negative power should + # result in reversing along the same path of a positive power + # in the same direction. If the steering is sharp the power of + # one motor is opposite sign of the other. + + # This all combines to make for overly complicated logic when the + # intent is to set motor attribs independent of knowing what + # methods are being used. To avoid this complexity I devised the + # following scheme: + # * Use the motor polarity as the common attribute to determine + # direction since it works the same for run_forever() and + # run_to_rel_pos() + # * Always program duty_cycle_sp and speed_sp to positive values + # * Always program position_sp to a positive value + pol = ['normal','inversed'] + brk = ['coast', 'brake'] + inv = invert + if p < 0: + inv = not invert + p = abs( p ) + if m: + m.polarity = pol[int( inv )] #e.g. True=1='inversed' + m.stop_command = brk[int( brake_at_end )] + #print ('speed: '+str(p)+' inv: '+str(inv)) + if regulated: + m.speed_regulation_enabled = 'on' + m.speed_sp = int( p*speed_x ) + else: + m.speed_regulation_enabled = 'off' + m.duty_cycle_sp = p + + def _move_wait( self ): + # wait for any running motors to finish their movement then stop + # all the motors. + left_is_off = False + right_is_off = False + if 'running' not in self.motor['left'].state: + left_is_off = True + if 'running' not in self.motor['right'].state: + right_is_off = True + while ((left_is_off or 'running' in self.motor['left'].state) and + (right_is_off or 'running' in self.motor['right'].state)): + time.sleep(0.1) + self.motor['left'].stop() + self.motor['right'].stop() + + def _move( self, mode, seconds, degrees, rotations ): + if mode == 'off': + self.motor['left'].stop() + self.motor['right'].stop() + elif mode == 'on': + self.motor['left'].run_forever() + self.motor['right'].run_forever() + elif mode == 'on_for_seconds': + self.motor['left'].run_forever() + self.motor['right'].run_forever() + time.sleep(seconds) + self.motor['left'].stop() + self.motor['right'].stop() + elif mode == 'on_for_degrees': + self.motor['left'].run_to_rel_pos(position_sp=degrees) + self.motor['right'].run_to_rel_pos(position_sp=degrees) + self._move_wait() + elif mode == 'on_for_rotations': + self.motor['left'].run_to_rel_pos( position_sp=int(rotations*360) ) + self.motor['right'].run_to_rel_pos( position_sp=int(rotations*360) ) + self._move_wait() + + def _validate_inputs( self, degrees, rotations ): + if (degrees < 0 or rotations < 0): + print( 'degrees and rotations should be positive. ' + 'Use negative power to go backward' ) + + def _run_motor( self, motor, mode, power, + seconds, degrees, rotations, + invert, regulated, brake_at_end ): + self._validate_inputs( degrees, rotations ) + self._set_motor_attribs( 'medium', [motor], [power], invert, regulated, + brake_at_end ) + if mode == 'off': + motor.stop() + elif mode == 'on': + motor.run_forever() + elif mode == 'on_for_seconds': + motor.run_forever() + time.sleep(seconds) + motor.stop() + elif mode == 'on_for_degrees': + motor.run_to_rel_pos(position_sp=degrees) + while 'running' in motor.state: + time.sleep(0.1) + elif motor and mode == 'on_for_rotations': + motor.run_to_rel_pos(position_sp=int(rotations*360)) + while 'running' in motor.state: + time.sleep(0.1) + + def _led_pulse( self, color ): + # Note that calling flash() multiple times on a LED causes an error + # Disable this routine for now + return + + if color == 'green': + led.green_left.flash( 200, 200 ) + led.green_right.flash( 200, 200 ) + elif color == 'orange': + led.green_left.flash( 200, 200 ) + led.red_left.flash( 200, 200 ) + led.green_right.flash( 200, 200 ) + led.red_right.flash( 200, 200 ) + elif color == 'red': + led.red_left.flash( 200, 200 ) + led.red_right.flash( 200, 200 ) + + + # Begin public methods + + def connect_motor( self, motor, port=None ): + default_port = { + 'medium':'A', + 'left' :'B', + 'right' :'C' + } + + port_enum = { + 'A' :OUTPUT_A, + 'B' :OUTPUT_B, + 'C' :OUTPUT_C, + 'D' :OUTPUT_D + } + if not port: + port = default_port[motor] + port = port.upper() + + if motor == 'medium': + self.motor['medium'] = medium_motor(port_enum[port]) + if not self.motor['medium'].connected: + print("Medium motor not connected to port " + port) + elif motor == 'right': + self.motor['right'] = large_motor(port_enum[port]) + if not self.motor['right'].connected: + print("Large right motor not connected to port " + port) + elif motor == 'left': + self.motor['left'] = large_motor(port_enum[port]) + if not self.motor['left'].connected: + print("Large left motor not connected to port " + port) + + def connect_sensor( self, sensor ): + if sensor == 'color': + self.cs = color_sensor() + if not self.cs.connected: + print("Color sensor not connected") + elif sensor == 'touch': + self.ts = touch_sensor() + if not self.ts.connected: + print("Touch sensor not connected") + elif sensor == 'infrared': + self.irs = infrared_sensor() + if not self.irs.connected: + print("Infrared sensor not connected") + elif sensor == 'gyro': + self.gs = gyro_sensor() + if not self.gs.connected: + print("Gyro sensor not connected") + elif sensor == 'ultrasonic': + self.us = ultrasonic_sensor() + if not self.us.connected: + print("Ultrasonic sensor not connected") + + + def large_motor( self, motor, mode, power=50, + seconds=0, degrees=0, rotations=0, + invert=False, regulated=True, brake_at_end=True ): + self._run_motor( self.motor[motor], mode, power, + seconds, degrees, rotations, + invert, regulated, brake_at_end ) + + def medium_motor( self, mode, power=50, + seconds=0, degrees=0, rotations=0, + invert=False, regulated=True, brake_at_end=True ): + self._run_motor( self.motor['medium'], mode, power, + seconds, degrees, rotations, + invert, regulated, brake_at_end ) + + def move_steering( self, mode, direction=0, power=50, + seconds=0, degrees=0, rotations=0, + invert=False, regulated=True, brake_at_end=True ): + ''' + direction [-100, 100]: + * -100 = pivot left i.e. right motor = power + * left motor = -power + * -50 = sharp left i.e. right motor = power + * left motor = 0 + * -25 = turn left i.e. right motor = power + * left motor = 1/2 power + * 0 = straight + * 25 = turn right + * 50 = sharp right + * 100 = pivot right + ''' + self._validate_inputs( degrees, rotations ) + powers = steering( direction, power ) + self._set_motor_attribs( 'large', + [self.motor['left'], self.motor['right']], + powers, invert, regulated, brake_at_end ) + self._move( mode, seconds, degrees, rotations ) + + def move_tank( self, mode, direction=0, lr_power=None, + seconds=0, degrees=0, rotations=0, + invert=False, regulated=True, brake_at_end=True ): + self._validate_inputs( degrees, rotations ) + if not lr_power: + lr_power=[50,50] + self._set_motor_attribs( 'large', + [self.motor['left'], self.motor['right']], + lr_power, invert, regulated, brake_at_end ) + self._move( mode, seconds, degrees, rotations ) + + def brick_status_light( self, mode, color='green', pulse=False ): + led.all_off() + if mode == 'on': + if color == 'green': + led.green_on() + elif color == 'orange': + led.all_on() + elif color == 'red': + led.red_on() + if mode == 'on' and pulse: + self._led_pulse( color ) + pass + # the sensor should be perfectly still while resetting the gyro + def resetGyro(self): + self.gs.mode = 'GYRO-RATE' + self.gs.mode = 'GYRO-ANG' + + def gyro_sensor_measure( self, mode ): + if mode == 'angle': + self.gs.mode = 'GYRO-ANG' # Angle (degrees), value: (-32768 to 32767) + elif mode == 'rate': + self.gs.mode = 'GYRO-RATE' + return self.gs.value() + + def ultrasonic_sensor_measure( self, mode ): + if mode == 'distance-cm': + self.gs.mode = 'US-DIST-CM' # Continuous measurement of distance (0-2550 mm) + return self.us.value() + + def color_sensor_measure( self, mode ): + if mode == 'color': + self.cs.mode = 'COL-COLOR' + elif mode == 'reflected_light_intensity': + self.cs.mode = 'COL-REFLECT' + elif mode == 'ambient_light_intensity': + self.cs.mode = 'COL-AMBIENT' + return self.cs.value() + + def ir_sensor_measure( self, mode, channel=1 ): + # value(n) returns the given value. See + # http://www.ev3dev.org/docs/sensors/lego-ev3-infrared-sensor/ + i = 2*(channel-1) + if mode == 'proximity': + self.irs.mode = 'IR-PROX' + return self.irs.value() + elif mode == 'beacon': + self.irs.mode = 'IR-SEEK' + # (heading, proximity, detected) + # heading in range (-25,25) + # proximity is -128 or in [0,100] + prox = self.irs.value(i+1) + return( self.irs.value(i), prox, prox != -128 ) + elif mode == 'remote': + self.irs.mode = 'IR-REMOTE' + ''' + 0 = No button (and Beacon Mode is off) + 1 = Button 1 + 2 = Button 2 + 3 = Button 3 + 4 = Button 4 + 5 = Both Button 1 and Button 3 + 6 = Both Button 1 and Button 4 + 7 = Both Button 2 and Button 3 + 8 = Both Button 2 and Button 4 + 9 = Beacon Mode is on + 10 = Both Button 1 and Button 2 + 11 = Both Button 3 and Button 4 + ''' + return self.irs.value( channel-1 ) + + + def motor_rotation_measure( self, motor, mode ): + motor = self.motor[motor] + if mode == 'reset': + motor.position = 0 + return None + if mode == 'current_power': + return (motor.speed, motor.duty_cycle) + degrees=motor.position + if mode == 'degrees': + return degrees + elif mode == 'rotations': + return degrees/360 + + def touch_sensor_measure( self ): + return self.ts.value() + + def sound( self, mode, volume=40, hz=523, path=None, + sentence=None, duration_ms=200 ): + # todo:Volume doesn't seem to work very well for tones. + # 0=just a click + # 1-49 same loud volume tone + # > 50 same volume really loud tone + sound.volume = volume + if mode == 'play_file': + # Sound.rsf files can be extracted from mindstorms by using the + # sound in a mindstorms program then going to mindstorms + # project->sounds tab and choosing "export" + + sound.play( path ) + elif mode == 'play_tone': + sound.tone( int(hz), duration_ms ) + elif mode == 'speak': + sound.speak( sentence, True ) + + |