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
