def set_ctrl(throttle, servo):
msg = control_t()
msg.timestamp = time.time()
if (throttle < 0):
throttle = 0.
if (throttle > 0):
throttle = 15+throttle*85
msg.motor = -int(throttle) # do przodu
msg.servo = int(servo*100.)
lc.publish('CTRL', msg.encode())
def adjust_servo(diff):
global lc
global last
if (last + diff > 100):
last = 100
elif (last + diff < -100):
last = -100
else:
last = last + diff
msg = control_t()
msg.timestamp = time.time()
msg.motor = args.prc_motor
msg.servo = last
lc.publish('CTRL', msg.encode())
def adjust_motor(diff_motor):
global lc
global last_motor
if (last_motor + diff_motor > 100):
last_motor = 100
elif (last_motor + diff_motor < -100):
last_motor = -100
else:
last_motor = last_motor + diff_motor
msg = control_t()
msg.timestamp = time.time()
msg.motor = last_motor
msg.servo = args.prc_servo
lc.publish('CTRL', msg.encode())
import lcm
from arc import distance_t
from arc import control_t
import sys
bPrint = 0
if (len(sys.argv) > 1 and sys.argv[1] == '--print'):
bPrint = 1
spi = spidev.SpiDev()
spi.open(0, 0)
spi.max_speed_hz = 50000
lastidx = -1
d_t = distance_t()
c_t = control_t()
lc = lcm.LCM()
while True:
t = long(time.time() * 1000 * 1000)
resp = spi.xfer2([0x01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
# print resp
dist = (int(resp[3]) << 8) + resp[2]
motor_read = (int(resp[5]) << 8) + resp[4]
servo_read = (int(resp[7]) << 8) + resp[6]
motor_applied = (int(resp[9]) << 8) + resp[8]
servo_applied = (int(resp[11]) << 8) + resp[10]
idx = resp[12]
is_autonomous = resp[13]
def stop():
msg = control_t()
msg.timestamp = time.time()
msg.motor = 0
msg.servo = 0
lc.publish('CTRL', msg.encode())
#!/usr/bin/python
import argparse
import time
import lcm
from arc import control_t
import sys
parser = argparse.ArgumentParser()
parser.add_argument('--prc_servo', type=int, default=0)
parser.add_argument('--prc_motor', type=int, default=0)
parser.add_argument('--time', type=int, default=5)
args = parser.parse_args()
#print(args)
#print(args.prc)
#sys.exit()
lc = lcm.LCM()
msg = control_t()
msg.timestamp = time.time()
msg.motor = args.prc_motor
msg.servo = args.prc_servo
lc.publish('CTRL', msg.encode())
time.sleep(args.time)
msg.timestamp = time.time()
msg.motor = 0
msg.servo = args.prc_servo
lc.publish('CTRL', msg.encode())
