def set(targetLevel): try: targetVoltage = levels[targetLevel] currentVoltage = mcp.read_adc(0) if (currentVoltage > targetVoltage): print("Positioning down to L:", targetLevel, "V:", targetVoltage) while mcp.read_adc(0) > targetVoltage: print(mcp.read_adc(0)) motors.motor1.setSpeed(int(round(-2.4 * MAX_SPEED / 6))) time.sleep(0.1) if (currentVoltage < targetVoltage): print("Positioning up to L:", targetLevel, "V:", targetVoltage) while mcp.read_adc(0) < targetVoltage: print(mcp.read_adc(0)) motors.motor1.setSpeed(int(round(2.4 * MAX_SPEED / 6))) time.sleep(0.1) motors.setSpeeds(0, 0) time.sleep(0.25) # at high levels, many adjustments need to be made if (not (mcp.read_adc(0) > targetVoltage - 1 and mcp.read_adc(0) < targetVoltage + 1)): set(targetLevel) finally: # Stop the motors, even if there is an exception # or the user presses Ctrl+C to kill the process. motors.setSpeeds(0, 0)
def stop(): speed = 0 motors.setSpeeds(0, 0) motors.motor1.setSpeed(speed) motors.motor2.setSpeed(speed) return ('OK')
def drive(): global angle # synDrive is the drive level for going forward or backward (for both wheels) synDrive = advance * (1 - diffDrive) * throttle * totalDrive leftDiff = bias * diffDrive * throttle * totalDrive rightDiff = -bias * diffDrive * throttle * totalDrive # construct the drive levels topSpeed = (synDrive + leftDiff) LDrive = topSpeed * (0.8 + 0.2*math.sin(angle)) RDrive = topSpeed * (0.8 - 0.2*math.sin(angle)) #(synDrive + rightDiff) angle = angle + 0.00012 print(LDrive, RDrive) # Make sure that it is outside dead band and less than the max if LDrive > deadband: if LDrive > MAX_MOTOR_SPEED: LDrive = MAX_MOTOR_SPEED elif LDrive < -deadband: if LDrive < -MAX_MOTOR_SPEED: LDrive = -MAX_MOTOR_SPEED else: LDrive = 0 if RDrive > deadband: if RDrive > MAX_MOTOR_SPEED: RDrive = MAX_MOTOR_SPEED elif RDrive < -deadband: if RDrive < -MAX_MOTOR_SPEED: RDrive = -MAX_MOTOR_SPEED else: RDrive = 0 # Actually Set the motors motors.setSpeeds(int(LDrive), int(RDrive))
def refuseToPlay(): motors.setSpeeds(0, 0) l_spin(1) sayNow("I'm going to dance") #say("SLEEP 1") time.sleep(2) right(1.5) left(1.5) forward(2) backward(2) r_spin(4) l_spin(3) r_spin(3) right(1.5) left(1.5) backward(2) forward(2) r_spin(5) l_spin(5) #while True: # ok = loop() # if not ok: # break #pixy.pixy_close() motors.setSpeeds(0, 0)
def drive(): # synDrive is the drive level for going forward or backward (for both wheels) synDrive = advance * (1 - diffDrive) * throttle * totalDrive leftDiff = bias * diffDrive * throttle * totalDrive rightDiff = -bias * diffDrive * throttle * totalDrive # construct the drive levels LDrive = (synDrive + leftDiff) RDrive = (synDrive + rightDiff) # Make sure that it is outside dead band and less than the max if LDrive > deadband: if LDrive > MAX_MOTOR_SPEED: LDrive = MAX_MOTOR_SPEED elif LDrive < -deadband: if LDrive < -MAX_MOTOR_SPEED: LDrive = -MAX_MOTOR_SPEED else: LDrive = 0 if RDrive > deadband: if RDrive > MAX_MOTOR_SPEED: RDrive = MAX_MOTOR_SPEED elif RDrive < -deadband: if RDrive < -MAX_MOTOR_SPEED: RDrive = -MAX_MOTOR_SPEED else: RDrive = 0 # Actually Set the motors motors.setSpeeds(int(LDrive), int(RDrive))
def take_reading(channel): kick_watchdog() if not reverse: motors.setSpeeds(0,0) time.sleep(10) print("Taking Reading") global filename global currentStep currentStep+=stepsize filename = "/home/pi/anemometer/data/"+device+"_"+percent+"per_"+str(currentStep)+"ft.csv" port=serial.Serial(serial_ports(),timeout=3) # OPEN PORT for x in range(0,stoptime): # HOW MANY READINGS TO TAKE AT EACH SPOT time.sleep(.5) # STOPS FLOODING rcv=port.read(40) # READ DATA data=struct.unpack("<2B6f12Bh",rcv) # PROCESS DATA print("TIME: " + time.strftime("%c") + "\t\tTEMP: " + str(data[3]) + "\t\tm/s: " + str(data[2])+"\t\tStep: "+str(currentStep)) if data[3] > 2 and data[3] < 60: # TEMP <2C indicates probable error; do not write to file; if data[2] < 30: # VELO > 30 meters per second indicates probable error, <0.01 probably indicates error, do not write to file f = open(filename,"a") f.write(time.strftime("%x")+","+time.strftime("%X")+","+str(data[3])+","+str(data[2])+","+str(currentStep)+"\n") f.close else: time.sleep(stoptime) break else: # if we get a bad reading, all the readings will be bad. stop taking readings to prevent rogue data time.sleep(stoptime) #because of the way the debouncer works, we have to stay on this point anyway... break print("Reading complete - forward") motors.setSpeeds(MAX_SPEED,MAX_SPEED) vtplot(filename) #Update temp/velocity GNUPlot at end of checkpoint
def oneshot(): server_sock = BluetoothSocket(RFCOMM) server_sock.bind(("", PORT_ANY)) server_sock.listen(1) port = server_sock.getsockname()[1] uuid = "94f39d29-7d6d-437d-973b-fba39e49d4ee" advertise_service( server_sock, "SampleServer", service_id=uuid, service_classes=[uuid, SERIAL_PORT_CLASS], profiles=[SERIAL_PORT_PROFILE], #protocols = [OBEX_UUID] ) print("Waiting for connection on RFCOMM channel %d" % port) try: client_sock, client_info = server_sock.accept() print("Accepted connection from ", client_info) reciever(client_sock) finally: motors.setSpeeds(0, 0) client_sock.close() print("disconnected") server_sock.close() print("all done")
def turn(dir): if dir == "left": motors.setSpeeds(-v3, v3) else: motors.setSpeeds(v3, -v3) # Start with a short turn to ensure that we will # leave the line under (or next to) the sensors. sleep (100 * delay) # Count loops while turning (for calibration) turn = 100 # Turn until line is lost while read_sensors("middle") == 1: turn += 1 sleep (delay) # Turn until line is reached again while read_sensors("middle") == 0: turn += 1 sleep (delay) return turn
def moveToLevel(targetLevel): try: targetVoltage = levels[targetLevel] currentVoltage = mcp.read_adc(0) if (currentVoltage > targetVoltage): print("Positioning down to L:", targetLevel, "V:", targetVoltage) while mcp.read_adc(0) > targetVoltage: print(mcp.read_adc(0)) motors.motor1.setSpeed(int(round(-2.4 * MAX_SPEED / 6))) time.sleep(0.1) if (currentVoltage < targetVoltage): print("Positioning up to L:", targetLevel, "V:", targetVoltage) while mcp.read_adc(0) < targetVoltage: print(mcp.read_adc(0)) motors.motor1.setSpeed(int(round(2.4 * MAX_SPEED / 6))) time.sleep(0.1) motors.setSpeeds(0, 0) time.sleep(0.25) if (mcp.read_adc(0) != targetVoltage): moveToLevel(targetLevel) finally: # Stop the motors, even if there is an exception # or the user presses Ctrl+C to kill the process. motors.setSpeeds(0, 0)
def MotorsStop(): try: motors.setSpeeds(0,0) except KeyboardInterrupt: motors.setSpeeds(0,0) finally: Speed=0 Active=0
def init_sequence(): '''Moving sequence for initializing turret''' print("Turret init sequence") for i in range(3): motors.setSpeeds(300, 300) time.sleep(.04) motors.setSpeeds(0, 0) time.sleep(.1)
def handleMotor(self, msg): dx = msg.linear.x # [m/s] dr = msg.angular.z # [rad/s] rv = int((1.0 * dx + dr * self.radius) * self.speed_ratio) lv = int((1.0 * dx - dr * self.radius) * self.speed_ratio) rospy.loginfo("drive: in(x:%+02.2f r:%+02.2f) out:(L:%4d R:%4d)", dx, dr, lv, rv) motors.setSpeeds(int(rv), int(lv))
def __init__(self): rospy.init_node('kame_driver') motors.setSpeeds(0, 0) self.radius = rospy.get_param("~base_width", 0.129) / 2 max_speed = rospy.get_param("~max_speed", 0.398) self.speed_ratio = 1.0 * MAX_SPEED / max_speed sub = rospy.Subscriber('/cmd_vel', Twist, self.handleMotor)
def forward(self): try: motors.setSpeeds(0, 0) motors.motor1.setSpeed(SPEED) motors.motor2.setSpeed(SPEED) except: # Stop the motors, even if there is an exception # or the user presses Ctrl+C to kill the process. motors.setSpeeds(0, 0) return "going forward"
def GET(self): try: motors.setSpeeds(0, 0) turnOffDiodes(diodesGpio[0], diodesGpio[1]) #GPIO.cleanup() except: # Stop the motors, even if there is an exception # or the user presses Ctrl+C to kill the process. motors.setSpeeds(0, 0) return "going forward"
def stop(self): try: self.turnOffDiodes(diodesGpio[0], diodesGpio[1]) motors.setSpeeds(0, 0) except: # Stop the motors, even if there is an exception # or the user presses Ctrl+C to kill the process. motors.setSpeeds(0, 0) return "stoped"
def accelerate_to(self, s1, s2): global speed1, speed2 delay = .1 time.sleep(delay * 2) for i in range(10): motors.setSpeeds((self.speed1 * (10 - i) + s1 * i) / 10, (self.speed2 * (10 - i) + s2 * i) / 10) time.sleep(delay) self.speed1 = s1 self.speed2 = s2
def adjustSpeed(self, rightSpeed, leftSpeed): self.rightSpeed = rightSpeed self.leftSpeed = leftSpeed 480 if rightSpeed > 480 else rightSpeed -480 if rightSpeed < -480 else rightSpeed 480 if leftSpeed > 480 else leftSpeed -480 if leftSpeed < -480 else leftSpeed motors.setSpeeds(rightSpeed, leftSpeed)
def irritate_sequence(): '''Moving sequence for irritated turret''' # quickly moving back and forth for a small distance # distance: ~1 cm? # cycles: 2? print("Turret irritate sequence...") for i in range(5): motors.setSpeeds(MAX_SPEED, MAX_SPEED) time.sleep(0.05) motors.setSpeeds(0, 0) time.sleep(0.05)
def hizlariAyarla(self, hizSag, hizSol): self.hizSag = hizSag self.hizSol = hizSol 480 if hizSag > 480 else hizSag -480 if hizSag < -480 else hizSag 480 if hizSol > 480 else hizSol -480 if hizSol < -480 else hizSol motors.setSpeeds(hizSag, hizSol)
def back(self): self.lightDiodes(diodesGpio[0], diodesGpio[1]) try: motors.setSpeeds(0, 0) motors.motor1.setSpeed(-SPEED) motors.motor2.setSpeed(-SPEED) except: # Stop the motors, even if there is an exception # or the user presses Ctrl+C to kill the process. motors.setSpeeds(0, 0) return "going forward"
def set(): try: lastV = accurate_voltage() while True: vout = accurate_voltage() if vout > lastV + 30 or vout < lastV - 30: continue lastV = vout if vout < 70 or vout > 260: print('out of range', vout) break output = int(pid(vout)) #print('motor:', output, "pot", mcp.read_adc(0)) if not(output > maxOutput or output < (-1 * maxOutput)): # print('set speed') motor_speed = output motors.motor1.setSpeed(output) # targetVoltage = levels[targetLevel] # currentVoltage = mcp.read_adc(0) # if (currentVoltage > targetVoltage): # print("Positioning down to L:", targetLevel, "V:", targetVoltage) # while mcp.read_adc(0) > targetVoltage: # print(mcp.read_adc(0)) # motors.motor1.setSpeed(int(round(-2.4 * MAX_SPEED / 6))) # time.sleep(0.1) # if (currentVoltage < targetVoltage): # print("Positioning up to L:", targetLevel, "V:", targetVoltage) # while mcp.read_adc(0) < targetVoltage: # print(mcp.read_adc(0)) # motors.motor1.setSpeed(int(round(2.4 * MAX_SPEED / 6))) # time.sleep(0.1) # motors.setSpeeds(0, 0) # time.sleep(0.25) # # at high levels, many adjustments need to be made # if (not(mcp.read_adc(0) > targetVoltage - 1 and mcp.read_adc(0) < targetVoltage + 1)): # set(targetLevel) finally: # Stop the motors, even if there is an exception # or the user presses Ctrl+C to kill the process. motors.setSpeeds(0, 0) print('V') print('\n'.join(map(str, plot_v))) print('T') print('\n'.join(map(str, plot_t)))
def Message_Received(self, data): pertinentData = data[5:] pipePosition = pertinentData.rindex('|') leftMotorData = pertinentData[:pipePosition] rightMotorData = pertinentData[pipePosition + 1:] increment = MAX_SPEED / self.MAX_THROTTLE_POSITION leftMotorForward = leftMotorData[:1] == "F" leftMotorSpeed = increment * int(leftMotorData[1:]) if (leftMotorForward == False): leftMotorSpeed = leftMotorSpeed * -1 rightMotorForward = rightMotorData[:1] == "F" rightMotorSpeed = increment * int(rightMotorData[1:]) if (rightMotorForward == False): rightMotorSpeed = rightMotorSpeed * -1 motors.setSpeeds(leftMotorSpeed, rightMotorSpeed)
def MotorUp(Speed,Active): try: int(Speed) if int(Active)==1: while True: motors.setSpeeds(-Speed,-Speed) sys.exit(0) elif int(Active)==0: MotorsStop() except KeyboardInterrupt: MotorsStop() finally: Speed=0 Active=0
def updateMotors(self): if self.speedL == 0: self.actSpeedL = 0 elif self.actSpeedL < self.speedL: self.actSpeedL += self.SPEED_STEP elif self.actSpeedL > self.speedL: self.actSpeedL -= self.SPEED_STEP if self.speedR == 0: self.actSpeedR = 0 elif self.actSpeedR < self.speedR: self.actSpeedR += self.SPEED_STEP elif self.actSpeedR > self.speedR: self.actSpeedR -= self.SPEED_STEP print ("motor lt, rt", self.actSpeedR, self.actSpeedL) motors.setSpeeds(self.actSpeedR, self.actSpeedL) time.sleep(0.100)
def run(self): motors.setSpeeds(0, 0) try: while not (self.stopped): vout = self.voltage() # if vout < 70 or vout > 260: # print('out of range', vout) # break output = int(self.pid(vout)) if not (output > maxOutput or output < (-1 * maxOutput)): motors.motor1.setSpeed(output) time.sleep(0.01) finally: print('Resistance finall called') motors.setSpeeds(0, 0)
def reciever(client_sock): motorspeeds = [] try: while True: data = client_sock.recv(1024) for i in data: if i == "q": return elif i == "m": print("received: %s" % motorspeeds) if len(motorspeeds) == 2: motors.setSpeeds(-motorspeeds[0], motorspeeds[1]) motorspeeds = [] else: i = struct.unpack('B', i)[0] motorspeeds.append(int((i - 50) * 5)) except IOError: pass
def loop(): """ Main loop, Gets blocks from pixy, analyzes target location, chooses action for robot and sends instruction to motors """ global throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError_prev, distError_prev, panLoop, killed if ser.in_waiting: print "reading line from serial.." code = ser.readline().rstrip() print "Got IR code %s" % code killed = True if killed: motors.setSpeeds(0, 0) time.sleep(5) currentTime = datetime.now() drive() return run_flag
def drive(): if not allow_move: return if advance < 0: say("Backup up. Beep. Beep. Beep.") print "Drive: Backing up. Beeeep...Beeeep...Beeeep" #print "Drive: advance=%2.2f, throttle=%2.2f, diffDrive=%2.2f, bias=%2.2f" % (advance, throttle, diffDrive, bias) # synDrive is the drive level for going forward or backward (for both wheels) synDrive = advance * (1 - diffDrive) * throttle * totalDrive leftDiff = bias * diffDrive * throttle * totalDrive rightDiff = -bias * diffDrive * throttle * totalDrive #print "Drive: synDrive=%2.2f, leftDiff=%2.2f, rightDiff=%2.2f" % (synDrive, leftDiff, rightDiff) # construct the drive levels LDrive = (synDrive + leftDiff) RDrive = (synDrive + rightDiff) # Make sure that it is outside dead band and less than the max if LDrive > deadband: if LDrive > MAX_MOTOR_SPEED: LDrive = MAX_MOTOR_SPEED elif LDrive < -deadband: if LDrive < -MAX_MOTOR_SPEED: LDrive = -MAX_MOTOR_SPEED else: LDrive = 0 if RDrive > deadband: if RDrive > MAX_MOTOR_SPEED: RDrive = MAX_MOTOR_SPEED elif RDrive < -deadband: if RDrive < -MAX_MOTOR_SPEED: RDrive = -MAX_MOTOR_SPEED else: RDrive = 0 # Actually Set the motors motors.setSpeeds(int(LDrive), int(RDrive))
def end_of_track(channel): # What to do when the track ends kick_watchdog() time.sleep(.1) global reverse global currentStep if GPIO.input(channel) is GPIO.LOW: # We hit the FRONT side of track if reverse: reverse = False print("Forward") currentStep=offset motors.setSpeeds(240,240) time.sleep(.1) GPIO.add_event_detect(20,GPIO.RISING,callback=take_reading,bouncetime=(stoptime+11)*1000) else: if not reverse: reverse = True print("Backward") GPIO.remove_event_detect(20) motors.setSpeeds(MAX_SPEED*-1,MAX_SPEED*-1)
def shoot(): def getValue(x, y, t): if(t): if(math.copysign(1, x) != math.copysign(1, y)): return (int)((-y + x) * 240) else: return (int)((-y + x) * 480) else: if(math.copysign(1, x) == math.copysign(1, y)): return (int)((-y - x) * 240) else: return (int)((-y - x) * 480) while True: buttons = [] for e in pygame.event.get(): if(e.type == pygame.JOYAXISMOTION): if(e.axis == 0): lx = e.value elif(e.axis == 1): ly = e.value if(e.type == pygame.JOYBUTTONDOWN): buttons.append(e.button) #else: #sys.exit() motors.setSpeeds(getValue(lx, ly, True), getValue(lx, ly, False)) if 14 in buttons: shoot() if R2 in buttons: if golfAngle > 0: golfAngle -= 10 golfServo.ChangeDutyCycle(golfAngle) if L2 in buttons: if golfAngle < 180: golfAngle += 10 golfServo.ChangeDutyCycle(golfAngle)
def final_pid(wall): sensor_array = mes_sense() if wall == "both": while(sensor_array[0]< left_thresh and sensor_array[2]<right_thresh): sensor_array = mes_sense() error = PID_calc(sensor_array) motor_control(error) elif wall == "left": while (sensor_array[0]< left_thresh and sensor_array[2]>right_thresh): sensor_array = mes_sense() error = side_PID_calc(sensor_array[0]) motor_control(-error) elif wall == "right": while (sensor_array[0]> left_thresh and sensor_array[2]<right_thresh): sensor_array = mes_sense() error = side_PID_calc(sensor_array[2]) motor_control(error) elif wall=="no_wall": while (sensor_array[1]>13): #print(sensor_array[1]) turn_motors(left_base,right_base) sensor_array = mes_sense() if (sensor_array[1]<10): break motors.setSpeeds(0,0)
def better_right(phi): teildrehung = 10 motors.setSpeeds(MAX_SPEED,0) for i in range(int(phi/teildrehung)): if i % 2 == 0: motors.setSpeeds(MAX_SPEED,0) else: motors.setSpeeds(0,-MAX_SPEED) if phi - i*teildrehung >= teildrehung: sleep((ttau*teildrehung/360)*1.1) else: sleep((ttau*(phi - teildrehung * i)/360)*1.1) motors.setSpeeds(0,0) sleep(0.2)
def drive_new(): # synDrive is the drive level for going forward or backward (for both wheels) throttle = 0.1 synDrive = advance * throttle * totalDrive diffDrive = 1 #totalDrive = 1 biasRescale = 0.1 leftDiff = -bias * biasRescale * diffDrive * throttle * totalDrive rightDiff = bias * biasRescale * diffDrive * throttle * totalDrive print "leftDiff is " + str(leftDiff) # construct the drive levels LDriveRaw = (synDrive + leftDiff) RDriveRaw = (synDrive + rightDiff) LPError_motorL.calculate(LDriveRaw) LPError_motorR.calculate(RDriveRaw) LDrive = LPError_motorL.currentValue RDrive = LPError_motorR.currentValue # Make sure that it is outside dead band and less than the max if LDrive > deadband: if LDrive > MAX_MOTOR_SPEED: LDrive = MAX_MOTOR_SPEED elif LDrive < -deadband: if LDrive < -MAX_MOTOR_SPEED: LDrive = -MAX_MOTOR_SPEED else: LDrive = 0 if RDrive > deadband: if RDrive > MAX_MOTOR_SPEED: RDrive = MAX_MOTOR_SPEED elif RDrive < -deadband: if RDrive < -MAX_MOTOR_SPEED: RDrive = -MAX_MOTOR_SPEED else: RDrive = 0 # Actually Set the motors motors.setSpeeds(int(LDrive), int(RDrive))
def drive_new(): # synDrive is the drive level for going forward or backward (for both wheels) throttle = 0.1 synDrive = advance * throttle * totalDrive diffDrive = 1 #totalDrive = 1 biasRescale = 0.1 leftDiff = - bias * biasRescale * diffDrive * throttle * totalDrive rightDiff = bias * biasRescale * diffDrive * throttle * totalDrive print "leftDiff is " + str(leftDiff) # construct the drive levels LDriveRaw = (synDrive + leftDiff) RDriveRaw = (synDrive + rightDiff) LPError_motorL.calculate(LDriveRaw) LPError_motorR.calculate(RDriveRaw) LDrive = LPError_motorL.currentValue RDrive = LPError_motorR.currentValue # Make sure that it is outside dead band and less than the max if LDrive > deadband: if LDrive > MAX_MOTOR_SPEED: LDrive = MAX_MOTOR_SPEED elif LDrive < -deadband: if LDrive < -MAX_MOTOR_SPEED: LDrive = -MAX_MOTOR_SPEED else: LDrive = 0 if RDrive > deadband: if RDrive > MAX_MOTOR_SPEED: RDrive = MAX_MOTOR_SPEED elif RDrive < -deadband: if RDrive < -MAX_MOTOR_SPEED: RDrive = -MAX_MOTOR_SPEED else: RDrive = 0 # Actually Set the motors motors.setSpeeds(int(LDrive), int(RDrive))
def main(): # Connect wiimote print 'Put Wiimote in discoverable mode now (press 1+2)' global wiimote wiimote = cwiid.Wiimote() wiimote.mesg_callback = wiimote_callback # Activate button reporting wiimote.rpt_mode = cwiid.RPT_BTN wiimote.enable(cwiid.FLAG_MESG_IFC); # Speed indicator set_leds() print('Ready to go. "x" to exit.') exit = False # Demonstrate that motors are working quarter_speed = int(MAX_SPEED/4) motors.setSpeeds(quarter_speed, quarter_speed) time.sleep(0.25) motors.setSpeeds(-quarter_speed, -quarter_speed) time.sleep(0.25) motors.setSpeeds(0,0) # Infinite loop / press x to quit while not exit: c = sys.stdin.read(1) if c == 'x': exit = True wiimote.close()
def axle_to_node(): # Number of loops depend on calibration cnt = (cal - 100) / 2 # Correct drifts to the left or right while driving while cnt: (L, M, R) = read_sensors() if L == 1 and R == 0: motors.setSpeeds(v1, v2) elif R == 1 and L == 0: motors.setSpeeds(v2, v1) else: motors.setSpeeds(v2, v2) sleep (delay) cnt -= 1 motors.setSpeeds(v2, v2)
def run_engines(): motors.setSpeeds(300, 300) #motors.motor1.setSpeed(350) #motors.motor2.setSpeed(350) time.sleep(.2) motors.motor2.setSpeed(-350) time.sleep(.5) motors.setSpeeds(300, 300) time.sleep(.2) motors.motor2.setSpeed(-350) time.sleep(.5) motors.setSpeeds(300, 300) time.sleep(.2) motors.motor2.setSpeed(-350) time.sleep(.5) motors.setSpeeds(-300, -300) time.sleep(.2) motors.motor2.setSpeed(-350) time.sleep(.3)
def turn_180(): # speed-up and slow-down for smooth movement sleep(0.2) motors.setSpeeds(-v1, v1) sleep(0.1) motors.setSpeeds(-v3, v3) sleep(0.5) motors.setSpeeds(-v1, v1) # After ca. 165 degrees: snap the track (L, M, R) = read_sensors() while M == 0: (L, M, R) = read_sensors() sleep(delay)
def better_right(phi): teildrehung = 10 for i in range(int(phi/teildrehung)): if i % 2 == 0: motors.setSpeeds(0,-MAX_SPEED) else: motors.setSpeeds(MAX_SPEED,0) if phi - i*teildrehung >= teildrehung: sleep(ttau*teildrehung/360) else: sleep(phi - i*teildrehung) motors.setSpeeds(MAX_SPEED*POWER_LEFT,-MAX_SPEED*POWER_RIGHT) sleep(ttau*phi/360*0.5)
def finish(*result): # Stand still for a moment print (*result) motors.setSpeeds(0, 0) sleep (0.5) # Depending on success ("HOORAY!") or failure ... f = 1 if result[0] == "HOORAY!" else -1 # ... nod or shake your head 4 times. for x in range (0, 4): motors.setSpeeds(f * -v1, -v1) drive (0.1) motors.setSpeeds(f * v1, v1) drive (0.1) motors.setSpeeds(0, 0) # Loop forever while True: sleep (1)
def better_left(phi): motors.setSpeeds(-MAX_SPEED*POWER_LEFT, MAX_SPEED*POWER_RIGHT) sleep(ttau*phi/360*0.5)
LDrive = -MAX_MOTOR_SPEED else: LDrive = 0 if RDrive > deadband: if RDrive > MAX_MOTOR_SPEED: RDrive = MAX_MOTOR_SPEED elif RDrive < -deadband: if RDrive < -MAX_MOTOR_SPEED: RDrive = -MAX_MOTOR_SPEED else: RDrive = 0 # Actually Set the motors motors.setSpeeds(int(LDrive), int(RDrive)) if __name__ == '__main__': setup() while True: print 'we run loop' ok = loop() if not ok: print 'not work' break pixy.pixy_close() motors.setSpeeds(0, 0) print "Robot Shutdown Completed"
def left(dt): motors.setSpeeds(MAX_SPEED, 0) sleep(dt)
def sigterm_handler(signal, frame): motors.setSpeeds(0, 0) sys.exit(0)
#!/usr/bin/env python from __future__ import print_function from pololu_drv8835_rpi import motors, MAX_SPEED from time import sleep import signal, sys def sigterm_handler(signal, frame): motors.setSpeeds(0, 0) sys.exit(0) signal.signal(signal.SIGTERM, sigterm_handler) try: while True: motors.setSpeeds(0, 0) print ("left forward") motors.setSpeeds(MAX_SPEED, 0) sleep (2) print ("left backward") motors.setSpeeds(-MAX_SPEED, 0) sleep (2) print ("right forward") motors.setSpeeds(0, MAX_SPEED) sleep (2) print ("right backward") motors.setSpeeds(0, -MAX_SPEED) sleep (2)
def excite_sequence(): '''Moving sequence for excited turret''' print("Turret excite sequence...") for i in range(1): motors.setSpeeds(300, 300) time.sleep(0.05) motors.setSpeeds(0, 0) time.sleep(.4) motors.setSpeeds(300, 300) time.sleep(0.05) motors.setSpeeds(0, 0) time.sleep(.1) motors.setSpeeds(300, 300) time.sleep(0.05) motors.setSpeeds(0, 0) time.sleep(.1)
def left(phi): motors.setSpeeds(0, MAX_SPEED) sleep(ttau*phi/360)
def forward(dt): motors.setSpeeds(MAX_SPEED, MAX_SPEED) sleep(dt)
def right(dt): motors.setSpeeds(0, MAX_SPEED) sleep(dt)
def __init__(self): motors.setSpeeds(0, 0) self.MAX_THROTTLE_POSITION = 100
def right(phi): motors.setSpeeds(MAX_SPEED, 0) sleep(ttau*phi/360)