def move_rel (m_dist): global position global moving global accel global velocity if (position + m_dist < 0): error = 3 print >> sys.stderr, "Invalid relative move (negative postion) aborting\n" return (error) if (dist < 0): direction = 'cw' else: direction = 'ccw' position += m_dist MOTOR.stepperCONFIG(card, motor, direction, 3, velocity, accel) stopped = 0 moving = 1 MOTOR.setLED(0) MOTOR.move(card, motor, m_dist) delay = m_dist / velocity delay += accel time.sleep(delay) moving = 0 stopped = 1 MOTOR.clrLED(0)
def stop(): global position global moving global jogging print "entered stop " moving = 0 jogging = 0 stopped = 1 MOTOR.clrLED(0) MOTOR.stepperSTOP (card, motor) position = 0
def signal_handler(sig, frame): # try 'None' instead of 'frame'sen global card global motor global accel MOTOR.stepperSTOP (card, motor) time.sleep (0.1) stopped = 1 MOTOR.clrLED(0) time.sleep (accel) MOTOR.stepperOFF (card, motor) enabled = 0 disabled = 1 print('You pressed Ctrl+C!') sys.exit(1)
def move (loc): global position global moving global accel global velocity print "entered move = ", loc if (loc <0 ): error=1 print >> sys.stderr, "Invalid move (negative postion) aborting\n" return(error) else: m_dist = loc - position if (m_dist == 0): error=2 print >> sys.stderr, "Invalid move (0 steps), aborting\n" return (error) elif (m_dist < 0): direction = 'cw' else: direction = 'ccw' position += m_dist print "move motor", m_dist m_dist = abs (m_dist) MOTOR.stepperCONFIG(card, motor, direction, 3, velocity, accel) stopped = 0 moving = 1 MOTOR.setLED(0) MOTOR.stepperMOVE(card, motor, m_dist) delay = m_dist / velocity delay += accel time.sleep(delay) moving = 0 stopped = 1 MOTOR.clrLED(0)
def resetCtl(): MOTOR.clrLED(ctl) sleep(2) MOTOR.RESET(ctl) return
def main(): #======================================================================= # Initialize Variables, processes, threads #======================================================================= # Initialize multi-threaded HC-SR04 sonic sensor # DistThread = SR04.ChkDist(1,"ChkDist-1") # DistThread.start() # Define Motors global FL global FR global RL global RR global status FL = 1 # Define Motor 1 = Front Left - FL FR = 2 # Define Motor 2 = Fright Right - FR RL = 3 # Defome Motor 3 = Rear Left - RL RR = 4 # Define Motor 4 = Rear Right - RR #Initalized the Xbox controller via the pygame module pygame.init() joystick = pygame.joystick.Joystick(0) joystick.init() print("JS Initialized: ",joystick.get_init()) # Return 1 or True if Successful print(joystick.get_name()) #PiPlate Motor controller motorDrvVersion = MotorDrv.version() print("Motor Controller: {0}".format(motorDrvVersion)) motor = MOTOR global ctl ctl = 1 resetCtl(ctl) status = initMotor('ccw','ccw','ccw','ccw') for i in range(10): MOTOR.clrLED(ctl) sleep(.0625) MOTOR.setLED(ctl) sleep(.0625) print("Reset and initalize controller {0} and motors".format(ctl)) axisMax = 1 minSpeed = 5 obstruction = 20 #======================================================================= # Main Logic Loop Start #======================================================================= # Forward # (-179 / 179) # 180 # | # | #Left 270 ----+------ 90 Right # (-90) | # | # 360 / 0 # (-1 / 1) # Reverse #======================================================================= while True: leftSpeed = 100 rightSpeed = 100 #sleep(.25) #status = chkDist(status) for event in pygame.event.get(): if event.type == pygame.JOYBUTTONDOWN: B = joystick.get_button(1) if (B == 1): print("Button B:",B) status = MotorOff() A = joystick.get_button(0) if (A == 1): print("Button A:",A) # exit() pass if event.type == pygame.JOYAXISMOTION: y = joystick.get_axis(1) x = joystick.get_axis(0) speed = joystick.get_axis(2) angle = math.degrees(math.atan2(x,y)) #status = chkDist(status) #=========================================================================================== # Determining drive direction is based on the calculated angle of the joystick's # x and y offset from each other. # NOTE: depending on the direction, forward, forward right, forward left, # left, right, reverse left, reverse right, the speed of the left or # right axels are adjusted by the x and y values. Right and left # axels speeds are determined by the x OR y offset values from its axis. # # Up/down or forward/reverse speed is determined by the veritical Y axis. # Left/Right speed is determined by the horizontal X axis. # # ========================================================================================== if(angle < 0): angle = angle + 360 yAxis = abs((y/axisMax * 100)) xAxis = abs((x/axisMax * 100)) # Hard Forward if(angle == 180): leftSpeed = yAxis rightSpeed = yAxis if status != 'forward': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "forward" # Forward Right Turning Angle if(angle >= 135 and angle < 180 and angle != 180): leftSpeed = yAxis rightSpeed = yAxis+(xAxis/2) if status != 'forward': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) # Forward Right Turning Angle if(angle > 90 and angle < 135 and angle != 90): leftSpeed = xAxis rightSpeed = yAxis+(xAxis/2) if (yAxis+(xAxis/2)) > 10 else 20 if status != 'forward': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) # Forward Left Turning Angle if(angle > 180 and angle < 226 and angle != 180): leftSpeed = yAxis-(xAxis/2) rightSpeed = yAxis if status != 'forward': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) # Forward Left Turning Angle if(angle >= 226 and angle < 270): leftSpeed = yAxis-(xAxis/2) if (yAxis-(xAxis/2)) > 10 else 20 rightSpeed = xAxis if status != 'forward': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) #Hard Left if(angle == 270): leftSpeed = xAxis rightSpeed = xAxis if status != 'left': status = MotorOff() status = left(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "left" # Reverse Right Turning Angle if(angle > 270 and angle <= 315): leftSpeed = yAxis rightSpeed = xAxis-(yAxis/2) if (xAxis-(yAxis/2)) > 10 else 20 if status != 'reverse': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "reverse" # Reverse Right Turning Angle if(angle > 315 and angle <= 360): leftSpeed = yAxis rightSpeed = xAxis # -(yAxis/2) if (xAxis-(yAxis/2)) > 10 else 20 if status != 'reverse': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "reverse" # Hard Reverse if(angle == 0 ): leftSpeed = yAxis rightSpeed = yAxis if status != 'reverse': status = MotorOff() status = reverse(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "reverse" # Reverse Left Turning Angle if(angle > 0 and angle <= 45): leftSpeed = xAxis rightSpeed = yAxis-(xAxis/2) if (yAxis-(xAxis/2)) > 10 else 20 if status != 'reverse': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "reverse" # Reverse Left Turning Angle if(angle > 45 and angle < 90): leftSpeed = xAxis rightSpeed = yAxis #-(xAxis/2) if (yAxis-(xAxis/2)) > 10 else 20 if status != 'reverse': status = MotorOff() status = fwd(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "reverse" #Hard Right Turn if(angle == 90.0): leftSpeed = xAxis rightSpeed = xAxis if status != 'right': status = MotorOff() status = right(leftSpeed,rightSpeed,leftSpeed,rightSpeed) motorSpeed(leftSpeed,rightSpeed,leftSpeed,rightSpeed) status = "right" if xAxis <=1 and yAxis <= 1 and status != 'stopped': status = MotorOff() print('x: {0:>6.3f} y:{1:>6.3f} = {2:.0f} Degrees Speed(L/R): {3:.2f} : {4:.2f} Status: {5}'.format(abs(x)*100,abs(y)*100,angle,leftSpeed,rightSpeed,status)) return