def grabcan():
set_servo_position(c.clawservo, 1775)
msleep(1000)
m.drive(10,50,400)
set_servo_position(c.clawservo, 800)
set_servo_position(c.armservo, 390)
msleep(1000)
wait_for_button()
def lineFollowToCan():
m.drive(100, 100, 100)
#follow line until It gets close to the can
dist = analog(c.etPort)
while(dist<2600):
dist=analog(c.etPort)
print analog(c.thport)
m.linefollow()
ao()
def seeStop():
while FrontSensor.getLastEvent() >= 3000: #3000 millimeters
print(FrontSensor.getLastEvent())
Motors.forward()
if FrontSensor.getLastEvent() <= 3000:
Motors.still()
os.system("flite -t 'Bravo Task ended'")
FrontSensor.stop()
break
def seeAvoid():
if FrontSensor.getLastEvent() <= 3000:
Servo.turn_right()
time.sleep(.5)
Servo.turn_left()
time.sleep(.5)
Motors.still()
Servo.cleanup()
Motors.stop()
FrontSensor.stop()
break
def main():
global piezo
global zMotor, zaberMotors
zaberMotors = Motors.ZaberMotor()
zMotor = Motors.ThorLabMotor(49853845, 43)
zMotor.goHome()
app = QtGui.QApplication(sys.argv)
app.setApplicationName("Motors control")
ex = MotorGUI()
app.exec_()
def updateMovement(self):
if (abs(self.position.x - self.home.x) > 5):
self.moveToX(self.home.x, "Searching", tolerance=5)
self.phase = "X_Positioning"
elif (abs(self.rotation) > 5):
Motors.rotate(self.motorSerial, -1 * self.rotation / 2, self.speed)
self.phase = "Rotating"
elif (abs(self.position.y - self.home.y) > 5):
self.moveToY(self.home.y, "Seraching", tolerance=5)
self.phase = "Y_Positioning"
else:
self.phase = "Home"
def initializeMotors(self):
# Motors
self.leftMotor = Motors.Motor(self.leftMotorPin1, self.leftMotorPin2,
self.leftMotorEnablePin, 200, "forward",
0, 1)
self.leftMotor.startMotor()
self.leftMotor.setDirection()
self.leftMotor.setSpeed()
self.rightMotor = Motors.Motor(self.rightMotorPin1,
self.rightMotorPin2,
self.rightMotorEnablePin, 200,
"forward", 0, 1)
self.rightMotor.startMotor()
self.rightMotor.setDirection()
self.rightMotor.setSpeed()
def __init__(self):
rospy.init_node('IO_Interface', anonymous=False)
# pub = rospy.Publisher('IO_Interface/', String, queue_size=10)
#create motors
self.motor_1 = Motors.Motor(motorID='t1', pin = 1, pwm_output = 0)
self.motor_2 = Motors.Motor(motorID='t2', pin = 2, pwm_output = 0)
self.motor_3 = Motors.Motor(motorID='t3', pin = 3, pwm_output = 0)
self.motor_4 = Motors.Motor(motorID='t4', pin = 4, pwm_output = 0)
#create inu sensors
self.mpu = IMU.Inertia_Measurement_Unit('mpu')
self.lsm = IMU.Inertia_Measurement_Unit('lsm')
#create led output
self.led = Led.Navio2_LED()
def __init__(self):
global k, pd, pid, comb_pid, max_pwm, max_theta, max_x, frequency, pwm_offset, arduino_port, display_camera_output, initialize_theta_set
#Cart variables
self.max_pwm = max_pwm
self.max_theta = max_theta
self.max_x = max_x
self.frequency = frequency
self.pwm_offset = pwm_offset
self.arduino_port = arduino_port
#Booleans
self.display_camera_output = display_camera_output
self.initialize_theta_set = initialize_theta_set
#Motor, Controller, and Camera Instance Definition
self.motors = Motors.Motors(max_pwm = self.max_pwm, frequency=self.frequency, arduino_port=self.arduino_port)
self.controller = Controller.Controller(max_theta = self.max_theta, max_x = self.max_x)
self.camera = Camera.Camera(self.display_camera_output)
#Controller Vectors
self.k = [-10.0000/1.5, -29.9836/1.5, 822.2578, 85.5362]
self.pd = pd
self.pid = pid
self.comb_pid = comb_pid
self.status = 1
def main():
a.init()
a.lineFollowToCan()
a.wait_for_button()
a.grabcan()
a.wait_for_button()
for x in range(0, 4):
m.drive(100, 100, 1000)
m.drive(100, 0, 980)
set_servo_position(c.clawservo,
2047) #opens up claw all the way to grab can
msleep(1000)
# Stop
ao()
msleep(1000)
def updateMovement(self):
if(self.phase == "Wait"):
pass;
if(self.phase == "MoveToPlant"):
plant = self.plantPositions[self.plantIndex];
if(abs(self.position.y - plant.y) > 2):
self.moveToY(plant.y, "MoveToPlant", tolerance=2);
elif(abs(self.position.x - plant.x) > 2):
self.moveToX(plant.x, "PickupPlant", tolerance=2);
else:
self.phase = "PickupPlant";
if(self.phase == "PickupPlant"):
Motors.stopMotors(self.motorSerial);
Motors.writeSerialString(self.motorSerial, "U");
time.sleep(1);
self.plantIndex = self.plantIndex + 1;
if(self.plantIndex > len(self.plantPositions)):
self.phase = "Wait";
else:
self.phase = "MoveToZone";
if(self.phase == "MoveToZone"):
zone = self.zonePositions[self.zoneIndex];
if(abs(self.position.y - zone.y) > 2):
self.moveToY(zone.y, "MoveToZone", tolerance=2);
elif(abs(self.position.x - zone.x) > 2):
self.moveToX(zone.x, "DropoffPlant", tolerance=2);
else:
self.phase = "DropoffPlant";
if(self.phase == "DropoffPlant"):
Motors.stopMotors(self.motorSerial);
Motors.writeSerialString(self.motorSerial, "D");
time.sleep(1);
self.zoneIndex = self.zoneIndex + 1;
if(self.zoneIndex > len(self.zonePositions)):
self.phase = "Wait";
else:
self.phase = "MoveToPlant";
if(abs(self.rotation) > 3):
Motors.rotate(self.motorSerial, -1 * self.rotation / 2, self.speed);
def updateMovement(self):
if (self.phase == "First"):
if (abs(self.position.y - self.first.y) > 2):
self.moveToY(self.first.y, "First", tolerance=2)
elif (abs(self.position.x - self.first.x) > 5):
self.moveToX(self.first.x, "Second", tolerance=5)
else:
self.phase = "Second"
if (self.phase == "Second"):
if (abs(self.position.y - self.second.y) > 2):
self.moveToY(self.second.y, "Second", tolerance=2)
elif (abs(self.position.x - self.second.x) > 5):
self.moveToX(self.second.x, "First", tolerance=5)
else:
self.phase = "First"
if (abs(self.rotation) > 3):
Motors.rotate(self.motorSerial, -1 * self.rotation / 2, self.speed)
def __init__(self):
"""
:type maxParts: NumberOfPart[]
:type currParts: NumberOfPart[]
:type location: Location
"""
self.location = Location.Location()
self.maxParts = []
self.currParts = []
self.motors = Motors.Motors()
self.name = 0
def turnAround(): Motors.still() Servo.turn_right() Motors.forward() time.sleep(6) Motors.still() Servo.center()
def turnLeft(): Motors.still() Servo.turn_left() Motors.forward() time.sleep(3) Motors.still() Servo.center()
def right():
Motors.still()
Servo.turn_right()
Motors.forward()
time.sleep(3)
Motors.still()
Servo.center()
def drive_relative(x, y, robot):
'''
Drive relative to the robot's coordinate frame
inputs: x, y
'''
finished = False
#have we reached our goal?
if not (((x - 0.05) <= robot.x[0] <=
(x + 0.05)) and ((y - 0.05) <= robot.x[1] <= (y + 0.05))):
#We shouldn't need to contain this angle as it is simply for a driving system and PID
heading = (np.arctan2(y - robot.x[1],
x - robot.x[0])) - robot.x[2] #in radians
# the output of our pid represents a signed number depending on the direction we are turning
pid_return = abs(robot.PID.update(
heading)) #abs so that our motors dont travel in reverse direction
#limit to robots maximum angular velocity
if pid_return > robot.std_steer:
pid_return = robot.std_steer
pid_wheel = int(
robot.std_vel -
pid_return) #integer as our motor function only accepts an integer
#basic drive loop
if heading > 0.0:
#pass
Motors.driveMotors(pid_wheel, robot.std_vel)
if heading <= 0.0:
#pass
Motors.driveMotors(robot.std_vel, pid_wheel)
else:
Motors.driveMotors(0, 0)
return True
return False
def setup():
global SerialPort1
global SerialPort2
global SerialPort3
global LeftSensor
global CenterSensor
global RightSensor
SerialPort1 = '/dev/ttyUSB0'
SerialPort2 = '/dev/ttyUSB1' # run ls/dev/tty* to see which usb ports it is connected to.
SerialPort3 = '/dev/ttyUSB2'
LeftSensor = Ultrasonic.MySensor(SerialPort1)
CenterSensor = Ultrasonic.MySensor(SerialPort2)
RightSensor = Ultrasonic.MySensor(SerialPort3)
LeftSensor.start()
CenterSensor.start()
RightSensor.start()
Motors.still()
os.system("flite -t 'Delta Task started'")
def main():
try:
dataLog = DataLog.DataLog(logDir = 'logs/')
dataLog.updateLog({'velocity_units':velocityUnits})
encoders = Encoders.Encoders(countsPerRevolution = countsPerRevolution, units = velocityUnits)
#time.sleep(2) #allow encoder time to attach
motors = Motors.Motors(i2c_bus_num,nmotors,min_throttle_percentage, max_throttle_percentage, min_throttle_pulse_width, max_throttle_pulse_width)
#start motor communication
motors.setPWMfreq(PWM_frequency)
motors.motor_startup()
for i in xrange(3):
motors.setPWM(i, 0)
freq = 1/2.0
dataLog.updateLog({'start_time': time.time()})
while True:
#get measured velocity array
#calculate commanded velocities array
#convert commanded velocities to throttle
#send commanded PWM signal to motors
loop_start_time = time.time()
count_array = encoders.returnCountArray()
measured_velocities = encoders.getVelocities()
print measured_velocities
command_time = time.time()
commanded_throttles = [wave(100,2*pi/nmotors*pwmNum, freq, command_time) for pwmNum in xrange(nmotors)]
for pwmNum, throttle in enumerate(commanded_throttles):
motors.setPWM(pwmNum,throttle)
loop_end_time = time.time()
#write information to logs
log_info = {
'counts': dict(zip(['time', 0, 1, 2], count_array)),
'commanded_velocity': dict(zip(['time', 0, 1, 2],[command_time] + map(actuatorVelocityModel, commanded_throttles))),
'commanded_throttle': dict(zip(['time', 0, 1, 2],[command_time] + commanded_throttles)),
'measured_velocity': dict(zip(['time', 0, 1, 2], measured_velocities)),
'iteration_latency': loop_end_time - loop_start_time,
'command_latency': loop_end_time - command_time,
'measurement_to_command_latency': command_time - measured_velocities[0]
}
dataLog.updateLog(log_info)
except KeyboardInterrupt:
for i in xrange(3):
motors.setPWM(i, 0)
dataLog.saveLog(baseName = 'Closed_Loop_Test')
def moveToY(self, yPosition, exit, tolerance=2):
if(abs(self.position.y - yPosition) > tolerance):
if(self.position.y - yPosition < 0):
Motors.setDirectionPosition(self.motorSerial, "RIGHT", abs(self.position.y - yPosition), self.speed);
elif(self.position.y - yPosition > 0):
Motors.setDirectionPosition(self.motorSerial, "LEFT", abs(self.position.y - yPosition), self.speed);
else:
Motors.stopMotors(self.motorSerial);
time.sleep(0.1);
self.phase = exit;
def moveToX(self, xPosition, exit, tolerance=2):
if(abs(self.position.x - xPosition) > tolerance):
if(self.position.x - xPosition > 0):
self.xDirection = -1;
Motors.setMotorSpeed(self.motorSerial, self.frSpeed, self.brSpeed, -1 * self.blSpeed, -1 * self.flSpeed);
elif(self.position.y - xPosition < 0):
self.xDirection = 1;
Motors.setMotorSpeed(self.motorSerial, -1 * self.frSpeed, -1 * self.brSpeed, self.blSpeed, self.flSpeed);
else:
Motors.stopMotors(self.motorSerial);
time.sleep(0.1);
self.phase = exit;
def main():
os.system("flite -t 'Alpha Task started'")
time.sleep(5) #Wait 5 seconds
Motors.forward()
time.sleep(2)
Servo.turn_right()
time.sleep(2)
Servo.center()
Motors.still()
Servo.cleanup()
Motors.stop()
os.system("flite -t 'Alpha Task ended'")
def moveToX(self, xPosition, exit, tolerance=3):
if (abs(self.position.x - xPosition) > tolerance):
if (self.position.x - xPosition > 0):
Motors.setDirectionPosition(self.motorSerial, "BACKWARD",
abs(self.position.x - xPosition),
self.speed)
elif (self.position.x - xPosition < 0):
Motors.setDirectionPosition(self.motorSerial, "FORWARD",
abs(self.position.x - xPosition),
self.speed)
else:
Motors.stopMotors(self.motorSerial)
time.sleep(0.1)
self.phase = exit
def __init__(self, *arg, **kw): cfg = dict() JoyApp.__init__(self, cfg=cfg, *arg, **kw) self.arm = Arm(); self.f = gcf() self.f.clf() self.ax = self.f.gca(projection='3d') motors = [] motors.append(self.robot.at.Nx02) motors.append(self.robot.at.Nx08) motors.append(self.robot.at.Nx04) motors.append(self.robot.at.Nx06) self.motors = Motors(motors) self.orientation = PaperOrientation.HORIZONTAL self.angles1 = None self.angles2 = None self.coordinates = Coordinates() self.calibrateIdx = -1 self.calibrationPoints = [] self.pos3d = None self.timeForPlot = self.onceEvery(1.0/3.0) self.moveToPointPlan = MoveToPointPlan(self) self.drawLinePlan = DrawLinePlan(self) self.drawSquarePlan = DrawSquarePlan(self) self.multipleLinePlan = MultipleLinePlan(self) self.moveToPointPlan.setPaperOrientation(self.orientation) self.startPoints = [[5, -10], [10, -15]] self.endPoints = [[18, -18], [1, -2]] self.lines = [ [(18,10), (10,22)], [(18,35), (27,24)], [(19,10), (27,25)], [(18,35), (10,22)], [(29,37), (30,11)], [(42,36), (42,12)], [(29,12), (41,36)], [(14,46), (13,70)], [(31,45), (20,58)], [(22,58), (32,70)], [(45,46), (33,54)], [(38,70), (49,61)], [(34,55), (49,62)] ]
def launchMouse():
time.sleep(1.0)
# Step 1 - Start Motors
print("Step 1 - Motors Starting")
Motors.launchMotorsOn()
# Step 2 - Launch Mouse
print("Step 2 - Mouse Launch Servo Starting")
Motors.launchServoStart()
# Step 3 - Stop Motors
print("Step 3 - Motors Stopping")
Motors.launchMotorsOff()
# Step 4 - Launch Mouse
print("Step 4 - Mouse Launch Servo Starting")
Motors.launchServoRetract()
print("Step 5- Ready for Next Launch")
def stop(self):
Motors.stopMotors(self.motorSerial)
def reorient(self, exit):
Motors.rotate(self.motorSerial, -1 * self.rotation, self.speed)
time.sleep(0.1)
self.phase = exit
import time # Does not execute this command when launched from terminal
from commands import *
from ClockAideTime import *
from questionBank import QuestionBank
import Keypad
import Motors
from DB import *
Keypad = Keypad.keypad()
motor = Motors.motors()
qBank = QuestionBank(databaseLocation)
clockAideDB = DB("./ClockAideDB")
def set(id, sessionActive):
qBank.generateTime()
setTime = setModeTime(id)
numAttempts = 0
while (sessionActive and (numAttempts < 3)):
keypad.SendLine(modeLookUp["set"])
motor.SendLine(modeLookUp["set"])
time.sleep(2)
keypad.SendLine(setTime)
randomTime = qBank.getTimeTouple()
speakTime(randomTime[0], randomTime[1])
comm = COMMAND[str(keypad.read())]
print("Pixy2 Python SWIG Example -- Get Blocks")
pixy.init()
pixy.change_prog("color_connected_components")
class Blocks(Structure):
_fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint),
("m_width", c_uint), ("m_height", c_uint), ("m_angle", c_uint),
("m_index", c_uint), ("m_age", c_uint)]
blocks = BlockArray(100)
frame = 0
Motors.still()
while 1:
Motors.forward()
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
print('frame %3d:' % (frame))
frame = frame + 1
for index in range(0, count):
#print '[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].m_signature, blocks[index].m_x, blocks[index].m_y, blocks[index].m_width, blocks[index].m_height)
if blocks[index].m_x >= 200:
print("right")
Servo.turn_right()
elif blocks[index].m_x <= 100:
print("left")
Servo.turn_left()
def testClawOpenWidth():
testMotors = Motors.Motors()
testMotors.openClawWidth(testMotors.CWL.getWidth(5))
assert testMotors.CWL.getWidth(5) == 40
import time # Does not execute this command when launched from terminal
from commands import *
from ClockAideTime import *
from questionBank import QuestionBank
import Keypad
import Motors
from DB import *
keypad = Keypad.keypad()
motor = Motors.motors()
qBank = QuestionBank(databaseLocation)
clockAideDB = DB("./ClockAideDB")
def read(id, sessionActive):
qBank.generateTime()
readTime = readModeTime(id)
numAttempts = 0
#answerRight =
#while sessionActive && answerWrong:
while (sessionActive and (numAttempts < 3)):
keypad.SendLine(modeLookUp["read"])
motor.SendLine(modeLookUp["read"])
time.sleep(2)
motor.SendLine(readTime) ## send time to be displayed to motor
#randomTime = qBank.getTimeTouple()
#speakTime(randomTime[0],randomTime[1])
keypadTime = keypad.ReadLine() ## include some timeout logic
def onExitHousekeeping(): GPIO.cleanup() Motors.releaseAllMotors()
#!/usr/bin/python import time import atexit import RPi.GPIO as GPIO #import Sensors import Motors #GPIO.setmode(GPIO.BOARD) # Needed for reading from range sensors def onExitHousekeeping(): # GPIO.cleanup() Motors.releaseAllMotors() atexit.register(onExitHousekeeping) TestSpeed = 150 TestSleep = 1.65 if __name__ == '__main__': # BEGIN MAIN PROGRAM while(1): # goFrwrd(arg) where arg = speed # 0 <= speed <= 255 Motors.goFrwrd( TestSpeed ) time.sleep( TestSleep ) Motors.goTurnR( TestSpeed ) time.sleep( TestSleep ) #Motors.goBckwrd(TestSpeed) #time.sleep(2)
#gets the current date and time
measure_date = datetime.now()
#adding the sensor values to the database
plant1_humidityData = (1, measure_date, plant1_humidityValue)
plant2_humidityData = (2, measure_date, plant2_humidityValue)
Database.cursor.execute(Database.add_humidity, plant1_humidityData)
Database.cursor.execute(Database.add_humidity, plant2_humidityData)
#Database.cnx.commit()
# gets the soil humidity of each plant and compares it
# to the threshold value
if plant1_humidityValue < plant1_threshold:
mo.valve1_on()
mo.pump_on()
pumpOn = True
else:
mo.valve1_off()
if plant2_humidityValue < plant2_threshold:
mo.valve2_on()
mo.pump_on()
pumpOn = True
else:
mo.valve2_off()
if plant1_humidityValue > plant1_threshold and plant2_humidityValue > plant2_threshold:
mo.pump_off()
pumpOn = False
class MainApp(JoyApp):
def __init__(self, *arg, **kw):
cfg = dict()
JoyApp.__init__(self, cfg=cfg, *arg, **kw)
self.arm = Arm();
self.f = gcf()
self.f.clf()
self.ax = self.f.gca(projection='3d')
motors = []
motors.append(self.robot.at.Nx02)
motors.append(self.robot.at.Nx08)
motors.append(self.robot.at.Nx04)
motors.append(self.robot.at.Nx06)
self.motors = Motors(motors)
self.orientation = PaperOrientation.HORIZONTAL
self.angles1 = None
self.angles2 = None
self.coordinates = Coordinates()
self.calibrateIdx = -1
self.calibrationPoints = []
self.pos3d = None
self.timeForPlot = self.onceEvery(1.0/3.0)
self.moveToPointPlan = MoveToPointPlan(self)
self.drawLinePlan = DrawLinePlan(self)
self.drawSquarePlan = DrawSquarePlan(self)
self.multipleLinePlan = MultipleLinePlan(self)
self.moveToPointPlan.setPaperOrientation(self.orientation)
self.startPoints = [[5, -10], [10, -15]]
self.endPoints = [[18, -18], [1, -2]]
self.lines = [
[(18,10), (10,22)], [(18,35), (27,24)], [(19,10), (27,25)], [(18,35), (10,22)],
[(29,37), (30,11)], [(42,36), (42,12)], [(29,12), (41,36)], [(14,46), (13,70)],
[(31,45), (20,58)], [(22,58), (32,70)], [(45,46), (33,54)], [(38,70), (49,61)],
[(34,55), (49,62)] ]
def onStart(self):
pass
def moveToPos(self, pos):
self.moveToPointPlan.setPoint(pos)
self.moveToPointPlan.start()
# angles = self.arm.inverseKinematics(pos, self.motors.get_angles())
# if (angles == None):
# print("Can't reach location")
# return
# self.motors.set_angles(angles)
# print("Target Angles: " + str(array(angles) * 180/pi))
def onEvent(self, evt):
if self.timeForPlot():
motorAngles = array(self.motors.get_angles())
modelAngles = self.arm.convertMotorToModelAngles(motorAngles)
self.arm.plotReal3D(modelAngles, self.ax)
self.ax.set(xlim=[-50,50],ylim=[-50,50],zlim=[-50,50])
draw()
pause(0.001)
if evt.type == TIMEREVENT:
return
if evt.type != KEYDOWN:
return
if evt.key == K_q:
self.angles1 = self.motors.get_angles()
print("Set Angles1")
elif evt.key == K_w:
self.angles2 = self.motors.get_angles()
print("Set Angles2")
elif evt.key == K_e:
if (self.angles1 == None):
print("Angles 1 not set, press q to set")
return
self.moveToAnglesPlan.setAngles(self.angles1)
self.moveToAnglesPlan.run()
# self.motors.set_angles(self.angles1)
elif evt.key == K_r:
if (self.angles1 == None):
print("Angles 2 not set, press w to set")
return
# self.motors.set_angles(self.angles2)
self.moveToAnglesPlan.setAngles(self.angles2)
self.moveToAnglesPlan.run()
elif evt.key == K_t:
if (self.orientation == PaperOrientation.HORIZONTAL):
self.orientation = PaperOrientation.VERTICAL
print("VERTICAL")
else:
self.orientation = PaperOrientation.HORIZONTAL
print("Horizontal")
elif evt.key == K_l:
startPoints = []
endPoints = []
for line in self.lines:
startPoint = list(line[0])
endPoint = list(line[1])
temp = startPoint[0]
startPoint[0] = startPoint[1]
startPoint[1] = -temp
temp = endPoint[0]
endPoint[0] = endPoint[1]
endPoint[1] = -temp
startPoints.append(startPoint)
endPoints.append(endPoint)
self.multipleLinePlan.setPoints(startPoints, endPoints, self.orientation)
self.multipleLinePlan.start()
elif evt.key == K_z:
self.motors.go_slack()
elif evt.key == K_p:
angles = self.motors.get_angles()
print("Motor Angles: " + str(array(angles) * 180 / pi))
modelAngles = self.arm.convertMotorToModelAngles(angles)
print("Model Angles: " + str(array(modelAngles) * 180/pi))
pos3d = self.arm.getTool(modelAngles)[0:3]
pos3d[2] -= effectorHeight
print("Pos: " + str(pos3d))
elif evt.key == K_s:
if (not self.coordinates.isCalibrated()):
print("Not calibrated")
return
self.drawSquarePlan.setOrientation(self.orientation)
self.drawSquarePlan.start()
# self.drawLinePlan.setPoints([10, 0], [10, 10], self.orientation)
# self.drawLinePlan.start()
elif evt.key == K_c:
modelAngles = self.arm.convertMotorToModelAngles(self.motors.get_angles())
temp = array(self.arm.getTool(modelAngles))[0:3]
if (self.orientation == PaperOrientation.HORIZONTAL):
temp[2] -= effectorHeight
elif (self.orientation == PaperOrientation.VERTICAL):
temp[0] += effectorHeight
if (self.calibrateIdx == -1):
print("Started Calibration. Move arm to lower left of paper, then press \'c\'")
elif (self.calibrateIdx == 0):
print("Move arm to lower right of paper, then press \'c\'")
self.calibrationPoints.append(temp)
elif (self.calibrateIdx == 1):
print("Move arm to upper right of paper, then press \'c\'")
self.calibrationPoints.append(temp)
elif (self.calibrateIdx == 2):
print("Move arm to upper left of paper, then press \'c\'")
self.calibrationPoints.append(temp)
elif (self.calibrateIdx == 3):
print ("Finished Calibration")
self.calibrationPoints.append(temp)
self.coordinates.calibrate(array(self.calibrationPoints), self.orientation)
self.arm.setPaperPoints(array(self.calibrationPoints))
self.calibrationPoints = []
self.calibrateIdx += 1
if (self.calibrateIdx >= 4):
self.calibrateIdx = -1
elif evt.key == K_a:
if (self.coordinates.isCalibrated()):
self.pos3d = self.coordinates.transformPaperToReal([paperWidth/2, paperLength/2, 1])
else:
self.pos3d = [30, 0, 20]
self.moveToPos(self.pos3d)
elif evt.key == K_UP:
self.pos3d[0] += 2
self.moveToPos(self.pos3d)
elif evt.key == K_DOWN:
self.pos3d[0] -= 2
self.moveToPos(self.pos3d)
elif evt.key == K_LEFT:
self.pos3d[1] += 2
self.moveToPos(self.pos3d)
elif evt.key == K_RIGHT:
self.pos3d[1] -= 2
self.moveToPos(self.pos3d)
elif evt.key == K_i:
self.pos3d[2] += 2
self.moveToPos(self.pos3d)
elif evt.key == K_k:
self.pos3d[2] -= 2
self.moveToPos(self.pos3d)
