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()