def bound(a):
gpg = EasyGoPiGo3()
servo1 = gpg.init_servo(port="SERVO1")
robot = gopigo3.GoPiGo3()
#while True:
#a=input()
print(a)
if a == 'w':
gpg.forward()
elif a == 'a':
gpg.right()
elif a == 'd':
gpg.left()
elif a == 's':
gpg.backward()
elif a == 'i':
gpg.drive_cm(10, True)
elif a == 'j':
gpg.backward_cm(10)
elif a == 'k':
gpg.turn_degrees(45)
elif a == 'l':
gpg.turn_degrees(-45)
elif a == 'f':
robot.set_servo(robot.SERVO_1, 1850)
#servo1.rotate_servo(-5)
time.sleep(1)
#servo1.rotate_servo(5)
robot.set_servo(robot.SERVO_1, 0)
else:
gpg.stop()
return 1
def main():
GPG = gopigo3.GoPiGo3()
rospy.init_node("ultra_sonic")
port_id = rospy.get_param("~port", 1)
if port_id == 1:
port = GPG.GROVE_1
elif port_id == 2:
port = GPG.GROVE_2
else:
rospy.logerr("unknown port %i", port_id)
return
GPG.set_grove_type(port, GPG.GROVE_TYPE.US)
pub_distance = rospy.Publisher("~distance", Range, queue_size=10)
msg_range = Range()
msg_range.header.frame_id = "ultra_sonic"
msg_range.radiation_type = Range.ULTRASOUND
msg_range.min_range = 0.02
msg_range.max_range = 4.3
rate = rospy.Rate(rospy.get_param('~hz', 30))
while not rospy.is_shutdown():
# read distance in meter
msg_range.range = GPG.get_grove_value(port) / 1000.0
msg_range.header.stamp = rospy.Time.now()
pub_distance.publish(msg_range)
rate.sleep()
def __init__(self):
"""
Instantiates the key-bindings between the GoPiGo3 and the keyboard's keys.
Sets the order of the keys in the menu.
"""
self.GPG = gopigo3.GoPiGo3()
self.gopigo3 = easy.EasyGoPiGo3()
self.keybindings = {
"w": ["Move the GoPiGo3 forward", "forward"],
"s": ["Move the GoPiGo3 backward", "backward"],
"a": ["Turn the GoPiGo3 to the left", "left"],
"d": ["Turn the GoPiGo3 to the right", "right"],
"<SPACE>": ["Stop the GoPiGo3 from moving", "stop"],
"f": ["fire the gun", "fire"],
"<F1>": ["Drive forward for 10 centimeters", "forward10cm"],
"<F2>": ["Drive forward for 10 inches", "forward10in"],
"<F3>": [
"Drive forward for 360 degrees (aka 1 wheel rotation)",
"forwardturn"
],
"1": ["Turn ON/OFF left blinker of the GoPiGo3", "leftblinker"],
"2": ["Turn ON/OFF right blinker of the GoPiGo3", "rightblinker"],
"3": ["Turn ON/OFF both blinkers of the GoPiGo3", "blinkers"],
"8": ["Turn ON/OFF left eye of the GoPiGo3", "lefteye"],
"9": ["Turn ON/OFF right eye of the GoPiGo3", "righteye"],
"0": ["Turn ON/OFF both eyes of the GoPiGo3", "eyes"],
"<INSERT>": ["Change the eyes' color on the go", "eyescolor"],
"<ESC>": ["Exit", "exit"],
}
self.order_of_keys = [
"w", "s", "a", "d", "<SPACE>", "<F1>", "<F2>", "<F3>", "1", "2",
"3", "8", "9", "0", "<INSERT>", "<ESC>"
]
def __init__(self): ## TODO: some of this should probably go in startup()?
self.api_full = gopigo3.GoPiGo3() # Create an instance of the GoPiGo3 class. GPG will be the GoPiGo3 object.
self.api_easy = easygopigo3.EasyGoPiGo3()
self.active = False
self.distance = self.api_easy.init_distance_sensor()
self.distance_servo = self.api_easy.init_servo("SERVO1")
self.camera = picamera.PiCamera()
self.camera_servo = self.api_easy.init_servo("SERVO2")
self.imu = inertial_measurement_unit.InertialMeasurementUnit(bus = "GPG3_AD1")
async def joystick_listen():
global exiting
GPG = gopigo3.GoPiGo3(
) # Create an instance of the GoPiGo3 class. GPG will be the GoPiGo3 object.
UDP_IP = "192.168.0.112"
UDP_PORT = 5005
print("UDP target IP:", UDP_IP)
print("UDP target port:", UDP_PORT)
sock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
server_address = (UDP_IP, UDP_PORT)
sock.bind(server_address)
while not exiting:
try:
data, address = sock.recvfrom(4096)
coordinatesJson = data.decode('utf-8')
coordinates = json.loads(coordinatesJson)
powerLeft = (127 - coordinates['y']) - (
(127 - coordinates['x']) / 2)
if (powerLeft > 100):
powerLeft = 100
elif (powerLeft < -100):
powerLeft = -100
powerRight = (127 - coordinates['y']) + (
(127 - coordinates['x']) / 2)
if (powerRight > 100):
powerRight = 100
elif (powerRight < -100):
powerRight = -100
if (abs(127 - coordinates['y']) < 15 &
(127 - coordinates['x']) > 110):
powerLeft = -100
powerRight = 100
elif (abs(127 - coordinates['y']) < 15 &
(127 - coordinates['x']) < -110):
powerLeft = 100
powerRight = -100
GPG.set_motor_power(GPG.MOTOR_LEFT, powerLeft)
GPG.set_motor_power(GPG.MOTOR_RIGHT, powerRight)
except Exception as e:
print(e)
# Reset the GoPiGo
GPG.reset_all(
) # Unconfigure the sensors, disable the motors, and restore the LED to the control of the GoPiGo3 firmware.
def navigate():
gpg = EasyGoPiGo3()
#fire servo
servo1 = gpg.init_servo(port="SERVO1")
#tilt servo
servo2 = gpg.init_servo(port="SERVO2")
robot = gopigo3.GoPiGo3()
rotate = 1750
robot.set_servo(robot.SERVO_2, rotate)
while True:
index()
video_feed()
a = input()
print(a)
if a == 'w':
gpg.drive_cm(2, True)
elif a == 'a':
gpg.turn_degrees(-9)
elif a == 'd':
gpg.turn_degrees(9)
elif a == 's':
gpg.drive_cm(-2)
elif a == 'i':
gpg.drive_cm(10, True)
elif a == 'k':
gpg.drive_cm(-10)
elif a == 'j':
gpg.turn_degrees(-45)
elif a == 'l':
gpg.turn_degrees(45)
elif a == 'f':
robot.set_servo(robot.SERVO_1, 1850)
#servo1.rotate_servo(-5)
time.sleep(.25)
#servo1.rotate_servo(5)
robot.set_servo(robot.SERVO_1, 0)
elif a == 't':
rotate = rotate + 20
robot.set_servo(robot.SERVO_2, rotate)
#time.sleep(1)
#robot.set_servo(robot.SERVO_2,0)
elif a == 'g':
rotate = rotate - 20
robot.set_servo(robot.SERVO_2, rotate)
#time.sleep(1)
#robot.set_servo(robot.SERVO_2,0)
elif a == '.':
screen()
time.sleep(5)
else:
gpg.stop()
def __init__(self):
self.rwheel_motorcmd = Float32()
self.lwheel_motorcmd = Float32()
# Create gopigo object driver
self.GPG = gopigo3.GoPiGo3()
# Create encoder object
self.encoder_reading = Encoder()
# Create subscriber objects
self.rwheel_motorcmd_sub = rospy.Subscriber(
"/rwheel_motorcmd", Float32, self.rwheel_motorcmd_callback)
self.lwheel_motorcmd_sub = rospy.Subscriber(
"/lwheel_motorcmd", Float32, self.lwheel_motorcmd_callback)
# Create publisher object
self.encoders_pub = rospy.Publisher("/encoder_reading",
Encoder,
queue_size=10)
def find_gopigo3():
'''
boolean function that detects the presence of a GoPiGo3
returns True or False
'''
debug_print("Detecting GoPiGo3")
try:
import gopigo3
try:
GPG3 = gopigo3.GoPiGo3()
return True
except gopigo3.FirmwareVersionError:
return True
except:
return False
except:
return False
#
# Copyright (c) 2017 Dexter Industries
# Released under the MIT license (http://choosealicense.com/licenses/mit/).
# For more information see https://github.com/DexterInd/GoPiGo3/blob/master/LICENSE.md
#
# This code is an example for using the GoPiGo3 software I2C busses.
#
# Hardware: Connect an I2C device to port AD1.
from __future__ import print_function # use python 3 syntax but make it compatible with python 2
from __future__ import division # ''
import time # import the time library for the sleep function
import gopigo3 # import the GoPiGo3 drivers
GPG = gopigo3.GoPiGo3(
) # Create an instance of the GoPiGo3 class. GPG will be the GoPiGo3 object.
I2C_Slave_Address = 0x24 # the address of the I2C slave
try:
GPG.set_grove_type(GPG.GROVE_1, GPG.GROVE_TYPE.I2C)
i = 0
while (True):
GPG.grove_i2c_transfer(GPG.GROVE_1, I2C_Slave_Address,
[i]) # write one byte
#print(GPG.grove_i2c_transfer(GPG.GROVE_1, I2C_Slave_Address, [0, 1, 0, 1], 1)) # write four bytes and read one byte
print(i, GPG.grove_i2c_transfer(GPG.GROVE_1, I2C_Slave_Address, [],
16)) # read sixteen bytes
time.sleep(0.1)
def __init__(self):
#### GoPiGo3 power management
# export pin
if not os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN):
gpio_export = os.open("/sys/class/gpio/export", os.O_WRONLY)
os.write(gpio_export, self.POWER_PIN.encode())
os.close(gpio_export)
time.sleep(0.1)
# set pin direction
gpio_direction = os.open(
"/sys/class/gpio/gpio" + self.POWER_PIN + "/direction",
os.O_WRONLY)
os.write(gpio_direction, "out".encode())
os.close(gpio_direction)
# activate power management
self.gpio_value = os.open(
"/sys/class/gpio/gpio" + self.POWER_PIN + "/value", os.O_WRONLY)
os.write(self.gpio_value, "1".encode())
# GoPiGo3 and ROS setup
self.g = gopigo3.GoPiGo3()
print("GoPiGo3 info:")
print("Manufacturer : ", self.g.get_manufacturer())
print("Board : ", self.g.get_board())
print("Serial Number : ", self.g.get_id())
print("Hardware version: ", self.g.get_version_hardware())
print("Firmware version: ", self.g.get_version_firmware())
self.reset_odometry()
rospy.init_node("gopigo3")
self.br = TransformBroadcaster()
# subscriber
rospy.Subscriber("motor/dps/left", Int16,
lambda msg: self.g.set_motor_dps(self.ML, msg.data))
rospy.Subscriber("motor/dps/right", Int16,
lambda msg: self.g.set_motor_dps(self.MR, msg.data))
rospy.Subscriber("motor/pwm/left", Int8,
lambda msg: self.g.set_motor_power(self.ML, msg.data))
rospy.Subscriber("motor/pwm/right", Int8,
lambda msg: self.g.set_motor_power(self.MR, msg.data))
rospy.Subscriber(
"motor/position/left", Int16,
lambda msg: self.g.set_motor_position(self.ML, msg.data))
rospy.Subscriber(
"motor/position/right", Int16,
lambda msg: self.g.set_motor_position(self.MR, msg.data))
rospy.Subscriber("servo/pulse_width/1", Int16,
lambda msg: self.g.set_servo(self.S1, msg.data))
rospy.Subscriber("servo/pulse_width/2", Int16,
lambda msg: self.g.set_servo(self.S2, msg.data))
rospy.Subscriber("servo/position/1", Float64,
lambda msg: self.set_servo_angle(self.S1, msg.data))
rospy.Subscriber("servo/position/2", Float64,
lambda msg: self.set_servo_angle(self.S2, msg.data))
rospy.Subscriber("cmd_vel", Twist, self.on_twist)
rospy.Subscriber("led/blinker/left", UInt8,
lambda msg: self.g.set_led(self.BL, msg.data))
rospy.Subscriber("led/blinker/right", UInt8,
lambda msg: self.g.set_led(self.BR, msg.data))
rospy.Subscriber(
"led/eye/left", ColorRGBA, lambda c: self.g.set_led(
self.EL, int(c.r * 255), int(c.g * 255), int(c.b * 255)))
rospy.Subscriber(
"led/eye/right", ColorRGBA, lambda c: self.g.set_led(
self.ER, int(c.r * 255), int(c.g * 255), int(c.b * 255)))
rospy.Subscriber(
"led/wifi", ColorRGBA, lambda c: self.g.set_led(
self.EW, int(c.r * 255), int(c.g * 255), int(c.b * 255)))
# publisher
self.pub_enc_l = rospy.Publisher('motor/encoder/left',
Float64,
queue_size=10)
self.pub_enc_r = rospy.Publisher('motor/encoder/right',
Float64,
queue_size=10)
self.pub_battery = rospy.Publisher('battery_voltage',
Float64,
queue_size=10)
self.pub_motor_status = rospy.Publisher('motor/status',
MotorStatusLR,
queue_size=10)
self.pub_odometry = rospy.Publisher("odom", Odometry, queue_size=10)
self.pub_joints = rospy.Publisher("joint_states",
JointState,
queue_size=10)
# services
self.srv_reset = rospy.Service('reset', Trigger, self.reset)
self.srv_spi = rospy.Service(
'spi', SPI, lambda req: SPIResponse(
data_in=self.g.spi_transfer_array(req.data_out)))
self.srv_pwr_on = rospy.Service('power/on', Trigger, self.power_on)
self.srv_pwr_off = rospy.Service('power/off', Trigger, self.power_off)
# main loop
rate = rospy.Rate(rospy.get_param('hz', 30)) # in Hz
while not rospy.is_shutdown():
self.pub_battery.publish(
Float64(data=self.g.get_voltage_battery()))
# publish motor status, including encoder value
(flags, power, encoder, speed) = self.g.get_motor_status(self.ML)
status_left = MotorStatus(low_voltage=(flags & (1 << 0)),
overloaded=(flags & (1 << 1)),
power=power,
encoder=encoder,
speed=speed)
self.pub_enc_l.publish(Float64(data=encoder))
(flags, power, encoder, speed) = self.g.get_motor_status(self.MR)
status_right = MotorStatus(low_voltage=(flags & (1 << 0)),
overloaded=(flags & (1 << 1)),
power=power,
encoder=encoder,
speed=speed)
self.pub_enc_r.publish(Float64(data=encoder))
self.pub_motor_status.publish(
MotorStatusLR(header=Header(stamp=rospy.Time.now()),
left=status_left,
right=status_right))
# publish current pose
(odom, transform) = self.odometry(status_left, status_right)
self.pub_odometry.publish(odom)
self.br.sendTransformMessage(transform)
rate.sleep()
self.g.reset_all()
self.g.offset_motor_encoder(self.ML, self.g.get_motor_encoder(self.ML))
self.g.offset_motor_encoder(self.MR, self.g.get_motor_encoder(self.MR))
# deactivate power management
os.write(self.gpio_value, "0".encode())
os.close(self.gpio_value)
# unexport pin
if os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN):
gpio_export = os.open("/sys/class/gpio/unexport", os.O_WRONLY)
os.write(gpio_export, self.POWER_PIN.encode())
os.close(gpio_export)
global l_speed
global r_speed
global current_angle
print("LLL")
if (l_speed > min_speed):
l_speed = l_speed - step_speed
if (r_speed < max_speed):
r_speed = r_speed + step_speed
if (current_angle > -30):
current_angle = current_angle - angle_step
################### Main ##################
if (config_no_motor == 0):
GPG = gopigo3.GoPiGo3()
# read weight
readWeights()
# Motor starts at initial speed
if (config_no_motor == 0):
GPG.set_motor_dps(GPG.MOTOR_LEFT, -1 * l_speed)
GPG.set_motor_dps(GPG.MOTOR_RIGHT, -1 * r_speed)
currentStatus[0] = 0.5 # speed
currentStatus[1] = 0.0 # angle difference
currentStatus[2] = 0.0 # -60, 0.0 = no blocking, 1.0 = very close
currentStatus[3] = 0.0 # -30
currentStatus[4] = 0.0 # 0
currentStatus[5] = 0.0 # 30
import time # import the time library for the sleep function
import gopigo3 # import the GoPiGo3 drivers
import easygopigo3 as easy
from di_sensors.light_color_sensor import LightColorSensor
import gopigo3 # import the GoPiGo3 drivers
gpg = easy.EasyGoPiGo3()
GPG2 = gopigo3.GoPiGo3()
lcs = LightColorSensor(led_state=True)
counter = 0
def TurnDegrees(degrees, speed):
# get the starting position of each motor
StartPositionLeft = GPG2.get_motor_encoder(GPG2.MOTOR_LEFT)
StartPositionRight = GPG2.get_motor_encoder(GPG2.MOTOR_RIGHT)
# the distance in mm that each wheel needs to travel
WheelTravelDistance = ((GPG2.WHEEL_BASE_CIRCUMFERENCE * degrees) / 360)
# the number of degrees each wheel needs to turn
WheelTurnDegrees = ((WheelTravelDistance / GPG2.WHEEL_CIRCUMFERENCE) * 360)
# Limit the speed
GPG2.set_motor_limits(GPG2.MOTOR_LEFT + GPG2.MOTOR_RIGHT, dps=speed)
# Set each motor target
GPG2.set_motor_position(GPG2.MOTOR_LEFT,
(StartPositionLeft + WheelTurnDegrees))
GPG2.set_motor_position(GPG2.MOTOR_RIGHT,
(StartPositionRight - WheelTurnDegrees))
import socket # Import socket module
from gopigo import * #Has the basic functions for controlling the GoPiGo Robot
import sys #Used for closing the running program
import pygame #Gives access to KEYUP/KEYDOWN events
import picamera
import os, os.path
import numpy as np
import argparse
import os
import sys
import gopigo3
camera = picamera.PiCamera()
charpre = 'p'
accelerate = 0
gpg = gopigo3.GoPiGo3()
if __name__ == '__main__':
iteration = 0
while True:
imagename = '/home/pi/Desktop/DL4GPG/capture.jpg'
camera.capture(imagename)
s = socket.socket() # Create a socket object
if (len(sys.argv) > 1):
host = sys.argv[1] # Get local machine name from command arg
else:
host = '192.168.0.102' # Get local machine name
port = 12344 # Reserve a port for your service.
s.connect((host, port))
f = open('capture.jpg', 'rb')
print 'Sending...'
l = f.read(1024)
#!/usr/bin/env python
# GoPiGo3 imports
from di_sensors import distance_sensor
import gopigo3
import time
# ROS imports
import rospy
# TODO Int16 import is now obsolete (I think)
from std_msgs.msg import Int16
from sensor_msgs.msg import LaserScan
# Instantiate GoPiGo object
objGpg = gopigo3.GoPiGo3()
# Instantiate DistanceSensor object
objDstSns = distance_sensor.DistanceSensor()
# Variables
stepSweep = 10
sweepDir = 1
def scan():
global stepSweep, sweepDir
# Maximum count to the left
countMax = 2420
# Minimum count to the right
countMin = 620
# Populate the LaserScan message
numReadings = (countMax - countMin) / abs(stepSweep)
now = rospy.get_rostime()
msgScan = LaserScan()
def run():
GPG = gopigo3.GoPiGo3()
server_socket = socket.socket()
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind(('0.0.0.0', 8002))
server_socket.listen(0)
while True:
print "Matlab server awaiting connection"
GPG.reset_all()
GPG.set_grove_type(GPG.GROVE_1, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_1, GPG.GROVE_INPUT_ANALOG)
GPG.set_grove_type(GPG.GROVE_2, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_2, GPG.GROVE_INPUT_ANALOG)
connection = server_socket.accept()[0]
print "Matlab server connected"
had_command = True
last_battery_good = time.time()
try:
while True:
# Battery check
battery = GPG.get_voltage_battery()
if battery < 10:
print "Low battery: ", battery
if time.time() > last_battery_good + 10:
print "Battery critically low, shutting down"
os.system("sudo shutdown now -h")
else:
last_battery_good = time.time()
ready = select.select([connection], [], [],
1.0)[0] # 1000ms timeout
if ready:
had_command = True
sz = struct.unpack('<L',
connection.recv(
struct.calcsize('<L')))[0]
if sz:
msg = connection.recv(sz)
msg = struct.unpack('<lll', msg)
# Apply commands
if battery > 10.5:
GPG.set_motor_dps(GPG.MOTOR_LEFT, msg[0])
GPG.set_motor_dps(GPG.MOTOR_RIGHT, msg[1])
GPG.set_servo(GPG.SERVO_1, msg[2])
else:
print "Not executing command due to low battery (", battery, "V)"
# Construct status
msg = struct.pack(
'<ll',
GPG.get_motor_encoder(GPG.MOTOR_LEFT) *
GPG.MOTOR_TICKS_PER_DEGREE,
GPG.get_motor_encoder(GPG.MOTOR_RIGHT) *
GPG.MOTOR_TICKS_PER_DEGREE)
msg = msg + struct.pack('<lllll',
*line_sensor.get_sensorval())
msg = msg + struct.pack('<f', battery)
msg = msg + struct.pack(
'<llll', grovepi.analogRead(0), grovepi.analogRead(1),
grovepi.analogRead(2), grovepi.analogRead(3))
msg = msg + struct.pack(
'<ll', GPG.get_grove_analog(GPG.GROVE_1_1),
GPG.get_grove_analog(GPG.GROVE_2_1))
msg = struct.pack('<L', len(msg)) + msg
connection.send(msg)
else:
if had_command:
print "Command timeout"
GPG.set_motor_dps(GPG.MOTOR_LEFT, 0)
GPG.set_motor_dps(GPG.MOTOR_RIGHT, 0)
had_command = False
except:
print "Matlab server error"
pass
finally:
try:
connection.close()
except:
pass
server_socket.close()
def run():
GPG = gopigo3.GoPiGo3()
last_command = time.time()
last_battery_good = time.time()
server_socket = socket.socket()
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind(('0.0.0.0', 8002))
server_socket.listen(0)
while True:
print "Data server awaiting connection"
GPG.reset_all()
GPG.set_grove_type(GPG.GROVE_1, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_1, GPG.GROVE_INPUT_ANALOG)
GPG.set_grove_type(GPG.GROVE_2, GPG.GROVE_TYPE.CUSTOM)
GPG.set_grove_mode(GPG.GROVE_2, GPG.GROVE_INPUT_ANALOG)
GPG.set_led(GPG.LED_WIFI, 0, 0, 0)
try:
grovepi.analogRead(0)
has_grove = True
except:
has_grove = False
print "Grove board detected: ", has_grove
connection = server_socket.accept()[0]
print "Data server connected"
try:
while True:
# Battery check
battery = GPG.get_voltage_battery()
if battery < 10:
print "Low battery: ", battery
if time.time() > last_battery_good + 1:
print "Battery critically low, shutting down"
os.system("shutdown now -h")
else:
last_battery_good = time.time()
ready = select.select([connection], [], [],
0.100)[0] # 100ms timeout
if ready:
sz = struct.unpack('<L',
connection.recv(
struct.calcsize('<L')))[0]
if not sz:
break
msg = ''
while len(msg) < sz:
msg = connection.recv(sz - len(msg))
# Apply commands
if sz == 12:
msg = struct.unpack('<lll', msg)
msg = msg + (0, 0, 0)
elif sz == 15:
msg = struct.unpack('<lllBBB', msg)
else:
print "Unknown message size ", sz
break
if battery > 10.5:
GPG.set_motor_dps(GPG.MOTOR_LEFT, msg[0])
GPG.set_motor_dps(GPG.MOTOR_RIGHT, msg[1])
GPG.set_servo(GPG.SERVO_1, msg[2])
GPG.set_servo(GPG.SERVO_2, msg[2])
GPG.set_led(GPG.LED_WIFI, msg[3], msg[4], msg[5])
last_command = time.time()
# Auto-stop if no commands are sent
if time.time() > last_command + 1:
print "Stopping (no data or low battery). Battery is at ", battery, "V"
last_command = time.time() + 3600
GPG.reset_all()
# Construct status
msg = struct.pack(
'<ll',
GPG.get_motor_encoder(GPG.MOTOR_LEFT) *
GPG.MOTOR_TICKS_PER_DEGREE,
GPG.get_motor_encoder(GPG.MOTOR_RIGHT) *
GPG.MOTOR_TICKS_PER_DEGREE)
msg = msg + struct.pack('<lllll', *line_sensor.get_sensorval())
msg = msg + struct.pack('<f', battery)
if has_grove:
msg = msg + struct.pack(
'<llll', grovepi.analogRead(1), grovepi.analogRead(0),
grovepi.analogRead(2), grovepi.analogRead(3))
msg = msg + struct.pack(
'<ll', GPG.get_grove_analog(GPG.GROVE_1_1),
GPG.get_grove_analog(GPG.GROVE_2_1))
msg = struct.pack('<L', len(msg)) + msg
connection.send(msg)
except Exception as e:
print "Data server error: ", e
pass
finally:
try:
connection.close()
except:
pass
server_socket.close()