try:
if (BP.get_sensor(TOUCH_SENSOR_PORT)):
BP.set_motor_speed(BELT_MOTOR_PORT, -50)
else:
BP.set_motor_speed(BELT_MOTOR_PORT, 0)
except brickpi3.SensorError:
BP.set_motor_speed(BELT_MOTOR_PORT, 0)
# Sort the M&M's based on color
ColorMM = GetMMColor() # Get the M&M color
if ColorMM != ColorMMLast: # If new color
ColorMMLast = ColorMM
if ColorMM != 0: # M&M color detected
print(ColorList[ColorMM]) # Print the color detected
if (ColorMM == 1 or ColorMM == 4): # if it's red or green
Arm(True) # Make the arm grab the M&M off the belt
ArmReturn = millis + 400 # Keep the arm in place for 0.4 seconds
time.sleep(
0.1
) # Slow down a bit so that a single M&M won't read as multiple colors
# Check if it's time to make the arm return
if (ArmReturn and ArmReturn <= millis): # After the specified time
Arm(False) # Return the arm
ArmReturn = 0
except KeyboardInterrupt: # On keyboard interrupt
pivotpi.angle_microseconds(ARM_SERVO, 0) # Make the arm servo float
BP.reset_all() # Reset the BrickPi motor and sensors
print('exiting')
def Arm(Position):
if Position:
pivotpi.angle_microseconds(ARM_SERVO, 1250)
else:
pivotpi.angle_microseconds(ARM_SERVO, 1450)
import pivotpi
# Setup the PivotPi
try:
pivotpi = pivotpi.PivotPi(0x40, 60)
except IOError:
print("PivotPi not found - quitting")
exit(-1)
except:
print("Unknown error with PivotPi. Is there a problem with the library?")
exit(-1)
print('Moving servos on channel 1-8, press Ctrl-C to quit...')
try:
while True:
for i in range(8):
#pivotpi.angle(i, 0)
pivotpi.led(i, 0) # Set the LED to 0 Power
pivotpi.angle_microseconds(i, 1500) # Set the Servo to 1500 angle
time.sleep(0.05)
for i in range(8):
#pivotpi.angle(i, 180)
pivotpi.led(i, (i + 1) * 4) # Increase the LED Power
pivotpi.angle_microseconds(i, 550 +
(i * 272)) # Change the pivotpi Angle
time.sleep(0.05) # Give the system a rest.
except KeyboardInterrupt:
print("\nGoodbye")
except:
print("Unknown error")
