def volt(self):
_ifMutexAcquire(self.use_mutex)
try:
voltage = gopigo.volt()
except:
voltage = 0
_ifMutexRelease(self.use_mutex)
return voltage
def process_command(_command, _time):
data = _command
if not data:
print "received data:", data
if len(data) != 1:
print ("Invalid command")
return "Invalid command"
elif data == 'w':
gopigo.fwd()
# return "Moving forward"
elif data == 'x':
gopigo.stop()
# return "Stopping"
elif data == 's':
gopigo.bwd()
# return "Moving back"
elif data == 'a':
gopigo.left()
# return "Turning left"
elif data == 'd':
gopigo.right()
# return "Turning right"
elif data == 't':
gopigo.increase_speed()
# return "Increase speed"
elif data == 'g':
gopigo.decrease_speed()
# return "Decrease speed"
elif data == 'v':
# print gopigo.volt(), 'V'
return str(gopigo.volt())
elif data == 'l':
gopigo.led_on(0)
gopigo.led_on(1)
time.sleep(1)
gopigo.led_off(0)
gopigo.led_off(1)
return "Flash LED"
else:
print ("Invalid command")
return "Invalid command" # run as a app
if _time:
time.sleep(_time)
gopigo.stop()
import gopigo
try:
import wx
except ImportError:
raise ImportError, "The wxPython module is required to run this program"
import atexit
atexit.register(gopigo.stop)
left_led = 0
right_led = 0
trim_val = gopigo.trim_read()
if trim_val == -3:
trim_val = 0
v = gopigo.volt()
f = gopigo.fw_ver()
slider_val = trim_val
class MainPanel(wx.Panel):
slider = 0
def __init__(self, parent):
wx.Panel.__init__(self, parent=parent)
self.SetBackgroundStyle(wx.BG_STYLE_CUSTOM)
self.SetBackgroundColour(wx.WHITE)
self.frame = parent
# main sizer
main_sizer = wx.BoxSizer(wx.VERTICAL)
import MoveRobot
import RobotPosePrediction
import SenseLandmarks
import gopigo
import time
import numpy
#MoveRobot.go_towards_nearest_obstacle()
#MoveRobot.go_forward(22.86)
#MoveRobot.go_backwards(100)
MoveRobot.go_towards_nearest_obstacle()
print("Pose is now ", RobotPosePrediction.currentRobotPose)
print("Uncertainty is now ",
RobotPosePrediction.currentRobotLocationUncertainty)
print("Voltage ", gopigo.volt())
def battery_button_OnButtonClick(self,event):
global v
v=gopigo.volt()
self.battery_label.SetLabel(str(v)+"V")
except:
no_auto_detect = True
import gopigo
try:
import wx
except ImportError:
raise ImportError,"The wxPython module is required to run this program"
import atexit
atexit.register(gopigo.stop)
left_led=0
right_led=0
trim_val=gopigo.trim_read()
v=gopigo.volt()
f=gopigo.fw_ver()
slider_val=gopigo.trim_read()
class gopigo_control_app(wx.Frame):
slider=0
def __init__(self,parent,id,title):
wx.Frame.__init__(self,parent,id,title,size=(475,600))
self.parent = parent
self.initialize()
# Exit
exit_button = wx.Button(self, label="Exit", pos=(240+75,550))
exit_button.Bind(wx.EVT_BUTTON, self.onClose)
# robot = "/home/pi/Desktop/GoBox/Troubleshooting_GUI/dex.png"
# png = wx.Image(robot, wx.BITMAP_TYPE_ANY).ConvertToBitmap()
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 volt():
I2C_Mutex_Acquire()
voltage = gopigo.volt()
I2C_Mutex_Release()
return voltage
def volt():
_wait_for_read()
_grab_read()
voltage = gopigo.volt()
_release_read()
return voltage
def main():
speed = INITIAL_SPEED
gopigo.trim_write(TRIM)
time.sleep(0.1)
print("Volt: " + str(gopigo.volt()))
time.sleep(0.1)
initial_ticks_left = enc_read(LEFT)
time.sleep(0.1)
initial_ticks_right = enc_read(RIGHT)
print("Initial\tL: " + str(initial_ticks_left) + "\tR: " +
str(initial_ticks_right))
print("Critical: " + str(CRITICAL_DISTANCE))
print("Safe: " + str(SAFE_DISTANCE))
print("Alert: " + str(ALERT_DISTANCE))
dists = collections.deque(maxlen=SAMPLE_SIZE)
dts = collections.deque(maxlen=SAMPLE_SIZE)
elapsed_ticks_left = 0
elapsed_ticks_right = 0
try:
gopigo.set_speed(0)
gopigo.fwd()
t = time.time()
while True:
if speed < 0:
speed = 0
print("========================")
#if not command_queue.empty():
# comm = command_queue.get()
# global MAX_SPEED
# global SAFE_DISTANCE
# MAX_SPEED = comm[0]
# SAFE_DISTANCE = comm[1]
# print(comm)
dt = time.time() - t
t = time.time()
#time.sleep(0.1)
print("Time: " + str(dt))
dist = get_dist()
print("Dist: " + str(dist))
if not isinstance(dist, str):
dists.append(float(dist))
dts.append(float(dt))
rel_speed = None
if len(dists) > 9:
rel_speed = calculate_relative_speed(dists, dts)
print("Rel speed: " + str(rel_speed))
if (isinstance(dist, str)
and dist != NOTHING_FOUND) or dist < CRITICAL_DISTANCE:
print("< Critical")
stop_until_safe_distance()
speed = 0
t = time.time()
elif dist < SAFE_DISTANCE:
print("< Safe")
if speed > STOP_THRESHOLD:
#speed = speed - dt * SPEED_DECCELLERATION
speed = speed - dt * get_deccelleration(speed)
else:
speed = 0
elif speed > MAX_SPEED:
print("Slowing down")
speed = speed - dt * SLOWING_DECCELLERATION
elif dist < ALERT_DISTANCE and rel_speed is not None:
speed = handle_alert_distance(speed, rel_speed, dt)
elif speed < MAX_SPEED:
print("Speeding up")
speed = speed + dt * SPEED_ACCELERATION
#speed = speed - dt * get_deccelleration(speed)
elapsed_ticks_left, elapsed_ticks_right = \
read_enc_ticks(initial_ticks_left, initial_ticks_right)
print("L: " + str(elapsed_ticks_left) + "\tR: " +
str(elapsed_ticks_right))
l_diff, r_diff = straightness_correction(speed, elapsed_ticks_left,
elapsed_ticks_right)
if elapsed_ticks_left >= 0 and elapsed_ticks_right >= 0:
set_speed_lr(speed, l_diff, r_diff)
else:
set_speed_lr(speed, 0, 0)
print("Speed: " + str(speed))
except (KeyboardInterrupt, Exception):
traceback.print_exc()
gopigo.stop()
gopigo.stop()
''' Helper to show charge of the batteries in the rover ''' import gopigo print gopigo.volt()
cv2.circle(img, (redPos, 100), int(15 / scale), (0, 0, 255), -1)
cv2.circle(img, (greenPos, 180), int(15 / scale), (0, 255, 0), -1)
cv2.circle(img, (bluePos, 260), int(15 / scale), (255, 0, 0), -1)
cv2.putText(img, "Enc", (redPos, 100 - 50), cv2.FONT_HERSHEY_SIMPLEX, 2,
(0, 0, 255), 2)
cv2.putText(img, "US", (greenPos, 180 - 50), cv2.FONT_HERSHEY_SIMPLEX, 2,
(0, 255, 0), 2)
cv2.putText(img, "Cam", (bluePos, 260), cv2.FONT_HERSHEY_SIMPLEX, 2,
(255, 0, 0), 2)
img = cv2.resize(img, (0, 0), fx=scale, fy=scale)
cv2.imshow('map', img)
cv2.waitKey(1)
print("Battery voltage: " + str(go.volt()))
ser = serial.Serial('/dev/ttyUSB0', 9600)
lineSensorOffset = 0
try:
_thread.start_new_thread(slowThread, ()) # Start the second thread.
go.set_speed(60)
ls1 = 0
ls2 = 0
ls3 = 0
ls4 = 0
ls5 = 0
clp = 0
dist = -1
def _perceive(self):
if not gopigo_available:
return None
return gopigo.volt()
conn.send("Stopping")
elif data == 's':
gopigo.bwd()
conn.send("Moving back")
elif data == 'a':
gopigo.left()
conn.send("Turning left")
elif data == 'd':
gopigo.right()
conn.send("Turning right")
elif data == 't':
gopigo.increase_speed()
conn.send("Increase speed")
elif data == 'g':
gopigo.decrease_speed()
conn.send("Decrease speed")
elif data == 'v':
# print gopigo.volt(), 'V'
conn.send(str(gopigo.volt()))
elif data == 'l':
gopigo.led_on(0)
gopigo.led_on(1)
time.sleep(1)
gopigo.led_off(0)
gopigo.led_off(1)
conn.send("Flash LED")
else:
print ("Invalid command")
conn.send("Invalid command")
conn.close()
def battery_button_OnButtonClick(self, event):
global v
v = gopigo.volt()
self.battery_label.SetLabel(str(v) + "V")
def process_command(self, command):
parts = command.split("/")
if parts[1] == "poll":
print "poll"
self.us_dist = gopigo.us_dist(usdist_pin)
self.enc_status = gopigo.read_status()[0]
self.volt = gopigo.volt()
self.fw_ver = gopigo.fw_ver()
self.trim = gopigo.trim_read() - 100
if self.enc_status == 0:
self.waitingOn = None
elif parts[1] == "stop":
gopigo.stop()
elif parts[1] == "trim_write":
gopigo.trim_write(int(parts[2]))
self.trim = gopigo.trim_read()
elif parts[1] == "trim_read":
self.trim = gopigo.trim_read() - 100
elif parts[1] == "set_speed":
if parts[2] == "left":
self.left_speed = int(parts[3])
elif parts[2] == "right":
self.right_speed = int(parts[3])
else:
self.right_speed = int(parts[3])
self.left_speed = int(parts[3])
gopigo.set_left_speed(self.left_speed)
gopigo.set_right_speed(self.right_speed)
elif parts[1] == "leds":
val = 0
if parts[3] == "on":
val = 1
elif parts[3] == "off":
val = 0
elif parts[3] == "toggle":
val = -1
if parts[2] == "right" or parts[2] == "both":
if val >= 0:
self.ledr = val
else:
self.ledr = 1 - self.ledr
if parts[2] == "left" or parts[2] == "both":
if val >= 0:
self.ledl = val
else:
self.ledl = 1 - self.ledl
gopigo.digitalWrite(ledr_pin, self.ledr)
gopigo.digitalWrite(ledl_pin, self.ledl)
elif parts[1] == "servo":
gopigo.servo(int(parts[2]))
elif parts[1] == "turn":
self.waitingOn = parts[2]
direction = parts[3]
amount = int(parts[4])
encleft = 0 if direction == "left" else 1
encright = 1 if direction == "left" else 0
gopigo.enable_encoders()
gopigo.enc_tgt(encleft, encright, int(amount / DPR))
if direction == "left":
gopigo.left()
else:
gopigo.right()
elif parts[1] == "move":
self.waitingOn = int(parts[2])
direction = parts[3]
amount = int(parts[4])
gopigo.enable_encoders()
gopigo.enc_tgt(1, 1, amount)
if direction == "backward":
gopigo.bwd()
else:
gopigo.fwd()
elif parts[1] == "beep":
gopigo.analogWrite(buzzer_pin, self.beep_volume)
time.sleep(self.beep_time)
gopigo.analogWrite(buzzer_pin, 0)
elif parts[1] == "reset_all":
self.ledl = 0
self.ledr = 0
gopigo.digitalWrite(ledl_pin, self.ledl)
gopigo.digitalWrite(ledr_pin, self.ledr)
gopigo.analogWrite(buzzer_pin, 0)
# gopigo.servo(90)
gopigo.stop()
def volt():
_wait_for_read()
_grab_read()
voltage = gopigo.volt()
_release_read()
return voltage
import MoveRobot
import RobotPosePrediction
import SenseLandmarks
import gopigo
import time
import numpy
#MoveRobot.go_towards_nearest_obstacle()
#MoveRobot.go_forward(22.86)
#MoveRobot.go_backwards(100)
MoveRobot.go_towards_nearest_obstacle()
print("Pose is now ", RobotPosePrediction.currentRobotPose)
print("Uncertainty is now ", RobotPosePrediction.currentRobotLocationUncertainty)
print("Voltage ", gopigo.volt())
def _perceive(self):
if not gopigo_available:
return None
return gopigo.volt()
def volt(kargs):
r = {'return_value': gopigo.volt()}
return r
