class MotorFunction:
def __init__(self):
self.leftArmPin = 4
self.leftArmMin = -180
self.leftArmMax = -100
self.rightArmPin = 17
self.rightArmMin = -180
self.rightArmMax = 0
self.swivelPin = 27
self.swivelArmMin = -180
self.swivelArmMax = 180
self.leftServo = AngularServo(self.leftArmPin, min_angle=-180, max_angle=180)
self.rightServo = AngularServo(self.rightArmPin, min_angle=-180, max_angle=180)
self.swivelServo = AngularServo(self.swivelPin, min_angle=-180, max_angle=180)
self.increment = 10
def move(self, startFraction, endFraction, servo, min, max, duration):
startPos = int((max - min) * startFraction + min)
endPos = int((max - min) * endFraction + min)
delay = duration / ((endPos - startPos+1.0) / self.increment)
# print("Start: " + startPos + " End: " + endPos + " Delay:" + delay)
for x in range(startPos, endPos, self.increment):
time.sleep(delay)
servo.angle = x
def moveSwivel(self, start, end, time):
self.move(
start, end, self.swivelServo, self.swivelArmMin, self.swivelArmMax, time,
)
def moveLeftArm(self, start, end, time):
self.move(
start, end, self.leftServo, self.leftArmMin, self.leftArmMax, time,
)
def moveRightArm(self, start, end, time):
self.move(
start, end, self.rightServo, self.rightArmMin, self.rightArmMax, time,
)
def clear(self):
self.leftServo.detach()
self.rightServo.detach()
self.swivelServo.detach()
import gpiozero
from gpiozero.pins.pigpio import PiGPIOFactory
gpiozero.Device.pin_factory = PiGPIOFactory()
from gpiozero import AngularServo
from time import sleep
print(gpiozero.Device.pin_factory)
pan = AngularServo(17, min_angle=-45, max_angle=45)
tilt = AngularServo(27, min_angle=-45, max_angle=45)
#while True:
print('start')
pan.angle = 0.0
tilt.angle = 0.0
sleep(5)
pan.angle = 40
tilt.angle = 40
sleep(5)
pan.detach()
tilt.detach()
servo = AngularServo(25, min_angle=50, max_angle=0)
controller = PidController(P=0.5, D=0.4, I=0.00, noise_threshold=10)
SETPOINT = 25
MIN_COMMAND = 0
MAX_COMMAND = 50
balanced_count = 0
balanced_time = 30
while True:
error = sensor.distance * 100 - SETPOINT
if abs(error) < 3:
balanced_count += 1
print('balanced for: {}'.format(balanced_count))
if balanced_count > balanced_time:
servo.detach()
print('error: {}'.format(error))
sleep(0.1)
continue
if balanced_count > 30:
balanced_count = 0
if abs(error) > 25:
print('error: {}'.format(error))
sleep(0.1)
continue
command = controller.update(error) + SETPOINT
if command < MIN_COMMAND or command > MAX_COMMAND:
command = MIN_COMMAND if command < MIN_COMMAND else MAX_COMMAND
motr = Motor(15, 14)
lr_pos_r = 0
fwrev_pos_r = 0
cam_servo = AngularServo(5,
min_angle=0,
max_angle=180,
min_pulse_width=0.0006,
max_pulse_width=0.0024)
initial_servo_pos = 80
max_servo_pos = 120
cam_servo.angle = initial_servo_pos
cam_servo.detach()
servo_pos = initial_servo_pos
servo_thread_running = False
up = False
down = False
def on_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc))
client.subscribe('joystick/lr')
client.subscribe('joystick/fwrev')
client.subscribe('joystick/accl')
client.subscribe('joystick/cam_rst')
roi_gray = gray[y:y + h, x:x + w]
eyes = eye_cascade.detectMultiScale(roi_gray)
for (ex, ey, ew, eh) in eyes:
if display:
cv2.rectangle(roi_color, (ex, ey), (ex + ew, ey + eh),
(0, 255, 0), 2)
isEye = True
break
if isEye:
print("[INFO] Face detected.")
# move to random position
sh.angle = randint(-50, 50)
sv.angle = randint(-50, 50)
else:
sh.detach()
sv.detach()
if display:
cv2.imshow("Shybot's view", frame)
key = cv2.waitKey(1) & 0xFF
# if the 'q' key was pressed, break from the loop
if key == ord("q"):
break
# do a bit of cleanup
cv2.destroyAllWindows()
vs.stop()
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()
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()
import time
from gpiozero import AngularServo
import numpy
print("Left: " + sys.argv[1])
print("Right:" + sys.argv[2])
leftArm = int(sys.argv[1])
rightArm = int(sys.argv[2])
min = -90
max = 180
leftServo = AngularServo(leftArm, min_angle=min, max_angle=max)
rightServo = AngularServo(rightArm, min_angle=min, max_angle=max)
while True:
for x in range(min, 180, 1):
print(x)
# leftServo.angle = 0 - x
rightServo.angle = x
time.sleep(0.005)
time.sleep(1)
leftServo.detach()
rightServo.detach()
#!/usr/bin/python3
import time
from gpiozero import AngularServo
cpu = AngularServo(7) #one on left
ram = AngularServo(25) #one on right
cpu.detach() #Detach servos to prevent "juddering"
ram.detach()
def pushToServos(cpuVal, ramVal):
print("Received {} cpu, {} ram".format(cpuVal, ramVal))
move(cpuVal, ramVal)
def move(cpuVal, ramVal):
cpuVal = (cpuVal * -1.4) + 70 #map percentages to relevant servo values
ramVal = (ramVal * -1.4) + 70 + 15 #same as above, minus servo offset
cpu.angle = cpuVal
ram.angle = ramVal
time.sleep(0.5) #Allow servo to reach new position before detaching
cpu.detach() #Detach servos to prevent "juddering"
ram.detach()
time.sleep(0.5) #Sleep so that if move() contantly called, no judder occurs
left_red_thruster = LEDBoard(27, pwm=True)
left_green_thruster = LEDBoard(22, pwm=True)
right_red_thruster = LEDBoard(4, pwm=True)
right_green_thruster = LEDBoard(17, pwm=True)
boosters = LEDBoard(24, 25, pwm=True)
small_Engine_leds = LEDBoard(10, 9, 11, 0, pwm=True)
large_Engine_led = LEDBoard(2, pwm=True)
## Thruster Servos
myCorrection = 0.45
maxPW = (2.0 + myCorrection) / 1000
minPW = (1.0 - myCorrection) / 1000
left_thruster = AngularServo(12, min_pulse_width=minPW, max_pulse_width=maxPW)
right_thruster = AngularServo(14, min_pulse_width=minPW, max_pulse_width=maxPW)
left_thruster.angle = 0
left_thruster.detach()
right_thruster.angle = 0
right_thruster.detach()
## Define the buttons
static_button = Button(1)
cruising_button = Button(8)
orbital_button = Button(15)
## Set thrusters default position and detach
def setThrusters(angle):
left_thruster.angle = angle
right_thruster.angle = angle
sleep(.3)
left_thruster.detach()
