def main(): M.initMotors() pat_a() M.flipCube(1) pat_a() M.rotateCube(1) M.flipCube(1) pat_a() sleep(1) M.moveArm(M.ArmPositions.Up) M.tableMotor.on_for_rotations(50, 3) sleep(1)
def initMotors(): motors.initMotors()
def main(): # Flush output for file logging sys.stdout.flush() # Init motor angle sensors via I2C magneticSensors = sensors.AMS() magneticSensors.connect(1) # Init motors via USB motorsModule.initMotors() motors = [0.0] * 9 # Motor outputs 1 to 8, ignore 0 # Motor speeds speed_left = 0.0 speed_right = 0.0 # Remote control old_remote_x = 0.0 old_remote_y = 0.0 # Wait on other threads to start up time.sleep(0.5) # Send OSC commands # ip = "192.168.4.1" # port = 2222 # client = udp_client.SimpleUDPClient(ip, port) # Loop while not keys or not keys.esc_key_pressed: # Read current accelerometer value to see how far forward we're leaning pitch = motorsModule.readIMU('ax') # Read battery level voltage = 0 #motorsModule.readBatteryVoltage() # Read current angles of motors currentAngles = functions.readCurrentAngles(magneticSensors) # Calculate difference in mouse x position try: diff_x = remote.x - old_remote_x diff_y = remote.y - old_remote_y old_remote_x = remote.x old_remote_y = remote.y except NameError: pass FORWARD_SPEED = 2.0 BACKWARD_SPEED = 2.0 TURNING_SPEED = 4.0 MOVEMENT_SPEED = 4.0 # Remote try: # Change motor speeds for turning left right speed_left -= diff_x * TURNING_SPEED / 100.0 speed_right += diff_x * TURNING_SPEED / 100.0 # Remote keyboard commands if remote.up: speed_left = speed_left - FORWARD_SPEED speed_right = speed_right - FORWARD_SPEED if remote.down: speed_left = speed_left + BACKWARD_SPEED speed_right = speed_right + BACKWARD_SPEED if remote.left: speed_left += 20.0 speed_right -= 20.0 if remote.right: speed_left -= 20.0 speed_right += 20.0 # Go forward backward on mouse y speed_left += diff_y * MOVEMENT_SPEED / 100.0 speed_right += diff_y * MOVEMENT_SPEED / 100.0 # Go foward backward on clicks if remote.left_mouse_down: speed_left = speed_left - FORWARD_SPEED speed_right = speed_right - FORWARD_SPEED if remote.right_mouse_down: speed_left = speed_left + BACKWARD_SPEED speed_right = speed_right + BACKWARD_SPEED except NameError: pass # Let's Robot controller try: if letsrobot_controller.forward: speed_left = speed_left - FORWARD_SPEED speed_right = speed_right - FORWARD_SPEED if letsrobot_controller.backward: speed_left = speed_left + BACKWARD_SPEED speed_right = speed_right + BACKWARD_SPEED if letsrobot_controller.left: speed_left += 1.0 * TURNING_SPEED / 5.0 speed_right -= 1.0 * TURNING_SPEED / 5.0 if letsrobot_controller.right: speed_left -= 1.0 * TURNING_SPEED / 5.0 speed_right += 1.0 * TURNING_SPEED / 5.0 except NameError: pass # Clamp speed_left = functions.clamp(speed_left, -100, 100) speed_right = functions.clamp(speed_right, -100, 100) # Send to BROBOT # client.send_message("/1/fader1", 0.5 + speed_left/400.0 + speed_right/400.0) # client.send_message("/1/fader2", 0.5 + speed_left/500.0 - speed_right/500.0) # Balance targetAngles = [None, 0, 0] targetAngles[1] = -pitch - 22 - speed_left / 2 targetAngles[2] = -pitch - 22 - speed_right / 2 # Run movement controller to see how fast we should set our motor speeds movement = walk.calculateMovement(currentAngles, targetAngles) # Send motor commands motors[1] = 0 #movement[1] # Right motor motors[2] = 0 #movement[2] # Left motor motors[1] = speed_right / 5 # Right motor motors[2] = speed_left / 5 # Left motor motorsModule.sendMotorCommands(motors, simulator, False, True) # Display balance, angles, target angles and speeds # functions.display( "Pitch: %3d. Right, Left: Hips: %3d, %3d, Targets: %3d, %3d, Speeds: %3d, %3d. %3d %3d" # % (pitch, currentAngles[1], currentAngles[2], targetAngles[1], targetAngles[2], motors[1], motors[2], speed_left, speed_right ) ) #functions.display( "Pitch: %3d. Right, Left: Knees: %3d, %3d, Targets: %3d, %3d, Speeds: %3d, %3d" # % (pitch, 0, 0, targetAngles[3], targetAngles[4], motors[3], motors[4] ) ) # functions.display( "Pitch: %3d. Right, Left: Feet: %3d, %3d, Targets: %3d, %3d, Speeds: %3d, %3d" # % (pitch, 0, 0, targetAngles[5], targetAngles[6], motors[5], motors[6] ) ) # Slow down speed_left = speed_left * 0.94 speed_right = speed_right * 0.94 # Stop motors motorsModule.stopMotors()
import face_recognition import camera import numpy as np import motors as motorsModule import matplotlib.image as mpimg import random import time # Start camera #camera.startCamera( (320, 160) ) output = np.empty((240, 320, 3), dtype=np.uint8) output = mpimg.imread('face.jpg') # Init motors motorsModule.initMotors() motors = [0.0] * 9 # Initialize some variables face_locations = [] while True: # Grab a single frame of video from the camera as a numpy array # camera.capture(output, format="rgb") # frame = camera.getFrame() # De-blue # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) print("\nFinding") # Find the faces and in the current frame of video
time_start = time.time() lastPWM = 0 vision_steering = 0 vision_speed = 0 acceleration_time = time.time() gy_sum = 0 time_now = 0 old_time = 0 dt = 0 # Write log sys.stdout.flush() # Start motors if motors is not None: motors.initMotors() # Loop print("Running.") t = 0 while not keys or not keys.esc_key_pressed: # Calculate dt time_now = time.time() dt = time_now - old_time old_time = time_now # Remote controls if remote: if remote.up: acceleration += 8 * dt if remote.down: