def __init__(self,
left_front_motor=None,
right_front_motor=None,
left_back_motor=None,
right_back_motor=None,
game="default"):
self.left_front = 0
self.right_front = 0
self.left_back = 0
self.right_back = 0
self.full_game_path = server + "/game/" + game
if left_front_motor is not None:
self.leftFrontMotor = left_front_motor
else:
self.leftFrontMotor = MotorKit().motor1
if right_front_motor is not None:
self.rightFrontMotor = right_front_motor
else:
self.rightFrontMotor = MotorKit().motor2
if left_back_motor is not None:
self.leftBackMotor = left_back_motor
else:
self.leftBackMotor = MotorKit().motor3
if right_back_motor is not None:
self.rightBackMotor = right_back_motor
else:
self.rightBackMotor = MotorKit().motor4
def reload_conf(self):
with open(
'/home/ubuntu/catkin_ws/src/rover/src/rover/motor_configuration.json'
) as json_file:
self.conf = json.load(json_file)
print(self.conf)
self.kitRight = MotorKit(
int(self.conf["right_controller_board"]["i2c_address"], 16))
self.kitLeft = MotorKit(
int(self.conf["left_controller_board"]["i2c_address"], 16))
for motor_name in self.motor_names:
motor_conf = self.conf[motor_name]
motor = getattr(self, motor_name)
kit = self.kitLeft if motor_conf[
'left_controller'] else self.kitRight
motor.dcmotor = getattr(
kit, 'motor' + str(motor_conf['controller_motor_number']))
motor.wired_backwards = motor_conf['wired_backwards']
motor.arm = motor_conf['arm']
print("Configured", motor_name, "on",
motor_conf['controller_motor_number'], 'and it',
'is' if motor_conf['wired_backwards'] else 'is not',
'wired backwards')
def run(self):
print ("Connection from : ", clientAddress)
#self.csocket.send(bytes("Hi, This is from Server..",'utf-8'))
msg = ''
while True:
data = self.csocket.recv(2048)
msg = data.decode()
if msg=='start':
kit = MotorKit()
kit.motor1.throttle = 0.1
#print ("from client", msg)
#self.csocket.send(bytes(msg,'UTF-8'))
if msg=='stop':
kit = MotorKit()
kit.motor1.throttle = 0
if msg=='bye':
break
print ("from client", msg)
self.csocket.send(bytes(msg,'UTF-8'))
print ("Client at ", clientAddress , " disconnected...")
def controller():
#Instantiate the motorkit instance
kit = MotorKit()
#Initially start the motors at same speed so they are running straight
kit.motor1.throttle = 0.0 # set left motor speed to 0
kit.motor2.throttle = 0.0 # set right motor speed to 0
#Instantiate the error class to calculate things for us
error = Error() # set up the error varriable using the error class
#Loop for the feedback loop
try:
while True:
#Calculate the PID value
error.getOptics()
PID = error.calculatePID()
if (error.count == 22):
kit.motor1.throttle = 0.0 # if error = 22 stop the car
kit.motor2.throttle = 0.0 # if error = 22 stop the car
break
time.sleep(0.05) # sleep for 0.05 seconds
#summ the pid value with the base throttle of 0.75 to turn left or right based on imbalances in the throttle values
kit.motor1.throttle = 0.27 + PID #Assuming this is the left motor
kit.motor2.throttle = 0.27 - PID #Assuming this is the right motor
except KeyboardInterrupt:
# when we keyboard interupt stop the car
kit.motor1.throttle = 0.0 # set left motor to zero
kit.motor2.throttle = 0.0 # set right motor to zero
def __init__(self, inputMonitor):
Thread.__init__(self)
self.inputMap = inputMonitor["inputMap"]
self.readLock = inputMonitor["readLock"]
self.writeLock = inputMonitor["writeLock"]
self.inputMonitor = inputMonitor
self.RX = 0.0
self.RY = 0.0
self.pitch = 0.0
self.roll = 0.0
self.heading = 0.0
self.D_Up = 0
self.D_Down = 0
self.pid = PID()
self.imuReadLock = inputMonitor["imuReadLock"]
self.imuWriteLock = inputMonitor["imuWriteLock"]
self.imuData = inputMonitor["imuData"]
self.kit = MotorKit()
self.initMotorKits()
self.motor1 = None
self.motor2 = None
self.motor3 = None
self.motor4 = None
self.initMotors()
def valve_loop(breathing, peeping, oxp, start, start_time, top, top_time, down,
down_time, bottom, bottom_time, peep_steps, peep_step_time,
peep_wait, count):
i2c = rpi2c.rpi_i2c(1)
kit = MotorKit(i2c=i2c)
breather = Breather(kit.motor1, kit.motor2)
breather.set_cycle(start, start_time, top.value, top_time, down, down_time,
bottom)
for i in range(0, count):
# breath
breather.breath(breathing, top, oxp)
# peep
peep_cycle(breathing, peeping, peep_steps, peep_step_time, peep_wait)
# wait
sleep_time = 0.1
sleep_count = (int)(bottom_time / sleep_time)
for i in range(0, sleep_count):
if breathing.value == constants.INSPIRING:
break
time.sleep(sleep_time)
# cleanup
logging.warn("cleaning up please wait")
breather.cleanup(breathing, oxp)
peep.peep_cleanup()
logging.warn("done")
def __init__(self):
'''Initialize stepper motors and get references to limit switches'''
# Get references to stepper motors
self.kit = MotorKit()
self.stepperX = self.kit.stepper1
self.stepperY = self.kit.stepper2
# Release the steppers to start
self.stepperX.release()
self.stepperY.release()
# Get reference to limit buttons
self.limXMaxBut = Button(4, bounce_time=0.05)
self.limYMinBut = Button(5, bounce_time=0.05)
# Initialize position variables
self.posX = 0
self.posY = 0
# Axis length
self.maxX = 4000 # X-axis length
self.maxY = 4000 # Y-axis length
# Initialization interlock
self.is_initialized_x = False
self.is_initialized_y = False
def full_move(move_cmd,direction=0,running=1):
kit = MotorKit()
direction_map = [stepper.BACKWARD, stepper.FORWARD]
step_count = 100
status_array = []
sleep_between_steps = 0.01
if move_cmd == 1:
#set move_cmd state to true
status_array.append(1)
if running == 2:
print("Process already running!")
return status_array
if running == 1:
try:
#update direction state in output list
status_array.append(direction)
#show whether or not the process is running
status_array.append(2)
for i in range(step_count):
kit.stepper1.onestep(direction=direction_map[direction], style=stepper.DOUBLE)
time.sleep(sleep_between_steps)
return status_array
except:
print("error!")
return status_array
if move_cmd == 0:
# status_array = [0,0,0]
return status_array
return status_array
def __init__(self, debug):
self.debugMode = debug
self.kit = MotorKit()
self.mFSx = self.kit.motor2
self.mBSx = self.kit.motor1
self.mFDx = self.kit.motor4
self.mBDx = self.kit.motor3
def __init__(self):
try:
self.kit = MotorKit()
except NameError:
self.kit = None
self.forward = 0.0
self.rotate = 0.0
def __init__(self):
self.left_speed = 0
self.right_speed = 0
# self.engine_ctrl = MotorKit()
self.engine_ctrl = MotorKit(i2c=board.I2C())
self.lowest_throttle = self.DEFAULT_LOWEST_THROTTLE
self.joystick_control_strategy = JoystickControlStrategy()
def left():
kit = MotorKit()
kit.motor1.throttle = 0.8
kit.motor2.throttle = 0
time.sleep(0.45)
kit.motor1.throttle = 0
kit.motor2.throttle = 0
def __init__(self):
self.kit = MotorKit()
self.circle_wheel = 21.5
self.circle_encoder = 20
self.left_encoder = 20
self.right_encoder = 21
self.left_counter_encoder = 0
self.left_state_encoder = 0
self.right_counter_encoder = 0
self.right_state_encoder = 0
self.speedLeftWheel = 0
self.speedRightWheel = 0
self.error = 0
self.lasterror = 0
self.output = 0
self.sumerror = 0
self.kp = 2
self.ki = 0.0
self.kd = 0.0
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.left_encoder, GPIO.IN)
GPIO.setup(self.right_encoder, GPIO.IN)
self.millis = int(round(time.time() * 1000))
def get_engine():
kit = MotorKit()
motor1 = Motor(kit.motor1, 1)
motor2 = Motor(kit.motor2, -1)
motor3 = Motor(kit.motor3, -1)
motor4 = Motor(kit.motor4, 1)
return Engine(motor1, motor2, motor3, motor4)
def right():
kit = MotorKit()
kit.motor1.throttle = 0
kit.motor2.throttle = -0.8
time.sleep(0.45)
kit.motor1.throttle = 0
kit.motor2.throttle = 0
def __init__(self, motor, steps_per_rotation, step_delay, step_style, i2c):
self._kit = MotorKit(i2c=i2c)
self._stepper = getattr(self._kit, motor)
self._stepper.release()
self._steps_per_rotation = int(steps_per_rotation)
self._delay = step_delay
assert step_style in {stepper.SINGLE, stepper.DOUBLE} # Don't want to microstep this geared motor
self._style = step_style
def close(steps):
kit = MotorKit()
for i in range(steps):
msg = 'move no {}'.format(i)
print(msg)
kit.stepper1.onestep(direction=stepper.BACKWARD,
style=stepper.MICROSTEP)
time.sleep(0.01)
kit.stepper1.release()
def main():
RUNNING = True
pygame.init()
pygame.display.set_caption("Motor Test")
screen = pygame.display.set_mode(list(SCREEN_SIZE))
robot = MotorKit(address=0x60)
while RUNNING:
for event in pygame.event.get():
if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN
and event.key == pygame.K_ESCAPE):
RUNNING = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP:
#accelerateLinear(robot, "forward")
#accelerateExp(robot, "forward")
#accelerateLog(robot, "forward")
robot.motor1.throttle = 1.0
robot.motor2.throttle = 1.0
#robot.motor3.throttle = 0.8
#robot.motor4.throttle = 0.8
elif event.key == pygame.K_DOWN:
#accelerateLinear(robot, "backward")
#accelerateExp(robot, "backward")
#accelerateLog(robot, "backward")
robot.motor1.throttle = -1.0
robot.motor2.throttle = -1.0
#robot.motor3.throttle = -0.8
#robot.motor4.throttle = -0.8
elif event.key == pygame.K_RIGHT:
robot.motor1.throttle = 1.0
robot.motor2.throttle = -1.0
#robot.motor3.throttle = 0.8
#robot.motor4.throttle = 0.0
elif event.key == pygame.K_LEFT:
robot.motor1.throttle = -1.0
robot.motor2.throttle = 1.0
#robot.motor3.throttle = 0.0
#robot.motor4.throttle = 0.8
elif event.type == pygame.KEYUP:
if event.key == pygame.K_UP:
#decelerateLinear(robot, "forward")
#decelerateExp(robot, "forward")
#decelerateLog(robot, "forward")
stop(robot)
elif event.key == pygame.K_DOWN:
#decelerateLinear(robot, "backward")
#decelerateExp(robot, "backward")
#decelerateLog(robot, "backward")
stop(robot)
elif event.key == pygame.K_LEFT:
stop(robot)
elif event.key == pygame.K_RIGHT:
stop(robot)
pygame.quit()
def __init__(self):
self.kit = MotorKit()
self.kit.motor1.throttle = 0
self.kit.motor4.throttle = 0
# max speed needs to be at most 1
self.initSpeed = 0.5
self.maxSpeed = 1
self.incrSpeedStep = 0.2
tranTime = 0.1
holdTime = 1
def __init__(self):
# helper variables for max and mid speeds
# this is to prevent magic variables throughout the class
self.maxSpeed = 1.0
self.subSpeed = 0.25
# only create the MotorKit if we are running on the raspberry pi
if pi:
self.kit = MotorKit()
def setup(self):
logging.basicConfig(level=logging.INFO,
format='[%(asctime)s] :: %(message)s')
logging.info("Hello, Kran-o-tron!")
self.uuid = "CAFE"
self.pos = dict() # used to store info on servo positions
self.pos['height'] = Decimal('0.0')
self.pos['pitch'] = Decimal('0.0')
self.pos['yaw'] = Decimal('0.0')
self.pos['roll'] = Decimal('0.0')
self.pos['width'] = Decimal('0.0')
# define limits for the stepper motors
self.limits = dict()
self.limits['height'] = (Decimal('10000'), Decimal('10000')
) # todo check
self.limits['width'] = (Decimal('10000'), Decimal('10000')
) # todo check
self.limits['pitch'] = (Decimal('30'), Decimal('30')) # todo check
self.limits['yaw'] = (Decimal('30'), Decimal('30')) # todo check
self.limits['roll'] = (Decimal('30'), Decimal('30')) # todo check
# initiate the steppers
try:
print("Init motors...")
self.yaw_pitch_motors = MotorKit(address=0x60,
steppers_microsteps=4)
self.roll_pan_motors = MotorKit(address=0x61,
steppers_microsteps=4)
self.height_motors = MotorKit(address=0x62)
self.yaw_pitch_motors.stepper1.release()
self.yaw_pitch_motors.stepper2.release()
self.roll_pan_motors.stepper1.release()
self.roll_pan_motors.stepper2.release()
self.height_motors.stepper1.release()
self.height_motors.stepper2.release()
except Exception as e:
print(e)
print("Couldn't find the stepper hats! Check the wiring!")
self.broken_steppers = True
def __init__(self, debug=False):
self.kit = MotorKit()
self.base_url = "http://192.168.0.187:8080/"
self.scan_url = "{}/photo_save_only.jpg".format(self.base_url)
self.stepper_style = stepper.DOUBLE
self.debug = debug
self.breakbeam = digitalio.DigitalInOut(board.D21)
self.breakbeam.direction = digitalio.Direction.INPUT
self.breakbeam.pull = digitalio.Pull.UP
def __init__(self, motor_locs, init_x, init_y):
self.kit = MotorKit()
self.motor_locs = motor_locs
length1, length2 = self._calculate_lengths(init_x, init_y)
self.motor1 = Motor("motor1", self.kit.stepper1, self.motor_locs[0], length1)
self.motor2 = Motor("motor2", self.kit.stepper2, self.motor_locs[1], length2)
self.x, self.y = self.original_x, self.original_y = init_x, init_y
def __init__(self, inputMonitor):
Thread.__init__(self)
self.inputMap = inputMonitor["inputMap"]
self.readLock = inputMonitor["readLock"]
self.writeLock = inputMonitor["writeLock"]
self.inputMonitor = inputMonitor
self.localInputMap = dict()
self.kit = MotorKit()
self.kit1 = MotorKit(0x61)
self.initMotorKits()
self.motor1 = None
self.motor2 = None
self.motor3 = None
self.motor4 = None
self.motor5 = None
self.motor6 = None
self.initMotors()
def __init__(self, logger):
self.logger = logger
self.kit = MotorKit()
self.motors = {
"lf": self.kit.motor3,
"rf": self.kit.motor4,
"lr": self.kit.motor1,
"rr": self.kit.motor2,
}
self.stop()
def __init__(self):
try:
kit == None
except UnboundLocalError:
#need to only load and construct if actually using
#this prevents errors when running not on the pi
from adafruit_motorkit import MotorKit
kit = MotorKit()
self.motors = [kit.motor1, kit.motor2, kit.motor3, kit.motor4]
self.stop()
def __init__(self):
self.right = 0.3
self.left = 0.3
self.sub = rospy.Subscriber("odom",
Odometry,
self.callback,
queue_size=1)
self.kit = MotorKit(i2c=board.I2C())
self.oldright = 0.1
self.oldleft = 0.3
self.num = 0
def flow_calibrate(bus, percent, count):
print("runing calibration on bus %d" % (bus,))
# setup pressure sensors and zero pressure them
in_p1 = PressureSensorLPS(rpi2c.rpi_i2c(1), address=0x5d)
in_p2 = PressureSensorLPS(rpi2c.rpi_i2c(1), address=0x5c)
ex_p1 = PressureSensorLPS(rpi2c.rpi_i2c(3), address=0x5d)
ex_p2 = PressureSensorLPS(rpi2c.rpi_i2c(3), address=0x5c)
sensor.pressure_zero(in_p1, in_p2, ex_p1, ex_p2)
flow = FlowSensorD6F(rpi2c.rpi_i2c(1))
kit = MotorKit(i2c=rpi2c.rpi_i2c(1))
breather = Breather(kit.motor1, kit.motor2)
ox = Value('i', 0)
if bus == 1:
p1 = in_p1
p2 = in_p2
else:
p1 = ex_p1
p2 = ex_p2
# open valve
breather.throttle(percent, ox)
total = 0
samples = 0
vmin = 100
vmax = 0
THRESH = 50
logging.warning("Start calibration")
for i in range(0,count):
p1.read()
p2.read()
flow.read()
r = flow.data.rate
hp = p2.data.pressure
lp = p1.data.pressure
if hp > (lp + THRESH):
vco = r / ((hp-lp)**0.5)
total += vco
samples += 1
vmin = min(vco, vmin)
vmax = max(vco, vmax)
logging.warning("%f %f %f %f" % (r, vco, hp, lp))
time.sleep(0.1)
# close valve
logging.warning("VCO: %f min %f max %f" % (total/samples, vmin, vmax))
breather.throttle(0, ox)
def test1():
kit = MotorKit()
print('onestep')
time.sleep(1)
kit.stepper1.onestep()
print('next step')
time.sleep(2)
kit.stepper1.onestep()
print('3rd step')
time.sleep(1)
kit.stepper1.onestep(direction=stepper.BACKWARD, style=stepper.DOUBLE)
print('release')
time.sleep(1)
kit.stepper1.release()
def feeder_thread(q1):
kit = MotorKit()
while (True):
if not q1.empty():
msg = q1.get()
if (msg == "feed"):
for i in range(50):
kit.stepper1.onestep(style=stepper.DOUBLE)
time.sleep(0.1)
kit.stepper1.release()
time.sleep(0.01)