def auto_brake(self):
if self.motor_speed > 0:
self.motor_speed -= self.motor_speed_increment
elif self.motor_speed < 0:
self.motor_speed += self.motor_speed_increment
motors.setSpeeds(self.motor_speed, self.motor_speed)
time.sleep(self.motor_run_time)
def turn_right():
motors.enable()
if not disable_motor_flag:
motors.setSpeeds(-turn_motor_speed, turn_motor_speed)
time.sleep(motor_turn_time)
motors.setSpeeds(0, 0)
motors.disable()
def callback_function(message):
cmd_pwr = int(message.data * float(MAX_SPEED))
rospy.loginfo("Command Recieved: %s" % cmd_pwr)
motors.enable()
motors.setSpeeds(cmd_pwr, cmd_pwr)
def __init__(self):
rospy.init_node('listener', anonymous=True)
self.cmdvel_sub = rospy.Subscriber("/cmd_vel", Twist, self.cmdvel_cb)
motors.enable()
motors.setSpeeds(0, 0)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def main():
global bus
global busloc
global axis_tilt
global axis_azi
initsensor(bus, busloc)
timezone = pytz.timezone(STRTZ)
motors.enable()
motors.setSpeeds(0, 0)
RUNNING = True
last_set_val = 0
last_set_time = 0
while RUNNING:
curtime = datetime.datetime.now()
curday = curtime.strftime("%Y-%m-%d")
mystrtime = curtime.strftime("%Y-%m-%d %H:%M:%S")
epochtime = int(time.time())
mydate = timezone.localize(curtime)
curalt, curaz = get_alt_az(mydate)
cur_r = mydeg(get_pos())
track_err = False
if curalt > 0:
# We only check if there is a track error if the sun is up, no point in correcting all night long
if math.fabs(math.fabs(cur_r) - math.fabs(last_set_val)) > 2.0:
print("%s - Track error, going to set track_err to true: cur_r: %s - last_set_val: %s" % (mystrtime, cur_r, last_set_val))
track_err = True
sun_r = getR(curalt, curaz, axis_tilt, axis_azi)
if INVERT_SENSOR:
sun_r = -sun_r
print("%s - Sun is up! - Sun Alt: %s - Sun Azi: %s - Cur Rot: %s - Potential Sun Rot: %s" % (mystrtime, curalt, curaz, cur_r, sun_r))
NEW_SET_VAL = None
if sun_r <= EAST_ANGLE and sun_r >= WEST_ANGLE:
print("%s - Potential new val: %s - cur: %s" % (mystrtime, sun_r, cur_r))
NEW_SET_VAL = sun_r
elif sun_r > EAST_ANGLE and (last_set_val != EAST_ANGLE or track_err == True):
print("%s - Sun Rot (%s) is Beyond East(%s), and array needs to move there" % (mystrtime, sun_r, EAST_ANGLE))
NEW_SET_VAL = EAST_ANGLE
elif sun_r < WEST_ANGLE and (last_set_val != WEST_ANGLE or track_err == True):
print("%s - Sun Rot (%s) is Beyond West(%s), and array needs to move there" % (mystrtime, sun_r, WEST_ANGLE))
NEW_SET_VAL = WEST_ANGLE
if epochtime - last_set_time >= MOVE_INTERVAL and NEW_SET_VAL is not None:
print("%s Setting New val: %s from %s" % (mystrtime, NEW_SET_VAL, cur_r))
last_set_time = epochtime
last_set_val = NEW_SET_VAL
goto_angle(NEW_SET_VAL)
else:
if last_set_val != NIGHT_POS:
print("%s - Sun is down setting to %s for the night" % (mystrtime, NIGHT_POS))
goto_angle(NIGHT_POS)
last_set_val = NIGHT_POS
last_set_time = epochtime
time.sleep(60)
def set_motors_cmd(self, msg):
x = msg.linear.x
z = msg.angular.z
if x:
s1, s2 = self.motor_linear_speed(msg.linear.x)
motors.setSpeeds(s1, s2)
raiseIfFault()
time.sleep(0.002)
else:
s1, s2 = self.motor_angular_speed(msg.angular.z)
motors.setSpeeds(s1, s2)
raiseIfFault()
time.sleep(0.002)
def main():
motors.enable()
motors.setSpeeds(0, 0)
inp = ""
while inp != "q":
try:
inp = input("f, b, s, or q: ")
except:
print("\nNo fun exiting")
motors.setSpeeds(0, 0)
motors.disable()
sys.exit(0)
if inp == "f":
motors.motor1.setSpeed(-480)
elif inp == "b":
motors.motor1.setSpeed(480)
elif inp == "s":
motors.motor1.setSpeed(0)
elif inp == "q":
motors.setSpeeds(0, 0)
motors.disable()
sys.exit(0)
else:
print("nope")
def main():
global bus
global busloc
initsensor(bus, busloc)
timezone = pytz.timezone(STRTZ)
motors.enable()
motors.setSpeeds(0, 0)
inp = ""
while inp != "q":
cur_pos = mydeg(get_pos())
try:
inp = input("Current Angle: %s - a, f, b, s, or q: " % cur_pos)
except:
print("\nNo fun exiting")
motors.setSpeeds(0, 0)
motors.disable()
sys.exit(0)
if inp == "f":
motors.motor1.setSpeed(-480)
elif inp == "b":
motors.motor1.setSpeed(480)
elif inp == "s":
motors.motor1.setSpeed(0)
elif inp == "q":
motors.setSpeeds(0, 0)
motors.disable()
sys.exit(0)
elif inp == "a":
print(
"Oh cool, you want to set an angle! Type it here, oh, use whole numbers for now"
)
print("")
myangle = input(
"Set a whole number angle between %s (East) and %s (West): " %
(EAST_ANGLE, WEST_ANGLE))
if 1 == 1:
ang = int(myangle)
print("Using %s as your angle (you entered %s, we int() it)" %
(ang, myangle))
print("Setting Now")
goto_angle(ang)
print("Set complete")
print("")
else:
print("We couldn't int your angle, ignoring you for now")
continue
else:
print("nope")
def turn_left(self):
motors.setSpeeds(self.turn_motor_speed, -self.turn_motor_speed)
time.sleep(self.motor_turn_time)
def drive(left, right):
speed = convertInput(left, right)
if speed is not False:
motors.setSpeeds(speed[0], speed[1])
def raiseIfFault():
if motors.motor1.getFault():
raise DriverFault(1)
if motors.motor2.getFault():
raise DriverFault(2)
MAX_MESSAGE_SIZE = 1024
if __name__ == "__main__":
while True:
print('awaiting connection...')
connection, client_address = s.accept()
print('client_address %s' % client_address)
try:
motors.setSpeeds(0, 0)
print('established connection with', client_address)
while True:
message = connection.recv(MAX_MESSAGE_SIZE)
print('message: {}'.format(message))
if not message:
break
data = json.loads(message.decode('utf-8'))
if data['commands'] == 'FORWARD':
motors.setSpeeds(MAX_SPEED / 2.03, MAX_SPEED / 2)
elif data['commands'] == 'BACKWARD':
motors.setSpeeds(-MAX_SPEED / 2.03, -MAX_SPEED / 2)
elif data['commands'] == 'LEFT':
def turn_right(self):
motors.setSpeeds(-self.turn_motor_speed, self.turn_motor_speed)
time.sleep(self.motor_turn_time)
def move_back(self):
motors.setSpeeds(self.motor_speed, self.motor_speed)
time.sleep(self.motor_run_time)
if (self.motor_speed > -self.max_allowed_speed):
self.motor_speed -= self.motor_speed_increment
while on == True:
dir = input(">> ")
if dir == "exit":
break
try:
dir = int(dir)
if dir >= 0 and dir <= MAX_SPEED:
s = dir
else:
print("type int between 0 and " + str(MAX_SPEED) + " to change speed")
except:
if dir == "r":
s = MAX_SPEED//2
print("speed reset to " + str(MAX_SPEED//2))
elif dir == "w":
motors.setSpeeds(s, s)
sleep(0.5)
motors.setSpeeds(0, 0)
elif dir == "s":
motors.setSpeeds(-s, -s)
sleep(0.5)
motors.setSpeeds(0, 0)
elif dir == "a":
motors.setSpeeds(-s, s)
sleep(0.5)
motors.setSpeeds(0, 0)
elif dir == "d":
motors.setSpeeds(s, -s)
sleep(0.5)
motors.setSpeeds(0, 0)
elif dir == "x":
#Round off the float to an integer so the motor controller can read it.
motoraxis = int(round(motoraxis))
return motoraxis
def callback(data):
rospy.loginfo("%s", data.data)
return data.data
##############################################################################
################ End of Definitions, beginning code execution ################
##############################################################################
motors.enable() #Keagan initializing motors
rover_enabled = True
motors.setSpeeds(0, 0) #Keagan guaranteeing motors are stopped
print msg
rospy.init_node('Rover',anonymous=True)
#Keagan -- is this where I should place my operating code?
print ('Send commands to rover now...')
while True:
ch = rospy.Subscriber("rover/cmd_vel",String,callback)
print ch
#Exit
if ch == '\x03':
motors.disable()
rover_enabled = False
exit(0)
def send_motor_pwr(left, right): motors.enable() motors.setSpeeds(int(left * float(MAX_SPEED)), int(right * float(MAX_SPEED)))
def shut_motors(self):
motors.setSpeeds(0, 0)
motors.disable()
def key_rover(ch,prev_ch,motorspeed,motors_enabled):
#check if this is the first time executing this function
while ch:
print ch
#Rover Motors Re-Enable
if ch == 'h':
if motors_enabled == False:
motors.enable()
motors_enabled = True
print ('Rover Motors Enabled. To disable, press \';\'')
#Don't allow any other input until motors re-enabled
elif motors_enabled == False:
print ('Rover Motors Disabled. To enable, press \'h\'')
#Rover Motors Disable
elif ch == ';':
if motors_enabled == True:
motors.disable()
motors_enabled = False
print ('Rover Motors Disabled. To enable, press \'h\'')
#Rover Forward
elif ch == 'i':
print ('Rover Forward')
motors.setSpeeds(motorspeed,motorspeed)
#Rover Backward
elif ch == 'k':
print ('Rover Backward')
motors.setSpeeds(-motorspeed,-motorspeed)
#Rover Pivot Right
elif ch == 'l':
print ('Rover Pivot Right')
motors.setSpeeds(motorspeed,-motorspeed)
#Rover Pivot Left
elif ch == 'j':
print ('Rover Pivot Left')
motors.setSpeeds(-motorspeed,motorspeed)
#Rover Speed Up
elif ch == 'o':
#check that the motorspeed is not MAX_SPEED
if(motorspeed == MAX_SPEED):
print ('Rover is at maximum defined speed of %d.', motorspeed)
else:
motorspeed = motorspeed + SPEED_INTERVAL;
#check if the increase put motorspeed over MAX_SPEED & correct
if(motorspeed >= MAX_SPEED):
motorspeed == MAX_SPEED
print ('Rover Speed Up: %d', motorspeed)
#Rover Slow Down
elif ch == 'u':
#check that the motorspeed is not 0
if(motorspeed == 0):
print ('Rover is at minimum speed of %d.', motorspeed)
else:
motorspeed = motorspeed - SPEED_INTERVAL;
#check if the decrease put motorspeed under 0 & correct
if(motorspeed <= 0):
motorspeed == 0
print ('Rover Slow Down: %d', motorspeed)
#Rover stop if no key input
else:
motors.setSpeeds(0, 0)
print ('Rover Stop')
prev_ch = 'Stopped'
return motorspeed, motors_enabled
from time import sleep
import serial
ser = serial.Serial('/dev/ttyUSB0', 9600)
motors.enable()
def arduino_input():
'''
Reads serial input from arduino, which gives values in the form:
b'-480 -480\r\n'
Splits this to get the two values, removes extra characters,
then returns the speed of both motors as a list
'''
motor_speeds = []
read_serial = ser.readline()
new = str(read_serial).split()
try:
motor_speeds.append(int(new[0][2:]))
motor_speeds.append(int(new[1][:-5]))
return motor_speeds
except:
print(new)
pass
while True:
speed = arduino_input()
motors.setSpeeds(speed[0], speed[1])
sleep(0.01)
if motors.motor2.getFault():
raise DriverFault(2)
if motors.motor3.getFault():
raise DriverFault(3)
if motors.motor4.getFault():
raise DriverFault(4)
# Set up sequences of motor speeds.
test_forward_speeds = list(range(0, MAX_SPEED, 1)) + \
[MAX_SPEED] * 200 + list(range(MAX_SPEED, 0, -1)) + [0]
test_reverse_speeds = list(range(0, -MAX_SPEED, -1)) + \
[-MAX_SPEED] * 200 + list(range(-MAX_SPEED, 0, 1)) + [0]
motors.setSpeeds(0, 0)
print("\n")
print("Created by Aaron Smith and Thomas McDonald")
print(
"w- forward full speed\ne- forward halfspeed\ns- back full speed\nd- back halfspeed\nb- bilge\np- pitch"
)
print("\n")
shiftcounter = 0
speedvar = MAX_SPEED
halfspeed = (MAX_SPEED * .5)
while True: # making a loop
try:
def move_forward(self):
motors.setSpeeds(self.motor_speed, self.motor_speed)
time.sleep(self.motor_run_time)
if (self.motor_speed < self.max_allowed_speed):
self.motor_speed += self.motor_speed_increment
