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