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.servoWrite(L, l)
RPL.servoWrite(R, r)
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()
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
from bsmLib import RPL
RPL.init()
import post_to_web as PTW
RPL.pinMode(1, RPL.INPUT)
RPL.analogRead(1)
frana = 1
rfana = 2
rbana = 3
brana = 4
blana = 5
lbana = 6
lfana = 7
flana = 8
while True:
PTW.state['FRanalog'] = FRanalog
PTW.state['BRanalog'] = BRanalog
PTW.state['LFanalog'] = LFanalog
PTW.state['FLanalog'] = FLanalog
PTW.state['RFanalog'] = RFanalog
PTW.state['BLanalog'] = BLanalog
PTW.state['LBanalog'] = LBanalog
PTW.state['RBanalog'] = RBanalog
PTW.post()
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 reverse():
RPL.servoWrite(motorL, back)
RPL.servoWrite(motorR, back)
def forward():
RPL.servoWrite(motorL, go)
RPL.servoWrite(motorR, go)
MIN = 1000 # Min servo speed
MAX = 2000 # Max servo speed
# Servo Pins
L = 0
R = 1
######################
## 0. Setup
######################
# Create tcp connection and listen
t = tcpServer(HOST, PORT)
t.listen()
RPL.init() # Init RoboPi hat connection
######################
## 1. drive
######################
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))
from bsmLib import RPL RPL.init() # Init RoboPiLib RPL.servoWrite(0, 1000) # Write to pin 0 value 1000
from bsmLib import RPL
RPL.init()
import curses, time
screen = curses.initscr()
curses.noecho()
curses.halfdelay(1)
swivel_pin = 1
key = ''
key_hit = time.time()
while key != ord('1'):
key = screen.getch()
screen.clear()
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.servoWrite(swivel_pin, 0)
if key == ord('1'):
RPL.servoWrite(swivel_pin, 0)
curses.endwin()
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)
# Servo Pins
L = 0
R = 1
# PWM
PERIOD = 3000
######################
## 0. Setup
######################
# Create tcp connection and listen
t = tcpServer(HOST, PORT)
t.listen()
RPL.init() # Init RoboPi hat connection
# Set to PWM pin mode
RPL.pinMode(L, RPL.PWM)
RPL.pinMode(R, RPL.PWM)
######################
## 1. drive
######################
def drive():
d = t.recv()
if d == "stop":
RPL.servoWrite(L, 0)
RPL.servoWrite(R, 0)
t.stop()
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)
from bsmLib import RPL
RPL.init()
sensor_pin = 5
RPL.pinMode(sensor_pin, RPL.INPUT)
def analogRead(pin):
putPacket(ANREAD, bytearray([5]), 1)
buff = getPacket()
return int(buff[3][1]) | (int(buff[3][2]) << 8)
import RPL
RPL.init()
import time
f_reverse = 1000
neutral = 1500
f_forward = 2000
#anglepersecond = 30
def DT_PWM_Speedrange():
ServoR = int(raw_input(0))
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
from bsmLib import RPL
RPL.init()
import curses, time
screen = curses.initscr()
curses.noecho()
curses.halfdelay(1)
elbow_dir = 6
elbow_pul = 5
swivel_pin = 1
speed = 500
key = ''
key_hit = time.time()
RPL.pinMode(elbow_pul, RPL.PWM)
RPL.pinMode(elbow_dir, RPL.OUTPUT)
RPL.pinModw(shoulder_pul, RPL.PWM)
RPL.pinMode(shoulder_dir, RPL.OUTPUT)
while key != ord('1'):
key = screen.getch()
screen.clear()
if key == ord('w'):
screen.addstr('Moving elbow up')
RPL.digitalWrite(elbow_dir, 0)
RPL.pwmWrite(elbow_pul, speed, speed * 2)
key_hit = time.time()
if key == ord('s'):
screen.addstr('Moving elbow down')
RPL.digitalWrite(elbow_dir, 1)
elif key.upper() == 'E': #increase z value
z_up()
if test() == False:
z_down()
elif key.upper() == 'Q': #decrease z value
z_down()
if test() == False:
z_up()
quit = False #for breaking the motor loop with the '1' key command
import math #to calculate all angle values and error
try: #if not connected to a RoboPi, it can still run
from bsmLib import RPL
RPL.init()
elbow_motor_speed = 300
shoulder_motor_speed = 300
distance_of_error = 3 #max distance arm can be away from intended point
max_error = distance_of_error / math.sqrt(
3) #to convert the error to the intiger unit of measurement
shoulder_range = 840 #range of read value for the shoulder POT
elbow_range = 775 #range of read value for the elbow POT
swivel_range = 800 #(NUMBER HERE: NEED TO FIND POT AND ITS RANGE FOR THIS) #range of read value for the swivel POT
shoulder_pul = 1 #shoulder pulse pin
shoulder_dir = 2 #shoulder direction pin
from bsmLib import RPL
RPL.init()
import curses, time
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'):
def slow_reverse():
RPL.servoWrite(motorL, slowback)
RPL.servoWrite(motorR, slowback)
def DT_PWM_Speedrange():
ServoR = int(raw_input(0))
RPL.pinMode(ServoR, RPL.PWM)
ServoL = int(raw_input(1))
RPL.pinMode(ServoL, RPL.PWM)
from bsmLib import RPL
RPL.init()
from marvelmind import MarvelmindHedge
from time import sleep
import sys
sensor_pin = 5
RPL.pinMode(sensor_pin, RPL.INPUT)
def analogRead(pin):
putPacket(ANREAD, bytearray([5]), 1)
buff = getPacket()
return int(buff[3][1]) | (int(buff[3][2]) << 8)
# import RPL
# RPL.init()
# import time
# f_reverse = 1000 ADD IF NECESSARY FOR TALONS
# neutral = 1500
# f_forward = 2000
# •anglepersecond = 30
#def DT_PWM_Speedrange():
#ServoR = int(raw_input(0))
#RPL.pinMode(ServoR, RPL.PWM)
#ServoL = int(raw_input(1))
#RPL.pinMode(ServoL, RPL.PWM)
hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=None,
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)])
from bsmLib import RPL RPL.init() import time, math import post_to_web as PTW x = "yes" # motors motorL = 0 motorR = 1 # analog sensors fana = 2 bana = 3 lana = 7 bl_ana = 5 br_ana = 4 fdig = 0, 1 # speeds go = 1700 slowgo = 1600 back = 1300 slowback = 1400 # readings #Fanalog = RPL.analogRead(fana) #Banalog = RPL.analogRead(bana) #Lanalog = RPL.analogRead(lana) #fsensor = RPL.analogRead(fdig)