def drive():
d = t.recv()
if d == "stop":
RPL.servoWrite(L, 0)
RPL.servoWrite(R, 0)
t.stop()
exit()
d = d.split(' ')
l = int(map(float(d[0]), -1, 1, MIN, MAX))
r = int(map(float(d[1]), -1, 1, MIN, MAX))
RPL.pwmWrite(L, l, PERIOD)
RPL.pwmWrite(R, r, PERIOD)
def drive_forward():
while True:
sleep(2)
c_read()
while distance > 20:
RPL.servoWrite(L, 2000)
RPL.servoWrite(R, 1000)
if distance < 20:
break
while distance < 20:
RPL.servoWrite(L, 0)
RPL.servoWrite(R, 0)
if distance > 20:
break
def drive_forward():
distance = 0
while True:
vec0 = s.c_read()
distance = f_distance(vec0, vec1)
if distance > 20:
RPL.servoWrite(L, 2000)
RPL.servoWrite(R, 1000)
elif distance < 20:
RPL.servoWrite(L, 0)
RPL.servoWrite(R, 0)
else:
print("KYS")
exit()
from bsmLib import map from bsmLib import RPL # Min/Max servo values min = 1000 max = 2000 # Min/Max math values math_min = -1 math_max = 1 # Math Value val = 0 # Midpoint of math values # Map math value to be in servo range of Min/Max val = map(val, math_min, math_max, min, max) # Returns midpoint of servo values # Write value to servo RPL.servoWrite(0, val)
def motor_runner(x, y, z): #sends signals to all the motors based on potentiometer readings
w = math.sqrt(x ** 2 + y ** 2 + z ** 2) #the momentary length of the arm
a_elbow = round(abs(math.acos((d_one ** 2 + d_two ** 2 - w ** 2) / (2 * d_one * d_two)) - math.pi), 4) #the elbow angle
a_shoulder = round(math.acos((w ** 2 + d_one ** 2 - d_two ** 2) / (2 * d_one * w)) + math.acos(math.sqrt(x ** 2 + z ** 2) / w), 4) #the shoulder angle
try:
a_swivel = round(math.atan(z / x) + math.pi / 2, 4) #the swivel angle
except:
a_swivel = math.pi / 2 #the swivel angle when its angle doesn't matter
try:
# start w/ grasper
if gopen == 0:
RPL.servoWrite(gpin, 1400)
elif gclose == 0:
RPL.servoWrite(gpin, 1700)
else:
RPL.servoWrite(gpin, 0)
#move shoulder motor
pot_shoulder = RPL.analogRead(ppin_shoulder) * 3 * math.pi / (2 * shoulder_range) - math.pi / 4
error_s = abs(pot_shoulder - a_shoulder) #how many degrees off the intended value the arm is
calculated_error_s = error_s * d_two
if calculated_error_s > max_error:
if pot_shoulder > a_shoulder:
RPL.digitalWrite(shoulder_dir, 0) #turn clockwise
else:
RPL.digitalWrite(shoulder_dir, 1) #turn counterclockwise
RPL.pwmWrite(shoulder_pul, shoulder_motor_speed, shoulder_motor_speed * 2)
else:
RPL.pwmWrite(shoulder_pul, 0, shoulder_motor_speed * 2) #stops running while in range
#move elbow motor
pot_elbow = RPL.analogRead(ppin_elbow) * 3 * math.pi / (2 * elbow_range) - math.pi / 4
error_e = abs(pot_elbow - a_elbow) #how many degrees off the intended value the arm is
calculated_error_e = error_e * d_two
if calculated_error_e > max_error:
if pot_elbow > a_elbow:
RPL.digitalWrite(elbow_dir, 0) #turn clockwise
else:
RPL.digitalWrite(elbow_dir, 1) #turn counterclockwise
RPL.pwmWrite(elbow_pul, elbow_motor_speed, elbow_motor_speed * 2)
else:
RPL.pwmWrite(elbow_pul, 0, elbow_motor_speed * 2) #stops running while in range
#move swivel motor
pot_swivel = RPL.analogRead(ppin_swivel) * 3 * math.pi / (2 * swivel_range) - math.pi / 4
error_sw = abs(pot_swivel - a_swivel) #how many degrees off the intended value the arm is
if error_sw > max_error:
if pot_swivel > a_swivel:
RPL.servoWrite(swivel_continuous, 2000) #turn clockwise
else:
RPL.servoWrite(swivel_continuous, 1000) #turn counterclockwise
else:
RPL.servoWrite(swivel_continuous, 0) #stops running while in range
def navigation():
#Pulls respective values of the hedge for x and y from the position array
MobileX = float(hedge.position()[1])
MobileY = float(hedge.position()[2])
# analogRead(1) = front left
# analogRead(2) = front right
# analogRead(3) = side left
# analogRead(4) = side right
while True:
MobileX = hedge.position()[1]
MobileY = hedge.position()[2]
if RPL.analogRead(1) > 200 and analogRead(
2) < 200: # front left sensor tank turn
RPL.servoWrite(0, 1000)
RPL.servoWrite(1, 2000)
print "1"
elif RPL.analogRead(2) > 200 and analogRead(
1) < 200: # tank turn left
RPL.servoWrite(0, 2000)
RPL.servoWrite(1, 1000)
print "2"
elif RPL.analogRead(3) > 200: # bank turn right
RPL.servoWrite(0, 1450)
RPL.servoWrite(1, 1000)
print "3"
elif RPL.analogRead(4) > 200: # bank turn left
RPL.servoWrite(0, 1000)
RPL.servoWrite(1, 1450)
print "4"
elif 0 > MobileX and 0 > MobileY:
RPL.servoWrite(0, 1000) # drive straight
RPL.servoWrite(1, 1000)
print "a"
elif MobileX < 0 and MobileY > 0:
RPL.servoWrite(0, 1450) # turn right
RPL.servoWrite(1, 1000)
print "b"
elif MobileX > 0 and MobileY < 0:
RPL.servoWrite(0, 1000) # turn left
RPL.servoWrite(1, 1450)
print "c"
elif 1 > MobileX > -1 and 1 > MobileY > -1:
RPL.servoWrite(0, 0) # stop
RPL.servoWrite(1, 0)
print "d"
elif MobileX > 1 or MobileY > 1:
RPL.servoWrite(0, 2000) # backwards
RPL.servoWrite(1, 2000)
print "e"
print MobileX
print MobileY
hedge.print_position()
def slow_reverse():
RPL.servoWrite(motorL, slowback)
RPL.servoWrite(motorR, slowback)
RPL.pinMode(ServoR, RPL.PWM)
ServoL = int(raw_input(1))
RPL.pinMode(ServoL, RPL.PWM)
# analogRead(1) = side slant left
# analogRead(2) = side slant right
# analogRead(3) = front left
# analogRead(4) = front right
# analogRead(5) = side left
# analogRead(6) = side right
# analogRead(7) = front slant left
# analogRead(8) = front slant right
while True:
if RPL.analogRead(7) > 250: # spin right
RPL.servoWrite(0, 2000)
RPL.servoWrite(1, 2000)
if RPL.analogRead(3) > 250 or RPL.analogRead(
5
) > 250: # turns right if a wall or object on the left side is detected
RPL.servoWrite(0, 1450)
RPL.servoWrite(1, 2000)
if RPL.analogRead(4) > 250 or RPL.analogRead(
6
) > 250: # turns left if a wall or object on the right side is detected
RPL.servoWrite(0, 1000)
RPL.servoWrite(1, 1550)
if 50 > RPL.analogRead(1) - RPL.analogRead(2) > -50: # drives straight
RPL.servoWrite(0, 1000)
RPL.servoWrite(1, 2000)
if RPL.analogRead(2) > RPL.analogRead(
def hardR():
RPL.servoWrite(motorL, slowgo) #TURN RIGHT TURN RIGHT
RPL.servoWrite(motorR, slowback)
def slightR():
RPL.servoWrite(motorL, go) #TURN LEFT TURN LEFT
RPL.servoWrite(motorR, slowgo)
def slightL():
RPL.servoWrite(motorL, slowgo)
RPL.servoWrite(motorR, go)
def hardL():
RPL.servoWrite(motorL, slowback) #TURN LEFT TURN LEFT
RPL.servoWrite(motorR, slowgo)
def stop():
RPL.servoWrite(motorL, 1500)
RPL.servoWrite(motorR, 1500)
def forward():
RPL.servoWrite(motorL, go)
RPL.servoWrite(motorR, go)
from bsmLib import RPL RPL.init() # Init RoboPiLib RPL.servoWrite(0, 1000) # Write to pin 0 value 1000
RPL.digitalWrite(elbow_dir, 1)
RPL.pwmWrite(elbow_pul, speed, speed * 2)
key_hit = time.time()
if key == ord('o'):
screen.addstr('Moving shoulder up')
RPL.digitalWrite(shoulder_dir, 0)
RPL.pwmWrite(shoulder_pul, speed, speed * 2)
key_hit = time.time()
if key == ord('l'):
screen.addstr('Moving shoulder down')
RPL.digitalWrite(shoulder_dir, 1)
RPL.pwmWrite(shoulder_pul, speed, speed * 2)
key_hit = time.time()
if key == ord('q'):
screen.addstr('Moving swivel clockwise')
RPL.servoWrite(swivel_pin, 2000)
key_hit = True
if key == ord('e'):
screen.addstr('Moving swivel counterclockwise')
RPL.servoWrite(swivel_pin, 1000)
key_hit = True
if time.time() - key_hit > 0.5:
screen.addstr('Stopped')
RPL.pwmWrite(elbow_pul, 0, speed * 2)
RPL.pwmWrite(shoulder_pul, 0, speed * 2)
RPL.servoWrite(swivel_pin, 0)
if key == ord('1'):
RPL.pwmWrite(elbow_pul, 0, speed * 2)
RPL.pwmWrite(shoulder_pul, 0, speed * 2)
RPL.servoWrite(swivel_pin, 0)
curses.endwin()
screen = curses.initscr()
curses.noecho()
curses.halfdelay(1)
motor_pin = 2
key = ''
key_down = time.time()
while key != ord('q'):
key = screen.getch()
screen.clear()
screen.addstr(0, 0, 'Hit "q" to quit, and "a" or "d" to turn')
screen.addstr(1, 0, 'Current movement state:')
if key == ord('a'):
RPL.servoWrite(motor_pin, 1400)
key_down = time.time()
screen.addstr(1, 24, 'Counterclockwise')
if key == ord('d'):
RPL.servoWrite(motor_pin, 1600)
key_down = time.time()
screen.addstr(1, 24, 'Clockwise')
if time.time() - key_down > 0.5:
RPL.servoWrite(motor_pin, 0)
screen.addstr(1, 24, 'Stopped')
if key == ord('q'):
RPL.servoWrite(motor_pin, 0)
curses.endwin()
RPL.servoWrite(motor_pin, 0)
def motor_runner(
): #sends signals to all the motors based on potentiometer readings
while quit == False:
reach_length = math.sqrt(x**2 + y**2 +
z**2) #the momentary length of the arm
a_elbow = round(
abs(
math.acos((d_one**2 + d_two**2 - reach_length**2) /
(2 * d_one * d_two)) - math.pi), 4) #the elbow angle
a_shoulder = round(
math.acos((reach_length**2 + d_one**2 - d_two**2) /
(2 * d_one * reach_length)) +
math.acos(math.sqrt(x**2 + z**2) / reach_length),
4) #the shoulder angle
try:
a_swivel = round(math.atan(z / x) + math.pi / 2,
4) #the swivel angle
except:
a_swivel = math.pi / 2 #the swivel angle when its angle doesn't matter
try: #move shoulder motor
pot_shoulder = RPL.analogRead(ppin_shoulder) * 3 * math.pi / (
2 * shoulder_range) - math.pi / 4
error_s = abs(
pot_shoulder - a_shoulder
) #how many degrees off the intended value the arm is
calculated_error_s = error_s * d_two
if calculated_error_s > max_error:
if pot_shoulder > a_shoulder:
RPL.digitalWrite(shoulder_dir, 0) #turn clockwise
else:
RPL.digitalWrite(shoulder_dir, 1) #turn counterclockwise
RPL.pwmWrite(shoulder_pul, shoulder_motor_speed,
shoulder_motor_speed * 2)
else:
RPL.pwmWrite(shoulder_pul, 0, shoulder_motor_speed *
2) #stops running while in range
#move elbow motor
pot_elbow = RPL.analogRead(ppin_elbow) * 3 * math.pi / (
2 * elbow_range) - math.pi / 4
error_e = abs(
pot_elbow -
a_elbow) #how many degrees off the intended value the arm is
calculated_error_e = error_e * d_two
if calculated_error_e > max_error:
if pot_elbow > a_elbow:
RPL.digitalWrite(elbow_dir, 0) #turn clockwise
else:
RPL.digitalWrite(elbow_dir, 1) #turn counterclockwise
RPL.pwmWrite(elbow_pul, elbow_motor_speed,
elbow_motor_speed * 2)
else:
RPL.pwmWrite(elbow_pul, 0, elbow_motor_speed *
2) #stops running while in range
#move swivel motor
pot_swivel = RPL.analogRead(ppin_swivel) * 3 * math.pi / (
2 * swivel_range) - math.pi / 4
error_sw = abs(
pot_swivel -
a_swivel) #how many degrees off the intended value the arm is
if error_sw > max_error:
if pot_swivel > a_swivel:
RPL.servoWrite(swivel_continuous, 2000) #turn clockwise
else:
RPL.servoWrite(swivel_continuous,
1000) #turn counterclockwise
else:
RPL.servoWrite(swivel_continuous,
0) #stops running while in range
if quit == True: #stop the motors when the code ends
RPL.servoWrite(swivel_continuous,
0) #stops running while in range
RPL.pwmWrite(elbow_pul, 0, elbow_motor_speed *
2) #stops running while in range
RPL.pwmWrite(shoulder_pul, 0, shoulder_motor_speed *
2) #stops running while in range
except: #to show the values of the motor arm
from time import sleep
sleep(0.5)
print('[elbow, shoulder, swivel]:', [
round(a_elbow, 4),
round(a_shoulder, 4),
round(a_swivel, 4)
], '[Speed]:', [speed], '[x, y, z]:',
[round(x, 2), round(y, 2),
round(z, 2)])
def reverse():
RPL.servoWrite(motorL, back)
RPL.servoWrite(motorR, back)