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