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

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();