diff --git a/movearm/game_stencil.py b/movearm/game_stencil.py new file mode 100644 index 0000000..3c7a671 --- /dev/null +++ b/movearm/game_stencil.py @@ -0,0 +1,78 @@ +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(); \ No newline at end of file