def main():
# now just write the code you would use on a real Raspberry Pi
from time import sleep
from gpiozero import AngularServo, LED, Button
from weather import Weather
weather = Weather()
def button1_pressed():
city_weather = weather.get_next_weather()
print(city_weather)
servo1.angle = convert_deg_temp(city_weather['temp'])
def button2_pressed():
print("button 2 pressed!")
button1 = Button(11)
button1.when_pressed = button1_pressed
button2 = Button(12)
button2.when_pressed = button2_pressed
servo1 = AngularServo(17, min_angle=-90, max_angle=90)
servo1.angle = convert_deg_temp(weather.get_next_weather()['temp'])
while True:
pass
def __init__(self):
self.p = pyaudio.PyAudio()
self.jaw = AngularServo(c.JAW_PIN, min_angle=c.MIN_ANGLE,
max_angle=c.MAX_ANGLE, initial_angle=None,
min_pulse_width=c.SERVO_MIN/(1*10**6),
max_pulse_width=c.SERVO_MAX/(1*10**6))
self.bp = BPFilter()
class Servo:
def __init__(self, pin, inital_angle=0, reverse=False):
self.servo = None
self.current_angle = inital_angle
angle = self.current_angle - 90
if reverse:
self.servo = AngularServo(pin, angle, min_angle=90, max_angle=-90)
else:
self.servo = AngularServo(pin, angle)
def __set_angle(self, value):
if value is None:
angle = None
else:
self.current_angle = value
angle = self.current_angle - 90
self.servo.angle = angle
def __get_angle(self):
return self.current_angle
angle = property(__get_angle, __set_angle)
def close(self):
self.servo.close()
def main():
parser = argparse.ArgumentParser(
description='Example application for displaying a dial indicator for '
'what object is seen')
parser.add_argument(
'--output_overlay',
default=True,
type=bool,
help='Should the visual overlay be generated')
parser.add_argument(
'--button_enabled',
default=True,
type=bool,
help='Should the button be monitored')
parser.add_argument(
'--button_active',
default=False,
type=bool,
help='Should the button start out active (true) or only be active once '
'pressed (false)')
flags = parser.parse_args()
load_model = time.time()
category_count = len(category_mapper.get_categories())
button = AutoButton(flags.button_active, flags.button_enabled)
for category in category_mapper.get_categories():
print('Category[%d]: %s' % (category_mapper.get_category_index(category),
category))
with picamera.PiCamera() as camera:
camera.resolution = (1640, 1232)
camera.start_preview()
overlay = OverlayManager(
camera) if flags.output_overlay else DummyOverlayManager()
servo = AngularServo(PIN_A, min_pulse_width=.0005, max_pulse_width=.0019)
with CameraInference(image_classification.model()) as classifier:
print('Load Model %f' % (time.time() - load_model))
for result in classifier.run():
if not button.on():
overlay.clear()
servo.angle = -90
continue
classes = image_classification.get_classes(result)
probs = [0] * (category_count + 1)
result_categories = []
for label, score in classes:
category = category_mapper.get_category(label) or 'Other'
probs[category_mapper.get_category_index(category) + 1] += score
result_categories.append(category)
overlay.update(classes, result_categories)
max_prob = max(probs)
best_category = probs.index(max_prob)
if best_category == 0 and max_prob > .5:
servo.angle = -90
elif best_category != 0:
servo.angle = -90 + (180 * best_category) / category_count
print('category: %d - %s' %
(best_category,
category_mapper.get_categories()[best_category - 1]))
def update_jaw(self):
self.jaw = AngularServo(c.JAW_PIN,
min_angle=c.MIN_ANGLE,
max_angle=c.MAX_ANGLE,
initial_angle=None,
min_pulse_width=c.SERVO_MIN / (1 * 10**6),
max_pulse_width=c.SERVO_MAX / (1 * 10**6))
def main():
# now just write the code you would use on a real Raspberry Pi
from time import sleep
from gpiozero import AngularServo, Button, LED
servo1 = AngularServo(17, min_angle=-90, max_angle=90)
# possible_status = ['Rain', 'Clear', 'Clouds', 'Drizzle']
#LED for showig activ city
led_Krk = LED(22)
led_Stk = LED(21)
active_city = "Krakow"
led_Krk.on()
button1 = Button(11)
button2 = Button(12)
button1.when_pressed = lambda: button1_pressed(active_city, led_Krk,
led_Stk)
button2.when_pressed = lambda: button2_pressed(active_city, led_Krk,
led_Stk)
while True:
print("Active city: ", active_city)
servo1.angle = status_to_angle(import_weather(active_city))
sleep(3)
def main():
parser = argparse.ArgumentParser(
description='Example application for displaying a dial indicator for '
'what object is seen')
parser.add_argument(
'--output_overlay',
default=True,
type=bool,
help='Should the visual overlay be generated')
parser.add_argument(
'--button_enabled',
default=True,
type=bool,
help='Should the button be monitored')
parser.add_argument(
'--button_active',
default=False,
type=bool,
help='Should the button start out active (true) or only be active once '
'pressed (false)')
flags = parser.parse_args()
load_model = time.time()
category_count = len(category_mapper.get_categories())
button = AutoButton(flags.button_active, flags.button_enabled)
for category in category_mapper.get_categories():
print('Category[%d]: %s' % (category_mapper.get_category_index(category),
category))
with picamera.PiCamera() as camera:
camera.resolution = (1640, 1232)
camera.start_preview()
overlay = OverlayManager(
camera) if flags.output_overlay else DummyOverlayManager()
servo = AngularServo(PIN_A, min_pulse_width=.0005, max_pulse_width=.0019)
with CameraInference(image_classification.model()) as classifier:
print('Load Model %f' % (time.time() - load_model))
for result in classifier.run():
if not button.on():
overlay.clear()
servo.angle = -90
continue
classes = image_classification.get_classes(result)
probs = [0] * (category_count + 1)
result_categories = []
for label, score in classes:
category = category_mapper.get_category(label) or 'Other'
probs[category_mapper.get_category_index(category) + 1] += score
result_categories.append(category)
overlay.update(classes, result_categories)
max_prob = max(probs)
best_category = probs.index(max_prob)
if best_category == 0 and max_prob > .5:
servo.angle = -90
elif best_category != 0:
servo.angle = -90 + (180 * best_category) / category_count
print('category: %d - %s' %
(best_category,
category_mapper.get_categories()[best_category - 1]))
def __init__(self):
self.__ServoVertical = AngularServo(5, min_angle=0, max_angle=180, min_pulse_width=.000553, max_pulse_width=.002425, frame_width=.02)
self.__ServoHorizontal = AngularServo(6, min_angle=0, max_angle=180, min_pulse_width=.000553, max_pulse_width=.002425, frame_width=.02)
self.__last_known_vert_loc = 0
self.__last_known_hori_loc = 0
#self.default_position()
self.__ServoVertical.angle = None
self.__ServoHorizontal.angle = None
def __init__(self, pin, inital_angle=0, reverse=False):
self.servo = None
self.current_angle = inital_angle
angle = self.current_angle - 90
if reverse:
self.servo = AngularServo(pin, angle, min_angle=90, max_angle=-90)
else:
self.servo = AngularServo(pin, angle)
def slider_angle(slider_num):
n = slider_num
servo1 = AngularServo(gpio20,
min_pulse_width=minPW,
max_pulse_width=maxPW,
initial_angle=0)
servo1.angle = int(n)
print(n)
sleep(.5)
def slider2_angle(slider2_num):
n2 = slider2_num
servo2 = AngularServo(gpio21,
min_pulse_width=minPW,
max_pulse_width=maxPW,
initial_angle=0)
servo2.angle = int(n2)
print(n2)
sleep(.5)
def Turn_Horizontal(slider_value):
n = int(slider_value)
servo_Horizontal = AngularServo(17,
min_angle=-90,
max_angle=90,
min_pulse_width=minPWM,
max_pulse_width=maxPWM)
servo_Horizontal.angle = n
sleep(2)
def slider_change2(slider_value): #slider value for motor 2
textbox.value = slider_value
print(slider_value)
servo2 = AngularServo(Servo2,
min_pulse_width=minPulseWidth,
max_pulse_width=maxPulseWidth,
initial_angle=0,
min_angle=-0,
max_angle=90)
servo2.angle = int(slider_value)
def cam_action(msg):
servo = AngularServo(config.CAMSERVO_TILT,
min_pulse_width=0.2/1000,
max_pulse_width=1.8/1000,
frame_width=20/1000,
)
angle = json.loads(msg["data"])["angle"]
if angle <= 90 and angle >= -90:
servo.angle = angle
time.sleep(1)
def __init__(self):
self.servo_de = AngularServo(self.porta_de,
min_angle=self.min_de,
max_angle=self.max_de)
self.servo_da = AngularServo(self.porta_da,
min_angle=self.min_da,
max_angle=self.max_da)
self.servo_dr = AngularServo(self.porta_dr,
min_angle=self.min_dr,
max_angle=self.max_dr)
def slider_change1(slider1_value):
text1.value = slider1_value
servo1 = AngularServo(Servo1,
min_pulse_width=minpw,
max_pulse_width=maxpw,
initial_angle=0,
min_angle=0,
max_angle=180)
servo1.angle = int(slider1_value)
time.sleep(.25)
def slider_change2(slider2_value):
text2.value = slider2_value
servo2 = AngularServo(Servo2,
min_pulse_width=minpw,
max_pulse_width=maxpw,
initial_angle=0,
min_angle=0,
max_angle=180)
servo2.angle = int(slider2_value)
time.sleep(.25)
def slidervalchange2(slider_val):
textbox.value = slider_val
print(slider_val)
servo2 = AngularServo(20,
min_pulse_width=minPW,
max_pulse_width=maxPW,
initial_angle=0,
min_angle=-90,
max_angle=90)
servo2.angle = int(slider_val)
time.sleep(0.4)
def main():
from time import sleep
from gpiozero import AngularServo, Button
servo1 = AngularServo(17, min_angle=-90, max_angle=90)
button1 = Button(12)
button1.when_activated = change_location
while True:
servo1.angle = set_angle(current_location)
sleep(0.1)
def main():
from time import sleep
from gpiozero import AngularServo, Button
servo1 = AngularServo(17, min_angle=-90, max_angle=90)
button_upper = Button(12)
change_location()
button_upper.when_activated = change_location
while True:
servo1.angle = get_servo_angle(location)
sleep(0.1)
def __init__(self):
self._TRUEZERO = 5
self.throttleScalingFactor = 0.25
IO.setwarnings(False)
IO.setmode(IO.BCM)
IO.setup(12, IO.OUT)
IO.setup(23, IO.OUT, initial=1)
self._p = IO.PWM(12, 500)
self._p.start(0)
self._servo = AngularServo(13, min_angle=-90, max_angle=90)
self._servo.angle = self._TRUEZERO
def camController():
global txCamData, camAngle, headAngle
camPin = 4
headPin = 17
print('camController Started')
camServo = AngularServo(camPin,
min_angle=0,
max_angle=180,
min_pulse_width=(0.5 / 1000),
max_pulse_width=(2.3 / 1000),
frame_width=20 / 1000)
headServo = AngularServo(headPin,
min_angle=-90,
max_angle=90,
min_pulse_width=(0.5 / 1000),
max_pulse_width=(2.3 / 1000),
frame_width=20 / 1000)
camServo.angle = 90
headServo.angle = 0
time.sleep(1)
while True:
txCamData = 0
#time.sleep(0.500)
camAngle = int(rxCamData % 1000) #end digits xxxabc abc is data
headAngle = int(rxCamData / 1000) #begin digits abcxxx
#if rxCamData:
#print("rxCamData: %d" % rxCamData)
camServo.angle = camAngle
headServo.angle = headAngle
def main():
from time import sleep
from gpiozero import AngularServo, Button
owm = OWM('api_key')
reg = owm.city_id_registry()
mgr = owm.weather_manager()
krakow = reg.ids_for('Kraków', country='PL')[0]
istanbul = reg.ids_for('Istanbul', country='TR')[0]
stockholm = reg.ids_for('Stockholm', country='SE')[0]
global btn_state
btn_state = 0
CITIES = [krakow, istanbul, stockholm]
CITY_COUNT = 3
WEATHER = {
'clear': -70,
'mist': -30,
'haze': -30,
'smoke': 10,
'dust': 10,
'sand': 10,
'clouds': 10,
'ash': 10,
'squall': 10,
'drizzle': 50,
'rain': 50,
'snow': 50,
'thunderstorm': 50,
'tornado': 50
}
servo1 = AngularServo(17, min_angle=-90, max_angle=90)
servo1.angle = -90
def button1_pressed():
global btn_state
btn_state += 1
print("Change to " + str(CITIES[btn_state % CITY_COUNT]))
button1 = Button(11)
button1.when_pressed = button1_pressed
while True:
weather = mgr.weather_at_id(CITIES[btn_state % CITY_COUNT][0]).weather
status = str(weather.status).lower()
servo1.angle = WEATHER.get(status)
print(str(CITIES[btn_state % CITY_COUNT]) + " : " + status)
sleep(3)
def main():
# Now just write the code you would use on a real Raspberry Pi
from time import sleep
from gpiozero import AngularServo
servo1 = AngularServo(17, min_angle=-90, max_angle=90)
servo1.angle = -90
while True:
for x in range(-90, 90):
servo1.angle = x
sleep(0.1)
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 __init__(self, codigoDesactivacion):
self.activo = True
self.DIR_BASE = os.path.dirname(os.path.abspath(__file__))
self.rutaLogs = os.path.join(self.DIR_BASE, 'logs')
self.nombreArchivo = 'bitacora_accesso.txt'
self.rutaArchivo = os.path.join(self.rutaLogs, self.nombreArchivo)
self.crearArchivo()
self.estadoServo = True
self.estadoServo2 = True
self.anguloDefault = 90
self.angulo2Default = 90
self.gpioServoPin = 17
self.gpioServo2Pin = 20
self.codigoDesactivacion = codigoDesactivacion
self.servo = AngularServo(self.gpioServoPin)
self.servo2 = AngularServo(self.gpioServo2Pin)
def gpioInit(self):
self.vrChannel = MCP3008(channel=7)
self.distanceSensor = DistanceSensor(23, 24)
self.servo = AngularServo(18, min_angle=-45, max_angle=45)
self.servo.angle = 0.0
Timer(1, self.gpioAutoUpdate).start()
class actuatorController:
def __init__(self):
self._TRUEZERO = 5
self.throttleScalingFactor = 0.25
IO.setwarnings(False)
IO.setmode(IO.BCM)
IO.setup(12, IO.OUT)
IO.setup(23, IO.OUT, initial=1)
self._p = IO.PWM(12, 500)
self._p.start(0)
self._servo = AngularServo(13, min_angle=-90, max_angle=90)
self._servo.angle = self._TRUEZERO
def actuate(self, direction, throttle, angle, brake):
if not brake:
self._p.ChangeDutyCycle(throttle * self.throttleScalingFactor)
else:
self.throttle = 0
if angle < 0:
self._servo.angle = (angle / 90 * 95) + self._TRUEZERO
elif angle > 0:
self._servo.angle = (angle / 90 * 85) + self._TRUEZERO
else:
self._servo.angle = self._TRUEZERO
IO.output(23, direction)
def stopAllMotors(self):
self._p.stop()
self.throttle = 0
self.brake = True
def releaseBrake(self):
self.brake = False
def setDirectionReverse(self):
self.direction = 0
#only call this on exit
def exit(self):
self._p.stop()
self._servo.close()
IO.cleanup()
self.running = False
class Custom_Servo(object):
def __init__(self, pin):
self.pin = pin
self.getValues()
self.motor = AngularServo(self.pin,
min_angle=self.min,
max_angle=self.max)
def getValues(self):
s = Servo(self.pin)
self.min = s.min() # measure the angle
self.max = s.max() # measure the angle
def open_servo(self):
self.motor.max()
def close_servo(self):
time.sleep(3)
self.motor.mid()
def __init__(self, pin: int, angMin: float = -90, angMax: float = 90):
self.pin = pin
if environ.get('development') == '1':
# This creates a mock servo, for simulating controls and testing,
# without having to use the Raspberry Pi every time
self.s = DevAngularServo(pin, initial_angle=0)
else:
self.s = AngularServo(pin, initial_angle=0)
self.angMax = angMax
self.angMin = angMin
def __init__(self, pin, min_angle=-90, max_angle=90, default_angle=0):
super().__init__()
self.pin = pin
self.min_angle = min_angle
self.max_angle = max_angle
self.resource = AngularServo(pin,
min_angle=min_angle,
max_angle=max_angle)
self.resource.angle = default_angle
def main():
# 初期化
factory = PiGPIOFactory()
# min_pulse_width, max_pulse_width, frame_width =>SG90仕様
servo = AngularServo(SERVO_PIN, min_angle=MIN_DEGREE, max_angle=MAX_DEGREE,
min_pulse_width=0.5/1000, max_pulse_width=2.4/1000, frame_width=1/50,
pin_factory=factory)
# SG90を -60度 <-> +60度で角度を変える
try:
for _ in range(5):
servo.angle = 60
sleep(1.0)
servo.angle = -60
sleep(1.0)
except KeyboardInterrupt:
print("stop")
return
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()
