def init_pi(self):
from gpiozero import Button, LED, PWMOutputDevice
from tachometer import Tachometer
# Input
self.pi_btn_left = Button(IN_KEY_LEFT)
self.pi_btn_right = Button(IN_KEY_RIGHT)
self.pi_btn_enter = Button(IN_KEY_ENTER)
# Output
self.pi_revr = LED(OUT_REVERSE)
self.pi_spdr = LED(OUT_SPD)
self.pi_pwm = PWMOutputDevice(OUT_THROTTLE)
self.pi_btn_left.when_pressed = self.btn_left_press
self.pi_btn_left.when_released = self.btn_left_unpress
self.pi_btn_right.when_pressed = self.btn_right_press
self.pi_btn_right.when_released = self.btn_right_unpress
self.pi_btn_enter.when_pressed = self.btn_enter_press
self.pi_btn_enter.when_released = self.btn_enter_unpress
# Tachometer
self.pi_tacho = Tachometer()
self.stop()
def __init__(self, winch: Winch):
logger.debug("IO : Initialize Hardware...")
super().__init__(winch)
# Power
self.__power_cmd = OutputDevice(OUT_PWR)
self.__power_cmd.off()
# Reverse
self.__reverse_cmd = OutputDevice(OUT_REVERSE)
# Speed mode (Lo, Medium, Hi)
self.__speed_cmd = OutputDevice(OUT_SPD)
# Throlle
self.__throttle_cmd = PWMOutputDevice(OUT_THROTTLE)
# Move
self.__key_enter_btn = Button(IN_KEY_ENTER)
self.__key_left_btn = Button(IN_KEY_LEFT)
self.__key_right_btn = Button(IN_KEY_RIGHT)
# Register event
self.__key_enter_btn.when_pressed = self.__pressedEnter
self.__key_left_btn = self.__pressedLeft
self.__key_right_btn = self.__pressedRight
def __init__ (self, leftPin, rightPin, simul = False):
''' Construtor da classe. Atribui pinos na GPIO. '''
if simul:
self.pLeft = PWMOutputDevice(leftPin)
self.pRight = PWMOutputDevice(rightPin)
self.tFeed = 0.5 # Avanço do motor
self.t_on = 0.05
self.power = 0.5 # Potencia do motor
def __init__(self, in1, in2, pwm):
self.in1 = DigitalOutputDevice(in1)
self.in1.off()
self.in2 = DigitalOutputDevice(in2)
self.in2.on()
self.pwm = PWMOutputDevice(pwm, frequency=1000)
def Pins(W = 19,X = 26,Y = 6,Z = 13):
global Mr1
global Mr2
global Ml1
global Ml2
Mr1 = PWMOutputDevice(W)
Mr2 = PWMOutputDevice(X)
Ml1 = PWMOutputDevice(Y)
Ml2 = PWMOutputDevice(Z)
def __init__(self, pin_fig_in1, pin_fig_in2, pin_fig_pwm,
frequency=None):
self.in1 = DigitalOutputDevice(pin=pin_fig_in1)
self.in2 = DigitalOutputDevice(pin=pin_fig_in2)
if frequency is None: # Not PWM mode
self.pwm = DigitalOutputDevice(pin=pin_fig_pwm)
else: # PWM mode
self.pwm = PWMOutputDevice(pin=pin_fig_pwm,
frequency=frequency)
def pwm_on(self, anode, cathode, fade_in, fade_out, n):
self.last_off()
self.anode = PWMOutputDevice(anode)
self.cathode = OutputDevice(cathode)
self.anode.pulse(fade_in_time=fade_in,
fade_out_time=fade_out,
n=n,
background=False)
self.cathode.off()
def __init__(self):
self.motor0 = PWMOutputDevice(6)
self.motor1 = PWMOutputDevice(24)
self.motor2 = PWMOutputDevice(19)
self.motor3 = PWMOutputDevice(26)
self.vibracao_total = 0.9
self.vibracao_media = 0.5
self.obstaculo_proximo = 0.2 # ~0.5 metros
self.distancia_media = 0.5 # ~2 metros
self.distancia_longa = 0.7 # ~3 metros
def __init__(self, pins):
fwdpin = pins[0]
revpin = pins[1]
pwmpin = pins[2]
self.pwm = PWMOutputDevice(pwmpin, True, 0, 1000)
self.fwd = PWMOutputDevice(fwdpin)
self.rev = PWMOutputDevice(revpin)
self.speed = 0
def initialize(self):
self.rightf = PWMOutputDevice(12, True, 0, 1000)
self.rightr = PWMOutputDevice(13, True, 0, 1000)
self.leftf = PWMOutputDevice(14, True, 0, 1000)
self.leftr = PWMOutputDevice(15, True, 0, 1000)
self.speedr = 0
self.speedl = 0
self.offsetr = 0
self.offsetl = 0
self.speedmax = 1
print("start motor")
def __init__(self):
self.leftMotorEnable = PWMOutputDevice(LEFT_MOTOR_ENABLE_PIN)
self.leftMotorDirection1 = DigitalOutputDevice(LEFT_MOTOR_IN1_PIN)
self.leftMotorDirection2 = DigitalOutputDevice(LEFT_MOTOR_IN2_PIN)
self.rightMotorEnable = PWMOutputDevice(RIGHT_MOTOR_ENABLE_PIN)
self.rightMotorDirection1 = DigitalOutputDevice(RIGHT_MOTOR_IN1_PIN)
self.rightMotorDirection2 = DigitalOutputDevice(RIGHT_MOTOR_IN2_PIN)
self.speed = 0
self.speedLeft = 0
self.speedRight = 0
self.direction = 0
class Motor:
def __init__(self, in1, in2, enable):
self._in1 = OutputDevice(in1)
self._in2 = OutputDevice(in2)
self._en = PWMOutputDevice(enable)
@property
def direction(self):
return self._in1.value > self._in2.value
@direction.setter
def direction(self, cw=True):
if cw is None:
self._in1.value = self._in2.value = 0
if cw:
self._in1.value = 1
self._in2.value = 0
else:
self._in1.value = 0
self._in2.value = 1
@property
def speed(self):
return self._en.value * (1.0 if self.direction else -1.0)
@speed.setter
def speed(self, speed: float):
"""
Turn on motor. Negative speed = CCW direction
:param speed: float [-1.0 .. 1.0]
"""
if self.speed == speed:
return
if speed == 0:
self.off()
return
if abs(speed) > 1.:
speed = 1. if speed > 0 else -1.
self.direction = speed > 0
self._en.value = abs(speed)
def off(self):
"""
Stop motor
"""
logger.debug(f'motor{self}.off()')
self._en.off()
self.direction = None
def set_pwm(self):
# Initialise objects for H-Bridge PWM pins
# Set initial duty cycle to 0 and frequency to 1000
self.forwardLeft = PWMOutputDevice(self.PWM_FORWARD_LEFT_PIN, True, 0,
100)
self.reverseLeft = PWMOutputDevice(self.PWM_REVERSE_LEFT_PIN, True, 0,
100)
self.forwardRight = PWMOutputDevice(self.PWM_FORWARD_RIGHT_PIN, True,
0, 100)
self.reverseRight = PWMOutputDevice(self.PWM_REVERSE_RIGHT_PIN, True,
0, 100)
def _fade_led(led_pin: PWMOutputDevice,
intensity: float,
fade: float = 1.0,
steps: int = 100):
brightness = led_pin.value
step = (intensity - brightness) / float(steps)
wait = fade / float(steps)
if step != 0.:
for i in np.arange(brightness, intensity, step):
led_pin.value = i
sleep(wait)
led_pin.value = intensity
def __init__(self):
MOTA_PWM_FORWARD_LEFT_PIN = 26 # IN1 - Forward Drive
MOTA_PWM_REVERSE_LEFT_PIN = 19 # IN2 - Reverse Drive
MOTB_PWM_FORWARD_RIGHT_PIN = 6 # IN1 - Forward Drive
MOTB_PWM_REVERSE_RIGHT_PIN = 13 # IN2 - Reverse Drive
MOTC_PWM_FORWARD_RIGHT_PIN = 22 # IN1 - Forward Drive
MOTC_PWM_REVERSE_RIGHT_PIN = 27 # IN2 - Reverse Drive
MOTD_PWM_FORWARD_LEFT_PIN = 9 # IN1 - Forward Drive
MOTD_PWM_REVERSE_LEFT_PIN = 10 # IN2 - Reverse Drive
self.forwardLeftMotA = PWMOutputDevice(MOTA_PWM_FORWARD_LEFT_PIN, True,
0, 1000) #frequency = 1000hz
self.reverseLeftMotA = PWMOutputDevice(MOTA_PWM_REVERSE_LEFT_PIN, True,
0, 1000)
self.forwardRightMotB = PWMOutputDevice(MOTB_PWM_FORWARD_RIGHT_PIN,
True, 0, 1000)
self.reverseRightMotB = PWMOutputDevice(MOTB_PWM_REVERSE_RIGHT_PIN,
True, 0, 1000)
self.forwardLeftMotC = PWMOutputDevice(MOTC_PWM_FORWARD_RIGHT_PIN,
True, 0, 1000)
self.reverseLeftMotC = PWMOutputDevice(MOTC_PWM_REVERSE_RIGHT_PIN,
True, 0, 1000)
self.forwardRightMotD = PWMOutputDevice(MOTD_PWM_FORWARD_LEFT_PIN,
True, 0, 1000)
self.reverseRightMotD = PWMOutputDevice(MOTD_PWM_REVERSE_LEFT_PIN,
True, 0, 1000)
def __init__(self):
'''
1. Read pin number from configure file
2. Init all GPIO configureation
'''
config = configparser.ConfigParser()
config.read("config.ini")
freFrequency = 30
backFrequency = 30
self.lfMotor = Motor(config.getint("car", "LEFT_FRONT_1"), config.getint("car", "LEFT_FRONT_2"), pin_factory=factory)
self.rfMotor = Motor(config.getint("car", "RIGHT_FRONT_1"), config.getint("car", "RIGHT_FRONT_2"), pin_factory=factory)
self.rfPwa = PWMOutputDevice(config.getint("car", "RIGHT_FRONT__PWA"), frequency=freFrequency, pin_factory=factory)
self.lfPwa = PWMOutputDevice(config.getint("car", "LEFT_FRONT__PWA"), frequency=freFrequency, pin_factory=factory)
self.forwardFire = LED(config.getint("car", "FORWARD_STBY"), pin_factory=factory)
self.lbMotor = Motor(config.getint("car", "LEFT_BEHIND_1"), config.getint("car", "LEFT_BEHIND_2"), pin_factory=factory)
self.rbMotor = Motor(config.getint("car", "RIGHT_BEHIND_1"), config.getint("car", "RIGHT_BEHIND_2"), pin_factory=factory)
self.rbPwa = PWMOutputDevice(config.getint("car", "RIGHT_BEHIND__PWA"), frequency=backFrequency, pin_factory=factory)
self.lbPwa = PWMOutputDevice(config.getint("car", "LEFT_BEHIND__PWA"), frequency=backFrequency, pin_factory=factory)
self.behindFire = LED(config.getint("car", "BEHIND_STBY"), pin_factory=factory)
self.fire_on()
class TB6612FNG:
def __init__(self, pin_fig_in1, pin_fig_in2, pin_fig_pwm,
frequency=None):
self.in1 = DigitalOutputDevice(pin=pin_fig_in1)
self.in2 = DigitalOutputDevice(pin=pin_fig_in2)
if frequency is None: # Not PWM mode
self.pwm = DigitalOutputDevice(pin=pin_fig_pwm)
else: # PWM mode
self.pwm = PWMOutputDevice(pin=pin_fig_pwm,
frequency=frequency)
def cw(self):
self.in1.on()
self.in2.off()
self.pwm.on()
def ccw(self):
self.in1.off()
self.in2.on()
self.pwm.on()
def stop(self):
self.in1.off()
self.in2.off()
self.pwm.off()
def stop_and_close(self):
self.stop()
self.in1.close()
self.in2.close()
self.pwm.close()
def __init__(self):
if not is_raspberry_pi():
raise Exception("This class works only on Raspberry Pi")
from gpiozero import PWMOutputDevice, OutputDevice
self._yaw = 0.0
self._speed = 0.0
self._pwm_left = PWMOutputDevice(18, frequency=440)
self._forward_left = OutputDevice(4)
self._backward_left = OutputDevice(3)
self._forward_right = OutputDevice(17)
self._backward_right = OutputDevice(27)
self._pwm_right = PWMOutputDevice(22, frequency=440)
def __init__(self):
if env == "prod":
log.debug("Using %s configuration" % self.CONTROLLER_BOARD )
self.SERVO_H = AngularServo(self.HEAD_HORIZONTAL_PIN, min_angle=-80, max_angle=80)
self.SERVO_V = AngularServo(self.HEAD_VERTICAL_PIN, min_angle=-80, max_angle=80)
##Digital devices
self.forward_left_device = None
self.forward_right_device = None
self.backward_left_device = None
self.backward_right_device = None
self.wheel_right_step = None
self.wheel_left_step = None
self.wheel_right_heading = None
self.wheel_left_heading = None
self.wheel_right_enabled = None
self.wheel_left_enabled = None
self.pwm_left = None
self.pwm_right = None
if self.CONTROLLER_BOARD == "v1":
self.forward_left_device = DigitalOutputDevice(self.FORWARD_LEFT_PIN, True, False)
self.forward_right_device = DigitalOutputDevice(self.FORWARD_RIGHT_PIN, True, False)
self.backward_left_device = DigitalOutputDevice(self.BACKWARD_LEFT_PIN, True, False)
self.backward_right_device = DigitalOutputDevice(self.BACKWARD_RIGHT_PIN, True, False)
self.pwm_left = PWMOutputDevice(self.PWM_LEFT_PIN, True, False, self.FRECUENCY)
self.pwm_right = PWMOutputDevice(self.PWM_RIGHT_PIN, True, False, self.FRECUENCY)
if self.CONTROLLER_BOARD == "v2":
self.wheel_right_step = DigitalOutputDevice(self.WHEEL_RIGHT_STEP, True, False)
self.wheel_left_step = DigitalOutputDevice(self.WHEEL_LEFT_STEP, True, False)
self.wheel_right_heading = DigitalOutputDevice(self.WHEEL_RIGHT_HEADING, True, False)
self.wheel_left_heading = DigitalOutputDevice(self.WHEEL_LEFT_HEADING, True, False)
self.wheel_right_enabled = DigitalOutputDevice(self.WHEEL_RIGHT_ENABLED, True, False)
self.wheel_left_enabled = DigitalOutputDevice(self.WHEEL_LEFT_ENABLED, True, False)
self.current_horizontal_head_pos = 0
self.current_vertical_head_pos = 0
self.center_head()
self._counting_steps = 0
self._current_steps = 0
self._stepper_current_step = 0
class MotorMove(object):
def __init__(self):
self.pwm = PWMOutputDevice(4)
def run(self, voice_command):
if 'on' in voice_command:
self.pwm.on()
elif 'off' in voice_command:
self.pwm.off()
# =========================================
# Makers! Add your own voice commands here.
# =========================================
actor.add_keyword('motor', MotorMove())
return actor
def __init__(
self,
fan_pin,
min_pwm_strength=0.5,
fan_max_rpm=100,
temp_lb=50,
temp_ub=70,
logger=print,
dry_run=False):
"""CPUFan init
"""
self.fan = PWMOutputDevice(fan_pin, frequency=20)
self.temp_reader = CPUTemperature(
min_temp=temp_lb,
max_temp=temp_ub,
threshold=temp_ub,
min_pwm_strength=min_pwm_strength)
self.fan_max_rpm = fan_max_rpm
self.logger = logger
if not dry_run:
self.fan.source = self.temp_reader
else:
self.fan.value = dry_run
def __init__(self, gpioMotor=4, tolerancia=8):
self.gpioMotor = gpioMotor
self.motor = PWMOutputDevice(self.gpioMotor)
self.tolerancia = tolerancia
self.stopped = True
self.commands = CommandsBussola()
self.distancia = 99999
def __init__(self, node1, node2, node3, node4):
self.anode = InputDevice(node1)
self.cathode = InputDevice(node2)
self.node1 = node1
self.node2 = node2
self.node3 = node3
self.node4 = node4
def _connectPins(cls):
"""Creates gpiozero objects for components based on pins specified at class level"""
if not Pins.pins_initialized:
logger.debug("Connecting Pins")
if IS_RASPBERRY_PI:
logger.info(W1ThermSensor.get_available_sensors())
cls.adc = ADS1115()
cls.tempsensors = [W1ThermSensor(W1ThermSensor.THERM_SENSOR_DS18B20, id) for id in Pins.TEMP_SENSOR_IDS]
for sensor in cls.tempsensors:
logger.info(f'Sensor ID: {sensor.id}')
cls.servo = Ax12()
else:
cls.adc = None
cls.servoconnection = None
cls.tempsensors = None
try:
# this little bit of weirdness is a consequence of gpiozero's fake pin code being goofy
cls.pwmPinFactory = Device.pin_factory
if not IS_RASPBERRY_PI:
Device.pin_factory = MockFactory()
cls.pwmPinFactory = MockFactory(pin_class=MockPWMPin)
cls.heatingElements = [
PWMOutputDevice(n, pin_factory=cls.pwmPinFactory)
for n in Pins.heatingElementGPIOs]
cls.ballValves = [OutputDevice(n) for n in Pins.ballValveGPIOs]
for index, valve in enumerate(cls.ballValves):
if (index >4):
valve.close()
cls.airPump = OutputDevice(Pins.airPumpGPIO)
cls.airPump.close()
cls.pumps = [PWMOutputDevice(n, pin_factory=cls.pwmPinFactory)
for n in Pins.pumpGPIOs]
for pump in cls.pumps:
pump.toggle()
logger.debug(f'Pump states: {[x.value for x in cls.pumps]}')
cls.heatingElementSwitch = OutputDevice(Pins.heatingElementSwitchGPIO)
cls.GPZeroComponents = (
cls.ballValves + cls.heatingElements + cls.pumps
)
except GPIOPinInUse as e:
print(e)
def __init__(self, IN1, IN2, PWM, STBY):
self.input_1 = OutputDevice(IN1)
self.input_2 = OutputDevice(IN2)
self.speed_control = PWMOutputDevice(PWM)
self.standby = OutputDevice(STBY)
self.standby.on()
self.speed_control.value = 1
def __init__(self):
self.sig_hndlr = signal.signal(signal.SIGINT, self.exit_gracefully)
#set up GPIO
self.driveLeft = PWMOutputDevice(PWM_DRIVE_LEFT, True, 0, 1000)
self.driveRight = PWMOutputDevice(PWM_DRIVE_RIGHT, True, 0, 1000)
self.forwardLeft = DigitalOutputDevice(FORWARD_LEFT_PIN)
self.reverseLeft = DigitalOutputDevice(REVERSE_LEFT_PIN)
self.forwardRight = DigitalOutputDevice(FORWARD_RIGHT_PIN)
self.reverseRight = DigitalOutputDevice(REVERSE_RIGHT_PIN)
# Set up sensors
self.camera = Camera()
self.microphone = Microphone()
self.state = Idle()
class Motor:
"""
The class takes three pin numbers as the input to control one of the motor connected to TB6612FNG module.
"""
def __init__(self, in1, in2, pwm):
self.in1 = DigitalOutputDevice(in1)
self.in1.off()
self.in2 = DigitalOutputDevice(in2)
self.in2.on()
self.pwm = PWMOutputDevice(pwm, frequency=1000)
def set_throttle(self, val):
"""Control the orientation and the speed of the motor.
Arguments:
val: a number between -1.0 and 1.0. The motor rotates in forward direction if val > 1, otherwise in reverse direction.
Setting val to None will set the motor to stop mode.
"""
# Set the motor to stop mode.
if val is None:
self.in1.off()
self.in2.off()
self.pwm.value = 1.0
else:
# Determine the orientation of the motor.
if val > 0.0:
self.in1.off()
self.in2.on()
else:
self.in1.on()
self.in2.off()
# Clamp the pwm signal (throttle) to [0, 1].
pwm = max(0.0, min(abs(val), 1.0))
# Note that setting PWM to low will brake the motor no matter what
# in1 and in2 input is.
self.pwm.value = pwm
def close(self):
self.in1.close()
self.in2.close()
self.pwm.close()
def __init__(self, **kwargs):
#super().__init__('py_driver')
#self.subscription = self.create_subscription(Twist,'cmd_vel',self.listener_callback,0) # Store 10 messages, drop the oldest one
#self.subscription # prevent unused variable warning
self.kwargs = kwargs
# print(self.kwargs)
self._left_pwm = PWMOutputDevice(kwargs['left_pin'],
kwargs['left_active_high'])
self._left_motor = Motor(kwargs['left_forward'],
kwargs['left_backward'], kwargs['left_pwm'])
self._right_pwm = PWMOutputDevice(kwargs['right_pin'],
kwargs['right_active_high'])
self._right_motor = Motor(kwargs['right_forward'],
kwargs['right_backward'],
kwargs['right_pwm'])
print("Py Pi driver initialiased")
def three_pin_motor(pin_factory):
motor = ThreePinMotor(
DigitalOutputDevice(1, pin_factory=pin_factory),
DigitalOutputDevice(2, pin_factory=pin_factory),
PWMOutputDevice(3, pin_factory=pin_factory),
)
yield motor
motor.close()
def setup(self, multiplex_handler, right_sensor_channel):
# IN1 - Forward Drive
self.L298_LEFT_PWM_FORWARD_PIN = 26
#IN2 - Reverse Drive
self.L298_LEFT_PWM_REVERSE_PIN = 19
self.forwardLeft = PWMOutputDevice(self.L298_LEFT_PWM_FORWARD_PIN,
True, 0, 1000)
self.reverseLeft = PWMOutputDevice(self.L298_LEFT_PWM_REVERSE_PIN,
True, 0, 1000)
self.i2c = multiplex_handler.getI2C()
self.sensor = adafruit_vl6180x.VL6180X(self.i2c)
self.multiplex_handler = multiplex_handler
self.right_sensor_channel = right_sensor_channel
def __init__(self, forward=None, backward=None, enable=None):
if not all(p is not None for p in [forward, backward, enable]):
raise GPIOPinMissing('all pins must be provided')
super(TwoWayMotor,
self).__init__(forward_device=DigitalOutputDevice(forward),
backward_device=DigitalOutputDevice(backward),
enable_device=PWMOutputDevice(enable),
_order=('forward_device', 'backward_device',
'enable_device'))
class Robot(object):
SPEED_HIGH = int(config.get("robot.speed","speed_high"))
SPEED_MEDIUM = int(config.get("robot.speed","speed_medium"))
SPEED_LOW = int(config.get("robot.speed","speed_low"))
##Define the arc of the turn process by a tuple wheels speed (left, right)
LEFT_ARC_CLOSE = eval(config.get("robot.speed","left_arc_close"))
LEFT_ARC_OPEN = eval(config.get("robot.speed","left_arc_open"))
RIGHT_ARC_CLOSE = eval(config.get("robot.speed","right_arc_close"))
RIGHT_ARC_OPEN = eval(config.get("robot.speed","right_arc_open"))
#Pin pair left
FORWARD_LEFT_PIN = int(config.get("robot.board.v1","forward_left_pin"))
BACKWARD_LEFT_PIN = int(config.get("robot.board.v1","backward_left_pin"))
#Pin pair right
FORWARD_RIGHT_PIN = int(config.get("robot.board.v1","forward_right_pin"))
BACKWARD_RIGHT_PIN = int(config.get("robot.board.v1","backward_right_pin"))
#PWM PINS
PWM_LEFT_PIN = int(config.get("robot.board.v1","pwm_left_pin"))
PWM_RIGHT_PIN = int(config.get("robot.board.v1","pwm_right_pin"))
#Frecuency by hertz
FRECUENCY = int(config.get("robot.board.v1","frecuency"))
# Cycles fits for pwm cycles
LEFT_CYCLES_FIT = int(config.get("robot.board.v1","left_cycles_fit"))
RIGHT_CYCLES_FIT = int(config.get("robot.board.v1","right_cycles_fit"))
#Pin settings for head control
HEAD_HORIZONTAL_PIN = int(config.get("robot.board.v1","head_pwm_pin_horizontal_axis"))
HEAD_VERTICAL_PIN = int(config.get("robot.board.v1","head_pwm_pin_vertical_axis"))
HEAD_HORIZONTAL_RANGE = config.get("robot.board.v1","head_horizontal_range").split(",")
HEAD_VERTICAL_RANGE = config.get("robot.board.v1","head_vertical_range").split(",")
RIGHT_WHEEL_SENSOR = int(config.get("robot.board.v1","right_wheel_sensor"))
LEFT_WHEEL_SENSOR = int(config.get("robot.board.v1", "left_wheel_sensor"))
CONTROLLER_BOARD = config.get("robot.controller", "board")
#v2
WHEEL_LEFT_ENABLED = int(config.get("robot.board.v2", "wheel_left_enabled"))
WHEEL_LEFT_HEADING = int(config.get("robot.board.v2", "wheel_left_heading"))
WHEEL_LEFT_STEP = int(config.get("robot.board.v2", "wheel_left_step"))
WHEEL_RIGHT_ENABLED = int(config.get("robot.board.v2", "wheel_right_enabled"))
WHEEL_RIGHT_HEADING = int(config.get("robot.board.v2", "wheel_right_heading"))
WHEEL_RIGHT_STEP = int(config.get("robot.board.v2", "wheel_right_step"))
SERVO_V = None
SERVO_H = None
head_vertical_current_position = None
head_horizontal_current_position = None
def __init__(self):
if env == "prod":
log.debug("Using %s configuration" % self.CONTROLLER_BOARD )
self.SERVO_H = AngularServo(self.HEAD_HORIZONTAL_PIN, min_angle=-80, max_angle=80)
self.SERVO_V = AngularServo(self.HEAD_VERTICAL_PIN, min_angle=-80, max_angle=80)
##Digital devices
self.forward_left_device = None
self.forward_right_device = None
self.backward_left_device = None
self.backward_right_device = None
self.wheel_right_step = None
self.wheel_left_step = None
self.wheel_right_heading = None
self.wheel_left_heading = None
self.wheel_right_enabled = None
self.wheel_left_enabled = None
self.pwm_left = None
self.pwm_right = None
if self.CONTROLLER_BOARD == "v1":
self.forward_left_device = DigitalOutputDevice(self.FORWARD_LEFT_PIN, True, False)
self.forward_right_device = DigitalOutputDevice(self.FORWARD_RIGHT_PIN, True, False)
self.backward_left_device = DigitalOutputDevice(self.BACKWARD_LEFT_PIN, True, False)
self.backward_right_device = DigitalOutputDevice(self.BACKWARD_RIGHT_PIN, True, False)
self.pwm_left = PWMOutputDevice(self.PWM_LEFT_PIN, True, False, self.FRECUENCY)
self.pwm_right = PWMOutputDevice(self.PWM_RIGHT_PIN, True, False, self.FRECUENCY)
if self.CONTROLLER_BOARD == "v2":
self.wheel_right_step = DigitalOutputDevice(self.WHEEL_RIGHT_STEP, True, False)
self.wheel_left_step = DigitalOutputDevice(self.WHEEL_LEFT_STEP, True, False)
self.wheel_right_heading = DigitalOutputDevice(self.WHEEL_RIGHT_HEADING, True, False)
self.wheel_left_heading = DigitalOutputDevice(self.WHEEL_LEFT_HEADING, True, False)
self.wheel_right_enabled = DigitalOutputDevice(self.WHEEL_RIGHT_ENABLED, True, False)
self.wheel_left_enabled = DigitalOutputDevice(self.WHEEL_LEFT_ENABLED, True, False)
self.current_horizontal_head_pos = 0
self.current_vertical_head_pos = 0
self.center_head()
self._counting_steps = 0
self._current_steps = 0
self._stepper_current_step = 0
def _set_left_forward(self):
if self.CONTROLLER_BOARD == "v1":
self.forward_left_device.on()
self.backward_left_device.off()
def _set_left_backward(self):
if self.CONTROLLER_BOARD == "v1":
self.forward_left_device.off()
self.backward_left_device.on()
def _set_left_stop(self):
if self.CONTROLLER_BOARD == "v1":
self.forward_left_device.on()
self.backward_left_device.on()
def _set_right_forward(self):
if self.CONTROLLER_BOARD == "v1":
self.forward_right_device.on()
self.backward_right_device.off()
def _set_right_backward(self):
if self.CONTROLLER_BOARD == "v1":
self.forward_right_device.off()
self.backward_right_device.on()
def _set_right_stop(self):
if self.CONTROLLER_BOARD == "v1":
self.forward_right_device.on()
self.backward_right_device.on()
def set_forward(self):
log.debug("setting movement to forward")
if env == "prod":
self._set_left_forward()
self._set_right_forward()
def set_backward(self):
log.debug("setting movement to backward")
if env == "prod":
self._set_left_backward()
self._set_right_backward()
def set_rotate_left(self):
log.debug("setting movement to rotate left")
if env == "prod":
self._set_left_backward()
self._set_right_forward()
def set_rotate_right(self):
log.debug("setting movement to rotate right")
if env == "prod":
self._set_right_backward()
self._set_left_forward()
def stop(self):
log.debug("stopping")
if env == "prod":
if self.CONTROLLER_BOARD == "v1":
self.pwm_left.off()
self.pwm_right.off()
def move(self, speed=None, arc=None, steps=100, delay=0.7, heading=1):
if self.CONTROLLER_BOARD == "v1":
self._move_dc(speed, arc)
elif self.CONTROLLER_BOARD == "v2":
self._move_steppers_bipolar(steps=steps, delay=delay, heading=heading)
def _move_dc(self, speed, arc):
log.debug("Moving using DC motors")
if (speed and arc):
print("Error: speed and arc could not be setted up at the same time")
return
if env == "prod":
self.pwm_left.on()
self.pwm_right.on()
if (speed):
log.debug("moving on " + str(speed))
log.debug("aplying fit: left " + str(self.LEFT_CYCLES_FIT) + " right " + str(self.RIGHT_CYCLES_FIT))
aditional_left_clycles = self.LEFT_CYCLES_FIT if ((speed + self.LEFT_CYCLES_FIT) <= 100.00) else 0.00
aditional_right_clycles = self.RIGHT_CYCLES_FIT if ((speed + self.RIGHT_CYCLES_FIT) <= 100.00) else 0.00
if env == "prod":
self.pwm_left.value = (speed + aditional_left_clycles) / 100.00
self.pwm_right.value = (speed + aditional_right_clycles) / 100.00
if (arc):
cycle_left, cycle_right = arc
log.debug("turning -> left wheel: " + str(cycle_left) + " right wheel: " + str(cycle_right))
if env == "prod":
self.pwm_left.value = cycle_left / 100.00
self.pwm_right.value = cycle_right / 100.00
def _move_steppers_bipolar(self, steps, heading, delay):
"""
Currently it would take 4 steps to complete a whole wheel turn
"""
log.debug("Moving steppers bipolars . Steps " + str(steps) + " delay " + str(delay))
steps_left = abs(steps)
if env == "prod":
self.wheel_left_enabled.off()
self.wheel_right_enabled.off()
while(steps_left!=0):
log.debug("Current step: " + str(steps_left))
if env == "prod":
if heading:
self.wheel_left_heading.on()
self.wheel_right_heading.off()
else:
self.wheel_left_heading.off()
self.wheel_right_heading.on()
self.wheel_left_step.off()
self.wheel_right_step.off()
sleep(delay/1000.00)
self.wheel_left_step.on()
self.wheel_right_step.on()
sleep(delay/1000.00)
steps_left -= 1
if env == "prod":
self.wheel_left_enabled.on()
self.wheel_right_enabled.on()
def center_head(self):
log.debug("centering head")
self.head_horizontal_current_position = 0
self.head_vertical_current_position = 0
if env == "prod":
self.SERVO_H.mid()
self.SERVO_V.mid()
sleep(0.2)
self.SERVO_H.detach()
self.SERVO_V.detach()
def _angle_to_ms(self,angle):
return 1520 + (int(angle)*400) / 45
def move_head_horizontal(self, angle):
log.debug("horizontal limits: " + self.HEAD_HORIZONTAL_RANGE[0] +" "+ self.HEAD_HORIZONTAL_RANGE[1])
log.debug("new horizontal angle: " + str(angle))
if angle > int(self.HEAD_HORIZONTAL_RANGE[0]) and angle < int(self.HEAD_HORIZONTAL_RANGE[1]):
log.debug("moving head horizontal to angle: " + str(angle))
self.head_horizontal_current_position = angle
if env == "prod":
self.SERVO_H.angle = angle
sleep(0.2)
self.SERVO_H.detach()
def move_head_vertical(self, angle):
log.debug("vertical limits: " + self.HEAD_VERTICAL_RANGE[0] +" "+ self.HEAD_VERTICAL_RANGE[1])
log.debug("new vertical angle: " + str(angle))
if angle > int(self.HEAD_VERTICAL_RANGE[0]) and angle < int(self.HEAD_VERTICAL_RANGE[1]):
log.debug("moving head vertical to angle: " + str(angle))
self.head_vertical_current_position = angle
if env == "prod":
self.SERVO_V.angle = angle
sleep(0.2)
self.SERVO_V.detach()
#Used for encoders
def steps(self, counting):
self._current_steps = counting
self.move(speed=self.SPEED_HIGH)
self.move(speed=self.SPEED_MEDIUM)
rpio.add_interrupt_callback(RIGHT_WHEEL_SENSOR, self._steps_callback, threaded_callback=True)
rpio.add_interrupt_callback(LEFT_WHEEL_SENSOR, self._steps_callback, threaded_callback=True)
rpio.wait_for_interrupts(threaded=True)
def _steps_callback(self, gpio_id, value):
self._counting_steps += 1
if self._counting_steps > self._current_steps:
self._counting_steps = 0
self._current_steps = 0
rpio.stop_waiting_for_interrupts()
from gpiozero import InputDevice, OutputDevice, PWMOutputDevice
from time import sleep, time
trig = OutputDevice(4)
echo = InputDevice(17)
motor = PWMOutputDevice(14)
sleep(2)
def get_pulse_time():
trig.on()
sleep(0.00001)
trig.off()
while echo.is_active == False:
pulse_start = time()
while echo.is_active == True:
pulse_end = time()
sleep(0.06)
return pulse_end - pulse_start
def calculate_distance(duration):
speed = 343
distance = speed * duration / 2
return distance
def calculate_vibration(distance):
vibration = (((distance - 0.02) * -1) / (4 - 0.02)) + 1
