|
|
|
@ -198,58 +198,58 @@ isRunning = False
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def adjustSpeed(motor, adjustment):
|
|
|
|
|
global nowTime, stopTime, currentMotor, speed, motorcfg, movement_time, max_speed, max_movement_time
|
|
|
|
|
|
|
|
|
|
if currentMotor==motor:
|
|
|
|
|
speed += adjustment * (abs(speed)/max_speed)
|
|
|
|
|
stopTime += movement_time/2 * max_movement_time/(stopTime - nowTime)
|
|
|
|
|
else:
|
|
|
|
|
currentMotor = motor
|
|
|
|
|
speed = adjustment
|
|
|
|
|
stopTime = nowTime + movement_time
|
|
|
|
|
if(speed>0):
|
|
|
|
|
global nowTime, stopTime, currentMotor, speed, motorcfg, movement_time, max_speed, max_movement_time
|
|
|
|
|
|
|
|
|
|
if currentMotor==motor:
|
|
|
|
|
speed += adjustment * (abs(speed)/max_speed)
|
|
|
|
|
stopTime += movement_time/2 * max_movement_time/(stopTime - nowTime)
|
|
|
|
|
else:
|
|
|
|
|
currentMotor = motor
|
|
|
|
|
speed = adjustment
|
|
|
|
|
stopTime = nowTime + movement_time
|
|
|
|
|
if(speed>0):
|
|
|
|
|
candy.setJOGcmd(motorcfg[currentMotor]['positive'])
|
|
|
|
|
#print("JOG command {}".format(motorcfg[currentMotor]['positive']))
|
|
|
|
|
else:
|
|
|
|
|
#print("JOG command {}".format(motorcfg[currentMotor]['positive']))
|
|
|
|
|
else:
|
|
|
|
|
candy.setJOGcmd(motorcfg[currentMotor]['negative'])
|
|
|
|
|
# print("JOG command {}".format(motorcfg[currentMotor]['negative']))
|
|
|
|
|
|
|
|
|
|
# print("JOG command {}".format(motorcfg[currentMotor]['negative']))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def stopMotorFunctions():
|
|
|
|
|
global currentMotor, speed
|
|
|
|
|
currentMotor = 0
|
|
|
|
|
speed = 0
|
|
|
|
|
global currentMotor, speed
|
|
|
|
|
currentMotor = 0
|
|
|
|
|
speed = 0
|
|
|
|
|
candy.stop()
|
|
|
|
|
|
|
|
|
|
def updateLoop():
|
|
|
|
|
global nowTime, stopTime, currentMotor, speed, friction, min_speed, motorcfg
|
|
|
|
|
nowTime = (time.time() - startTime)
|
|
|
|
|
threading.Timer(0.2, updateLoop).start()
|
|
|
|
|
|
|
|
|
|
if currentMotor != 0:
|
|
|
|
|
if nowTime > stopTime:
|
|
|
|
|
stopMotorFunctions()
|
|
|
|
|
return
|
|
|
|
|
else:
|
|
|
|
|
speed *= friction
|
|
|
|
|
if abs(speed) < min_speed:
|
|
|
|
|
stopMotorFunctions()
|
|
|
|
|
return
|
|
|
|
|
global nowTime, stopTime, currentMotor, speed, friction, min_speed, motorcfg
|
|
|
|
|
nowTime = (time.time() - startTime)
|
|
|
|
|
threading.Timer(0.2, updateLoop).start()
|
|
|
|
|
|
|
|
|
|
if currentMotor != 0:
|
|
|
|
|
if nowTime > stopTime:
|
|
|
|
|
stopMotorFunctions()
|
|
|
|
|
return
|
|
|
|
|
else:
|
|
|
|
|
speed *= friction
|
|
|
|
|
if abs(speed) < min_speed:
|
|
|
|
|
stopMotorFunctions()
|
|
|
|
|
return
|
|
|
|
|
candy.setparams(motorcfg[currentMotor]['name'], abs(speed)))
|
|
|
|
|
#print("JOG parameter {} to {}".format( motorcfg[currentMotor]['speedslot'], abs(speed)) )
|
|
|
|
|
#print("JOG parameter {} to {}".format( motorcfg[currentMotor]['speedslot'], abs(speed)) )
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def rt():
|
|
|
|
|
adjustSpeed(1, 5)
|
|
|
|
|
adjustSpeed(1, 5)
|
|
|
|
|
|
|
|
|
|
def lt():
|
|
|
|
|
adjustSpeed(1, -5)
|
|
|
|
|
adjustSpeed(1, -5)
|
|
|
|
|
|
|
|
|
|
def up():
|
|
|
|
|
adjustSpeed(2, 5)
|
|
|
|
|
adjustSpeed(2, 5)
|
|
|
|
|
|
|
|
|
|
def dn():
|
|
|
|
|
adjustSpeed(2, -5)
|
|
|
|
|
adjustSpeed(2, -5)
|
|
|
|
|
|
|
|
|
|
updateLoop()
|
|
|
|
|
|
|
|
|
|