You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
78 lines
1.6 KiB
78 lines
1.6 KiB
6 years ago
|
import threading
|
||
|
|
import time
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
startTime = time.time()
|
||
|
|
nowTime = 0;
|
||
|
|
|
||
|
|
currentMotor = 0
|
||
|
|
motorcfg = [0] * 10
|
||
|
|
motorcfg[1] = {"positive":1, "negative":2, 'speedslot':2}
|
||
|
|
motorcfg[2] = {"positive":3, "negative":4, 'speedslot':4}
|
||
|
|
motorcfg[3] = {"positive":5, "negative":6, 'speedslot':6}
|
||
|
|
|
||
|
|
friction = 0.99
|
||
|
|
movement_time = 5
|
||
|
|
max_movement_time = 10
|
||
|
|
min_speed = 1
|
||
|
|
max_speed = 50
|
||
|
|
|
||
|
|
speed = 0
|
||
|
|
stopTime = 0
|
||
|
|
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):
|
||
|
|
print("JOG command {}".format(motorcfg[currentMotor]['positive']))
|
||
|
|
else:
|
||
|
|
print("JOG command {}".format(motorcfg[currentMotor]['negative']))
|
||
|
|
|
||
|
|
|
||
|
|
def stopMotorFunctions():
|
||
|
|
global currentMotor, speed
|
||
|
|
currentMotor = 0
|
||
|
|
speed = 0
|
||
|
|
print ("JOB command stop all motors")
|
||
|
|
|
||
|
|
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
|
||
|
|
print("JOG parameter {} to {}".format( motorcfg[currentMotor]['speedslot'], abs(speed)) )
|
||
|
|
|
||
|
|
|
||
|
|
def rt():
|
||
|
|
adjustSpeed(1, 5)
|
||
|
|
|
||
|
|
def lt():
|
||
|
|
adjustSpeed(1, -5)
|
||
|
|
|
||
|
|
def up():
|
||
|
|
adjustSpeed(2, 5)
|
||
|
|
|
||
|
|
def dn():
|
||
|
|
adjustSpeed(2, -5)
|
||
|
|
|
||
|
|
updateLoop();
|