def __init__(self): self.var = 0 self.finalPos = 0 self.finalType = "" self.initialPos = 0 self.initialType = "" self.capture = False self.MC = MotorControl() self.EE = EndEffectorControl() self.import_chessboard_dic(21) self.EE.set_elevation("top")
def MotorControl(*args, **kwargs): try: from MotorControl import MotorControl as MotorControl return MotorControl(*args, **kwargs) except Exception as e: print "Failed to Initialize Hardware Motor Control (I2C PWM Device)" print "Error: %s" % e.message print "Using Mock Motor Control" from MotorControl_Mock import MotorControl as MotorControl_Mock return MotorControl_Mock()
from MotorControl import MotorControl import time motors = MotorControl(526) # for i in range(5): # motors.initialize_motor(i) motors.initialize_all() #motors.run_motor(0, 55) #time.sleep(2) #motors.stop_all()
from MotorControl import MotorControl import time mc = MotorControl() right_address = 128 left_address = 129 front_address = 130 controller_addresses = mc.list_controllers() print(controller_addresses) #if not (controller_addresses.__contains__(right_address) and controller_addresses.__contains__(left_address) and controller_addresses.__contains__(front_address)): #exit() time.sleep(5) mc.drive_both(right_address, 1) mc.drive_both(left_address, 1) #mc.drive_M2(front_address, 1) time.sleep(5) mc.drive_both(right_address, -1) mc.drive_both(left_address, -1) #mc.drive_M2(front_address, -1) time.sleep(5) mc.drive_both(right_address, 0) mc.drive_both(left_address, 0) #mc.drive_M2(front_address, 0)
def __init__(self, ser): self.motor_r = MotorControl(0, ser) self.motor_l = MotorControl(1, ser) self.R = 0.115 self.l = 0.5081
class DifControl(): def __init__(self, ser): self.motor_r = MotorControl(0, ser) self.motor_l = MotorControl(1, ser) self.R = 0.115 self.l = 0.5081 def set_speed(self, vX, wZ): #meteres per second / radians per second v_r = -1*(2*vX - self.l*wZ)/(2*self.R) v_l = (2*vX + self.l*wZ)/(2*self.R) # print("v_r:"+str(v_r)+" v_l:"+str(v_l)) self.motor_r.goal_velocity(v_r) self.motor_l.goal_velocity(v_l) def get_motors_speed(self): self.motor_r.get_status() self.motor_l.get_status() if self.motor_r.direction_of_turn == '0': v_r = self.motor_r.motor_speed else: v_r = -self.motor_r.motor_speed if self.motor_l.direction_of_turn == '1': v_l = self.motor_l.motor_speed else: v_l = -self.motor_l.motor_speed return v_r, v_l def init_motors(self, holl, acc, br): self.motor_r.holl_impulse(holl) self.motor_r.sef_acceleration(acc) self.motor_r.set_brake(br) self.motor_r.turn_on() self.motor_l.holl_impulse(holl) self.motor_l.sef_acceleration(acc) self.motor_l.set_brake(br) self.motor_l.turn_on()
def testReverse(self): motorIO = MotorIOStub() motorControl = MotorControl(motorIO) motorControl.reverse() self.assertTrue(motorIO.getValues()[0] == 'reverse')
def __init__(self): self.engine = MotorControl() self.sensors = Sensors()
def __init__(self,L_F_GPIO,L_B_GPIO,R_F_GPIO,R_B_GPIO): self.motorControl = MotorControl(L_F_GPIO,L_B_GPIO,R_F_GPIO,R_B_GPIO) self.operation = None
class MasterControl: motorControl = None operation = None #eventually I will read this from a config def __init__(self,L_F_GPIO,L_B_GPIO,R_F_GPIO,R_B_GPIO): self.motorControl = MotorControl(L_F_GPIO,L_B_GPIO,R_F_GPIO,R_B_GPIO) self.operation = None def forward(self): if(self.operation != "forward"): self.motorControl.setLeftForwardHigh() self.motorControl.setRightForwardHigh() self.operation = "forward" print "Operation set to Forward" else: print "Operation is already set to Forward" def backward(self): if(self.operation != "backward"): self.motorControl.setLeftBackwardHigh() self.motorControl.setRightBackwardHigh() self.operation = "backward" print "Operation set to Backward" else: print "Operation is already set to Backward" def turnLeft(self): if(self.operation != "left"): self.motorControl.setLeftBackwardHigh() self.motorControl.setRightForwardHigh() self.operation = "left" print "Operation set to Turn Left" else: print "Operation is already set to Turn Left" def turnRight(self): if(self.operation != "right"): self.motorControl.setLeftForwardHigh() self.motorControl.setRightBackwardHigh() self.operation = "right" print "Operation set to Turn Right" else: print "Operation is already set to Turn Right" def stop(self): if(self.operation != "stop"): self.motorControl.setLeftForwardLow() self.motorControl.setRightForwardLow() self.motorControl.setLeftBackwardLow() self.motorControl.setRightBackwardLow() self.operation = "stop" print "Operation set to Stop" else: print "Operation is already set to Stop" def exit(self): self.motorControl.cleanUp()
from MotorControl import MotorControl from RemoteController import RemoteController from time import sleep motor = MotorControl(0, 17, 1, 27) motor.armMotors() cont = RemoteController() cont.startListening() while True: lx, ly = cont.getLeftJoystick() #print(lx, ly) #motor.setMotorSpeeds(70,70) #sleep(5) #motor.setMotorSpeeds(-70, -70) #sleep(5) #motor.setMotorSpeeds(0, 0) #sleep(5)
def testStopMotor(self): motorIO = MotorIOStub() motorControl = MotorControl(motorIO) motorControl.stopMotor() self.assertTrue(motorIO.getValues()[0] == 'stopMotor')
def testTurnRight(self): motorIO = MotorIOStub() motorControl = MotorControl(motorIO) motorControl.turnRight() self.assertTrue(motorIO.getValues()[0] == 'turnRight')
from MiniMu9 import Accelerometer from MotorControl import MotorControl import sys import time PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS) servo = PWM.Servo(pulse_incr_us=1) PWM_MAX = 2000 PWM_MIN = 1000 PWM_RANGE = PWM_MAX - PWM_MIN pid = VideoPID(setpoint=0, kP=2.5, kI=0.2, kD=0.5, zeros=False) accel = Accelerometer() controller = MotorControl(pins=[25]) controller.begin_calibration() print("Press [RETURN] once you have connected power to the motors.") raw_input() controller.continue_calibration() print("Motors Calibrated. Beginning PID Loop") motor_weight_offset = 27 # Percentage power at which the motor balances. print("WAIT") raw_input() while True: # Get Reading current = accel.readX()
""" Determine the raw speed percentage required to actually balance a propellor's weight against gravity (helping PID scaling) """ from MotorControl import MotorControl controller = MotorControl(pins=[25], debug=True) controller.begin_calibration() raw_input("Press 'Return' once you have connected power to the motors") controller.continue_calibration() print("Ready to calibrate motor weight") terminated = False while terminated == False: data = raw_input("Enter new speed [0-100%%] or [q] to quit: ") if data == 'q': terminated = True break elif int(data) >= 0 and int(data) <= 100: print("Setting speed to {}".format(data)) speed = int(data) controller.set_motor(0, speed) controller.cleanup()
from RemoteController import RemoteController from MotorControl import MotorControl cont = RemoteController() motor = MotorControl(0, 17, 1, 27) motor.armMotors() cont.startListening() while True: lx, ly = cont.getLeftJoystick() boost = 1 if (6 in cont.buttons): boost = 0.25 elif (7 in cont.buttons): boost = 2 r, l = motor.controllerToMotor(-lx, -ly) motor.setMotorSpeeds(r * boost, l * boost)
class MasterControl: motorControl = None operation = None #eventually I will read this from a config def __init__(self, L_F_GPIO, L_B_GPIO, R_F_GPIO, R_B_GPIO): self.motorControl = MotorControl(L_F_GPIO, L_B_GPIO, R_F_GPIO, R_B_GPIO) self.operation = None def forward(self): if (self.operation != "forward"): self.motorControl.setLeftForwardHigh() self.motorControl.setRightForwardHigh() self.operation = "forward" print "Operation set to Forward" else: print "Operation is already set to Forward" def backward(self): if (self.operation != "backward"): self.motorControl.setLeftBackwardHigh() self.motorControl.setRightBackwardHigh() self.operation = "backward" print "Operation set to Backward" else: print "Operation is already set to Backward" def turnLeft(self): if (self.operation != "left"): self.motorControl.setLeftBackwardHigh() self.motorControl.setRightForwardHigh() self.operation = "left" print "Operation set to Turn Left" else: print "Operation is already set to Turn Left" def turnRight(self): if (self.operation != "right"): self.motorControl.setLeftForwardHigh() self.motorControl.setRightBackwardHigh() self.operation = "right" print "Operation set to Turn Right" else: print "Operation is already set to Turn Right" def stop(self): if (self.operation != "stop"): self.motorControl.setLeftForwardLow() self.motorControl.setRightForwardLow() self.motorControl.setLeftBackwardLow() self.motorControl.setRightBackwardLow() self.operation = "stop" print "Operation set to Stop" else: print "Operation is already set to Stop" def exit(self): self.motorControl.cleanUp()
class RobotControl: def __init__(self): self.var = 0 self.finalPos = 0 self.finalType = "" self.initialPos = 0 self.initialType = "" self.capture = False self.MC = MotorControl() self.EE = EndEffectorControl() self.import_chessboard_dic(21) self.EE.set_elevation("top") # return home #self.MC.move_arm_home() def import_chessboard_dic(self,chessboard_in): self.chessboardDictionary = chessboard_in # return the class of piece occupying the square def get_square_type(self, indexKey): typeCode = self.chessboardDictionary.get(indexKey) switcher = { 6: "empty", 0:"bishop", 1: "king", 2: "knight", 3: "pawn", 4:"queen", 5: "rooke", 7:"bishop", 8: "king", 9: "knight", 10: "pawn", 11:"queen", 12: "rooke" } return (switcher[typeCode]) ## self.chessboardDictionary.get(indexKey) ## 0 blue bishop ## 1 blue kning ## 2 blue knight ## 3 blue pawn ## 4 blue queen ## 5 blue rooke ## 6 nothing ## 7 yellow bishop ## 8 ## 9 ## 10 ## 11 ## 12 yellow rooke # command to robot to mave a chess move from square arg1 to arg2 (a1,h8) def move_command(self, chessEngine_in): ts1 = time.time() # return home self.MC.move_arm_home() self.initialPos = chessEngine_in[0] self.initialType = "queen" #self.get_square_type(self.initialPos) self.finalPos = chessEngine_in[1] self.finalType = "pawn"#self.get_square_type(self.finalPos) if (self.finalType == "empty") : self.capture = False else : self.capture = True # chek if a piece needs to be captured if self.capture == True: print ("Chess Capture: ",self.finalType,"(",self.finalPos,")") #get final pos coord goCoord = p.get_square_coord(self.finalPos) #move to final position self.MC.move_robot_arm(goCoord[0],goCoord[1]) #pick up piece self.EE.set_elevation(self.finalType) #engage magnet self.EE.set_magnet(1) #pick up piece self.EE.set_elevation("top") #move to drop off self.MC.move_robot_arm(250,250) #drop piece self.EE.set_elevation(self.finalType) #disengage magnet self.EE.set_magnet(0) #raise ee self.EE.set_elevation("top") print("Chess Move: ",self.initialType," (",self.initialPos,") to (",self.finalPos,")") #get initial pos coord goCoord = p.get_square_coord(self.initialPos) #move to initial position self.MC.move_robot_arm(goCoord[0],goCoord[1]) #pick up piece self.EE.set_elevation(self.initialType) #engage magnet self.EE.set_magnet(1) #pick up piece self.EE.set_elevation("top") #get final pos coord goCoord = p.get_square_coord(self.finalPos) #move to final position self.MC.move_robot_arm(goCoord[0],goCoord[1]) #drop piece self.EE.set_elevation(self.initialType) #disengage magnet self.EE.set_magnet(0) #return self.EE.set_elevation("top") # return home self.MC.move_arm_home() ts2 = time.time() moveTime = float (ts2 - ts1) print("Chess move time: ",moveTime)
def __init__(self, L_F_GPIO, L_B_GPIO, R_F_GPIO, R_B_GPIO): self.motorControl = MotorControl(L_F_GPIO, L_B_GPIO, R_F_GPIO, R_B_GPIO) self.operation = None
#!/usr/bin/env python # -*- coding: utf-8 -*- import struct from binascii import hexlify from codecs import encode from time import sleep import serial from math import pi from MotorControl import MotorControl if __name__ == '__main__': ser = serial.Serial('/dev/ttyUSB0', timeout=1.0) motor_1 = MotorControl(0, ser) motor_2 = MotorControl(1, ser) motor_1.holl_impulse(8) motor_1.sef_acceleration(20) motor_1.set_brake(20) motor_1.set_direction(1) motor_1.turn_on() motor_2.holl_impulse(8) motor_2.sef_acceleration(20) motor_2.set_brake(20) motor_2.set_direction(1) motor_2.turn_on() while True: # controller.goal_velocity(0.5) motor_1.goal_velocity(0) motor_2.goal_velocity(0) # motor_1.set_speed(0)
def testForward(self): motorIO = MotorIOStub() motorControl = MotorControl(motorIO) motorControl.forward() self.assertTrue(motorIO.getValues()[0] == 'forward')