def __init__( self ):
gopigo.set_speed(0)
gopigo.fwd()
self.lastServoSettingsSendTime = 0.0
self.lastUpdateTime = 0.0
self.lastMotionCommandTime = time.time()
def __init__(self):
'''
On Init, set speed to half-way, so GoPiGo is predictable
and not too fast.
'''
DEFAULT_SPEED = 128
gopigo.set_speed(DEFAULT_SPEED)
def main():
hedge = MarvelmindHedge(tty="/dev/ttyACM0", adr=10,
debug=False) # create MarvelmindHedge thread
hedge.start() # start thread
sleep(.1)
start_pos = convert2D(hedge.position())
print hedge.position()
print start_pos
new_pos = start_pos
dst = 0.0
gopigo.set_speed(100)
gopigo.fwd()
try:
while (abs(dst) < 1):
sleep(.1)
new_pos = convert2D(hedge.position())
hedge.print_position()
dst = distance(start_pos, new_pos)
print "start: ", start_pos
print "current: ", new_pos
print "distance: ", dst
except KeyboardInterrupt:
hedge.stop() # stop and close serial port
gopigo.stop()
sys.exit()
hedge.stop()
gopigo.stop()
def reset_speed(self):
_grab_read()
try:
gopigo.set_speed(DEFAULT_SPEED)
except:
pass
_release_read()
def set_speed(self, new_speed):
_grab_read()
try:
gopigo.set_speed(new_speed)
except:
pass
_release_read()
def __init__(self, default_motor_speed=50):
gopigo.set_speed(default_motor_speed)
gopigo.stop()
#gopigo.fwd()
self.lastServoSettingsSendTime = 0.0
self.lastUpdateTime = 0.0
self.lastMotionCommandTime = time.time()
def set_speed(self,new_speed):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.set_speed(new_speed)
except:
pass
finally:
_ifMutexRelease(self.use_mutex)
def reset_speed(self):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.set_speed(self.DEFAULT_SPEED)
except:
pass
finally:
_ifMutexRelease(self.use_mutex)
def set_speed(self,new_speed):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.set_speed(new_speed)
except:
pass
finally:
_ifMutexRelease(self.use_mutex)
def __init__(self,default_motor_speed=50):
gopigo.set_speed(default_motor_speed)
gopigo.stop()
#gopigo.fwd()
self.lastServoSettingsSendTime = 0.0
self.lastUpdateTime = 0.0
self.lastMotionCommandTime = time.time()
def reset_speed(self):
_ifMutexAcquire(self.use_mutex)
try:
gopigo.set_speed(self.DEFAULT_SPEED)
except:
pass
finally:
_ifMutexRelease(self.use_mutex)
def __init__(self):
gopigo.set_speed(200)
gopigo.stop()
#gopigo.fwd()
self.lastServoSettingsSendTime = 0.0 #son servo ayarları gonderme suresi
self.lastUpdateTime = 0.0 #son guncelleme zamanı
self.lastMotionCommandTime = time.time(
) #son hareket komutunun geldiği zaman
def __init__(self, use_mutex = False):
'''
On Init, set speed to half-way, so GoPiGo is predictable
and not too fast.
'''
self.DEFAULT_SPEED = 128
gopigo.set_speed(self.DEFAULT_SPEED)
self.use_mutex = use_mutex
def __init__(self, use_mutex = False):
'''
On Init, set speed to half-way, so GoPiGo is predictable
and not too fast.
'''
self.DEFAULT_SPEED = 128
gopigo.set_speed(self.DEFAULT_SPEED)
self.use_mutex = use_mutex
def stop_until_safe_distance():
gopigo.stop()
dist = get_dist()
while (isinstance(dist, str)
and dist != NOTHING_FOUND) or dist < SAFE_DISTANCE:
dist = get_dist()
gopigo.set_speed(0)
gopigo.fwd()
def setMotorSpeeds():
speeds = go.read_motor_speed()
# If one motor is moving slower, change the other to match it
if speeds[0] < speeds[1]:
print 'MOTOR: Left motor moving slower'
go.set_speed(speeds[0])
elif speeds[1] < speeds[0]:
print 'MOTOR: Right motor moving slower'
go.set_speed(speeds[1])
def go_backwards(backwards_distance): gopigo.enable_encoders() encoder_steps_required = int(backwards_distance / DISTANCE_PER_ENCODER_STEP) gopigo.set_speed(DEFAULT_ROBOT_SPEED) gopigo.enc_tgt(1, 1, encoder_steps_required) gopigo.bwd() wait_till_encoder_target_reached() RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(currentRobotPose = RobotPosePrediction.currentRobotPose, movementType = RobotPosePrediction.MOVEMENT_TYPE_FORWARD, movementAmount = -1 * encoder_steps_required * DISTANCE_PER_ENCODER_STEP) RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(currentRobotLocationUncertainty = RobotPosePrediction.currentRobotLocationUncertainty, movementType = RobotPosePrediction.MOVEMENT_TYPE_FORWARD, movementAmount = -1 * encoder_steps_required * DISTANCE_PER_ENCODER_STEP, currentRobotPose = RobotPosePrediction.currentRobotPose)
def init():
global cap, gospeed
# This function should do everything required to initialize the robot.
# Among other things it should open the camera and set gopigo speed.
# Some of this has already been filled in.
# You are welcome to add your own code, if needed.
cap = cv2.VideoCapture(0)
go.set_speed(gospeed)
return
def __stop_until_safe_distance(self):
"""
Stops the rover until it achieves a safe distance from the obstacle.
This can occur when/if the obstacle moves away from the rover.
"""
gopigo.stop()
self.obstacle_distance = get_dist()
while (isinstance(self.obstacle_distance, str) and
self.obstacle_distance != NOTHING_FOUND) or \
self.obstacle_distance < self.safe_distance:
self.obstacle_distance = get_dist()
gopigo.set_speed(0)
gopigo.fwd()
def turn_right():
if msg_en:
print("Turn right")
if gpg_en:
gopigo.enc_tgt(0, 1, 15)
gopigo.fwd()
time.sleep(.9)
gopigo.stop()
time.sleep(1)
gopigo.set_speed(80)
gopigo.enc_tgt(1, 1, 35)
gopigo.right_rot()
time.sleep(.7)
gopigo.stop()
time.sleep(1)
curr = absolute_line_pos()
ic.right_turns += 1
def main():
# Sets the intitial speed to 0
go.set_speed(0)
# Sets the initial state to 0 = NORMAL
state = STATE()
# Program loop
while True:
try:
stateCheck(state)
# Shuts the ACC down when a Ctrl + c command is issued
except KeyboardInterrupt:
print '\nACC shut off'
go.stop()
sys.exit()
def go_forward(forward_distance):
gopigo.enable_encoders()
encoder_steps_required = int(forward_distance / DISTANCE_PER_ENCODER_STEP)
gopigo.set_speed(DEFAULT_ROBOT_SPEED)
gopigo.enc_tgt(1, 1, encoder_steps_required)
gopigo.fwd()
wait_till_encoder_target_reached()
RobotPosePrediction.currentRobotPose = RobotPosePrediction.getPredictedRobotPose(
currentRobotPose=RobotPosePrediction.currentRobotPose,
movementType=RobotPosePrediction.MOVEMENT_TYPE_FORWARD,
movementAmount=encoder_steps_required * DISTANCE_PER_ENCODER_STEP)
RobotPosePrediction.currentRobotLocationUncertainty = RobotPosePrediction.getPredictedRobotUncertainty(
currentRobotLocationUncertainty=RobotPosePrediction.
currentRobotLocationUncertainty,
movementType=RobotPosePrediction.MOVEMENT_TYPE_FORWARD,
movementAmount=encoder_steps_required * DISTANCE_PER_ENCODER_STEP,
currentRobotPose=RobotPosePrediction.currentRobotPose)
def turn_around():
if msg_en:
print("Turn around")
if gpg_en:
gopigo.bwd()
curr = absolute_line_pos()
gopigo.enc_tgt(0, 1, 5)
gopigo.stop()
time.sleep(.5)
gopigo.set_speed(80)
gopigo.enc_tgt(1, 1, 35)
gopigo.left_rot()
time.sleep(.7)
gopigo.stop()
time.sleep(1)
curr = absolute_line_pos()
ic.turn += 1
def __main(self):
"""
Runs the main loop of the ACC.
If an exception is thrown in the main loop or the ACC is killed via
Ctrl+c then the rover is stopped in order to prevent a collision.
"""
try:
gopigo.set_speed(0)
gopigo.fwd()
self.t = time.time()
while self.power_on:
self.__update_system_info()
self.__process_commands()
dt = time.time() - self.t
self.t = time.time()
self.__observe_obstacle(dt)
self.__calculate_relevant_distances(dt)
self.__validate_user_settings()
self.__obstacle_based_acceleration_determination(dt)
# Reset the speed if it becomes negative
if self.speed < 0:
self.speed = 0
# Reset the speed if it goes below the minimum speed
if self.user_set_speed < MIN_SPEED:
self.speed = 0
l_diff, r_diff = self.__straightness_correction()
self.__actualize_power(l_diff, r_diff)
# Catch any exceptions that occur
except (KeyboardInterrupt, Exception):
traceback.print_exc()
gopigo.stop() # Stop to prevent a collision
gopigo.stop()
def main():
# Sets the intitial speed to 0
go.set_speed(50)
# Sets the initial encoder values
encoders = Encoders(*read_encoders())
# Sets the initial state to 0 = NORMAL
state = STATE()
# Program loop
while True:
try:
calculateEncodersError(encoders)
stateCheck(state)
# Shuts the ACC down when a Ctrl + c command is issued
except KeyboardInterrupt:
print '\nACC shut off'
go.stop()
sys.exit()
def follow_line(self, fwd_speed=30):
slight_turn_speed = int(35)
print("FOLLOWING LINE")
while True:
self.read_position()
self.read_position()
#print(self.read())
pos = self.read_position()
#print(pos)
debug(pos)
if pos == "center":
gopigo.set_speed(30)
gopigo.forward()
elif pos == "wayleft":
gopigo.set_right_speed(25)
gopigo.set_left_speed(40)
elif pos == "wayright":
gopigo.set_right_speed(40)
gopigo.set_left_speed(25)
elif pos == "left":
gopigo.set_right_speed(25)
gopigo.set_left_speed(slight_turn_speed)
elif pos == "right":
gopigo.set_right_speed(slight_turn_speed)
gopigo.set_left_speed(25)
elif pos == "white":
print("white Brake")
gopigo.stop()
break
elif pos == "intersection":
gopigo.stop()
break
elif pos == "unknown":
print("Unknown???")
gopigo.stop()
break
else:
break
def wait_for_button():
gopigo.stop()
while (gopigo.digitalRead(button_pin) != 1):
try:
time.sleep(.5)
except IOError:
print ("Error")
print "Button pressed!"
gopigo.set_speed(gopigo_speed)
distance = 100
while (distance > distance_from_body):
try:
distance = gopigo.us_dist(distance_sensor_pin)
print ("Distance: " + str(distance))
time.sleep(.1)
gopigo.fwd()
except IOError:
print ("Error")
gopigo.stop()
sound("Hello!")
def wait_for_button():
gopigo.stop()
while (gopigo.digitalRead(button_pin) != 1):
try:
time.sleep(.5)
except IOError:
print("Error")
print "Button pressed!"
gopigo.set_speed(gopigo_speed)
distance = 100
while (distance > distance_from_body):
try:
distance = gopigo.us_dist(distance_sensor_pin)
print("Distance: " + str(distance))
time.sleep(.1)
gopigo.fwd()
except IOError:
print("Error")
gopigo.stop()
sound("Hello!")
def main():
go.set_speed(MIN_SPEED)
INIT_LEFT_ENC, INIT_RIGHT_ENC = read_encoders()
print('Left: ', INIT_LEFT_ENC, ' Right: ', INIT_RIGHT_ENC)
time.sleep(5)
go.forward()
while True:
try:
if STATE == 3:
go.stop()
else:
go.forward()
print 'MOTORS: ', go.read_motor_speed()
rs.rs(INIT_LEFT_ENC, INIT_RIGHT_ENC)
stateCheck()
# Shuts the ACC down when a Ctrl + c command is issued
except KeyboardInterrupt:
print '\nACC shut off'
go.stop()
sys.exit()
# Calibrate speed at first run # 100 is good with fresh batteries # 125 for batteries with half capacity fwd_speed = 110 # Forward speed at which the GoPiGo should run. # If you're swinging too hard around your line # reduce this speed. poll_time = 0.01 # Time between polling the sensor, seconds. slight_turn_speed = int(.7 * fwd_speed) turn_speed = int(.7 * fwd_speed) last_val = [0] * 5 # An array to keep track of the previous values. curr = [0] * 5 # An array to keep track of the current values. gopigo.set_speed(fwd_speed) gpg_en = 1 # Enable/disable gopigo msg_en = 1 # Enable messages on screen. Turn this off if you don't want messages. # Get line parameters line_pos = [0] * 5 white_line = line_sensor.get_white_line() black_line = line_sensor.get_black_line() range_sensor = line_sensor.get_range() threshold = [a + b / 2 for a, b in zip(white_line, range_sensor)] # Make an iterator that aggregates elements from each of the iterables. # Position to take action on mid = [0, 0, 1, 0, 0] # Middle Position. mid1 = [0, 1, 1, 1, 0] # Middle Position.
def set_speed(kargs):
r = {'return_value': gopigo.set_speed(int(kargs['speed']))}
return r
def _open(self):
if not gopigo_available:
return False
gopigo.set_speed(self.speed)
return True
def back_away(): gopigo.set_speed(gopigo_speed) gopigo.bwd() time.sleep(10) gopigo.stop()
def slowing(state):
if (getCurrentSpeed() >= MIN_SPEED):
go.set_speed(getCurrentSpeed() - 10)
go.forward()
else:
state.state = 0
def accelerating():
if(getCurrentSpeed() < MAX_SPEED):
go.set_speed(getCurrentSpeed() + 15)
else:
global STATE
STATE = 0
def go_back(): if msg_en: print "Go Back" if gpg_en: gopigo.set_speed(turn_speed) gopigo.bwd()
def stopped():
go.set_speed(0)
go.stop()
def accelerating(state):
if (getCurrentSpeed() < MAX_SPEED):
go.set_speed(getCurrentSpeed() + 10)
go.forward()
else:
state.state = 0
def follow_line(self):
slight_turn_speed = int(50)
default_speed = int(35)
wayoff_turn_speed = int(115)
print("FOLLOWING LINE")
gopigo.set_speed(default_speed)
gopigo.forward()
time.sleep(1)
while True:
self.read_position()
self.read_position()
#print(self.read())
pos = self.read_position()
#print(pos)
debug(pos)
if pos == "center":
gopigo.set_speed(default_speed)
gopigo.forward()
elif pos == "wayleft":
gopigo.set_right_speed(default_speed)
gopigo.set_left_speed(wayoff_turn_speed)
elif pos == "wayright":
gopigo.set_right_speed(wayoff_turn_speed)
gopigo.set_left_speed(default_speed)
elif pos == "left":
gopigo.set_right_speed(default_speed)
gopigo.set_left_speed(slight_turn_speed)
elif pos == "right":
gopigo.set_right_speed(slight_turn_speed)
gopigo.set_left_speed(default_speed)
elif pos == "white":
print("white Brake")
gopigo.stop()
time.sleep(1)
whiteTest = self.read_position()
whiteTest = self.read_position()
if whiteTest == "white":
print("really white")
break
else:
gopigo.forward()
elif pos == "intersection":
gopigo.stop()
time.sleep(1)
gopigo.set_right_speed(40)
gopigo.set_left_speed(40)
gopigo.forward()
time.sleep(1.2)
gopigo.stop()
time.sleep(1)
intersectionTest = self.read_position()
intersectionTest = self.read_position()
print("Reading found!" + intersectionTest)
gopigo.backward()
time.sleep(1.5)
gopigo.stop()
if intersectionTest != "white":
print("interesection hit!")
else:
print("T intersection hit!")
break
elif pos == "left corner" or "right corner":
gopigo.stop()
time.sleep(1)
gopigo.set_right_speed(40)
gopigo.set_left_speed(40)
gopigo.forward()
time.sleep(1.2)
gopigo.stop()
time.sleep(1)
intersectionTest = self.read_position()
intersectionTest = self.read_position()
print("Reading found! " + intersectionTest)
gopigo.backward()
time.sleep(1.5)
gopigo.stop()
if intersectionTest != "white":
print("T interesection hit!")
break
else:
print("corner! turning!")
if pos == "left corner":
self.turn_left()
else:
self.turn_right()
gopigo.forward()
time.sleep(.5)
elif pos == "unknown":
print("Unknown???")
#gopigo.stop()
#break
else:
break
print(pos)
gopigo.stop()
gopigo.motor2(1, 255)
time.sleep(2)
print("Motor 2 moving forward at half speed")
gopigo.motor2(1, 127)
time.sleep(2)
print("Motor 2 stopping ")
gopigo.motor2(1, 0)
time.sleep(1)
print("Motor 2 moving back at full speed")
gopigo.motor2(0, 255)
time.sleep(2)
print("Motor 2 moving back at half speed")
gopigo.motor2(0, 127)
time.sleep(2)
print("Motor 2 stopping")
gopigo.motor2(1, 0)
spd = gopigo.read_motor_speed()
print ("Current speed M1:%d ,M2:%d " % (spd[0], spd[1]))
print("Changing speed")
gopigo.set_speed(200) # Setting motor speed to 200, so that the motors still move when the next program uses it
spd = gopigo.read_motor_speed()
print ("New speed M1:%d ,M2:%d " % (spd[0], spd[1]))
def slowing():
if(getCurrentSpeed() >= MIN_SPEED):
go.set_speed(getCurrentSpeed() - 5)
else:
global STATE
STATE = 0
def stopped():
go.set_speed(MIN_SPEED)
go.stop()
def turn_left(): if msg_en: print "Turn left" if gpg_en: gopigo.set_speed(turn_speed) gopigo.left()
#!/usr/bin/env python # arguemtn : speed : 0 - 255 import gopigo gopigo.set_speed(sys.argv[1])
def turn_right(): if msg_en: print "Turn right" if gpg_en: gopigo.set_speed(turn_speed) gopigo.right()
def do_command(command=None):
logging.debug(command)
if command in ["forward", "fwd"]:
gopigo.fwd()
elif command == "left":
gopigo.left()
elif command == "left_rot":
gopigo.left_rot()
elif command == "right":
gopigo.right()
elif command == "right_rot":
gopigo.right_rot()
elif command == "stop":
gopigo.stop()
elif command == "leftled_on":
gopigo.led_on(0)
elif command == "leftled_off":
gopigo.led_off(0)
elif command == "rightled_on":
gopigo.led_on(1)
elif command == "rightled_off":
gopigo.led_off(1)
elif command in ["back", "bwd"]:
gopigo.bwd()
elif command == "speed":
logging.debug("speed")
speed = flask.request.args.get("speed")
logging.debug("speed:" + str(speed))
if speed:
logging.debug("in if speed")
gopigo.set_speed(int(speed))
left_speed = flask.request.args.get("left_speed")
logging.debug("left_speed:" + str(left_speed))
if left_speed:
logging.debug("in if left_speed")
gopigo.set_left_speed(int(left_speed))
right_speed = flask.request.args.get("right_speed")
logging.debug("right_speed:" + str(right_speed))
if right_speed:
logging.debug("in if right_speed")
gopigo.set_right_speed(int(right_speed))
speed_result = gopigo.read_motor_speed()
logging.debug(speed_result)
return flask.json.jsonify({"speed": speed_result, "right": speed_result[0], "left": speed_result[1]})
elif command == "get_data":
speed_result = gopigo.read_motor_speed()
enc_right = gopigo.enc_read(0)
enc_left = gopigo.enc_read(1)
volt = gopigo.volt()
timeout = gopigo.read_timeout_status()
return flask.json.jsonify(
{
"speed": speed_result,
"speed_right": speed_result[0],
"speed_left": speed_result[1],
"enc_right": enc_right,
"enc_left": enc_left,
"volt": volt,
"timeout": timeout,
"fw_ver": gopigo.fw_ver(),
}
)
elif command in ["enc_tgt", "step"]:
tgt = flask.request.args.get("tgt")
direction = flask.request.args.get("dir")
if tgt:
gopigo.gopigo.enc_tgt(1, 1, int(tgt))
if dir:
if dir == "bwd":
gopigo.bwd()
else:
gopigo.fwd()
else:
gopigo.fwd()
return ""
def go_straight(): if msg_en: print "Going straight" if gpg_en: gopigo.set_speed(fwd_speed) gopigo.fwd()
val=line_sensor.read_sensor() #Add a multipler to each sensor multp=[-100,-50,0,50,100] #TRAIN for the first time #reading when all sensors are at white white=[767,815,859,710,700] #reading when all sensors are black black=[1012,1013,1015,1003,1004] #difference between black and white range_col=list(map(operator.sub, black, white)) #Calibrate at first run gopigo.set_speed(150) gpg_en=1 while True: curr=get_sensorval() #Get current difference bwtween white and line diff_val=list(map(operator.sub, curr, white)) #find how much black line each sensor is able to get #find the position of the bot # -10000 -> extreme left # 0 -> centre # 10000 -> extreme right curr_pos=0 percent_black_line=[0]*5 for i in range(5):