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 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 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 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)
