def deliverBin():
print("deliverBin")
drive(-100, 0)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
driveTimed(100, 100, 500)
def approachCube(color=MARKER_TOKEN_GOLD, timeout=5):
min_dist = 80
t0 = R.time()
if not checkColor(color):
return False
i = 1
while R.time() < t0 + timeout:
i += 1
leftd = ultrasound.getDistance(0)
rightd = ultrasound.getDistance(1)
print(f"Left {leftd} Right {rightd}")
m = None
if leftd is None and rightd is None:
print("Straight")
drive.driveStraight(20)
elif leftd is None:
print("Rotating CW")
drive.drive(20, 10, -1)
m = rightd
elif rightd is None:
print("Rotating CCW")
drive.drive(10, 20, -1)
m = leftd
else:
print("Straight slowly")
drive.driveStraight(15)
m = leftd
if not m is None and m < min_dist:
print("Stopping")
drive.driveStraight(0)
return True
return False
def lineUpWithRamp():
print("lineUpWithRamp")
driveTimed(100, 20, 2000)
driveTimed (0, 100, 1000)
driveTimed(-100, -80, 4000)
drive(-100, -20)
moveOutrigger(c.outriggerBin, 25)
msleep(1750)
driveTimed(-100, -100, 3000)
def start():
while GPIO.input(conf.tasterStop):
sonicarray = [sonic.read("L"), sonic.read("M"), sonic.read("R")]
if sonicarray[0] or sonicarray[1] or sonicarray[2]:
correction()
else:
drive.drive("L",speed)
drive.drive("R",speed)
def follow_slow(err):
if err < 0:
drive(1, 1)
# it turns very fast, so let's turn in very short pulses
sleep(.02)
coast()
else:
drive(1, -1)
sleep(.02)
coast()
def turnToSouth():
print("turnToSouth")
driveTimed(90, 100, 1000)
deliverPoms()
drive(100, 0)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
driveTimed(100, 0, 500)
moveOutrigger(c.outriggerOut, 100)
def grabBin():
print("grabBin")
moveOutrigger(c.outriggerBin, 20)
driveTimed(0, -100, 500)
drive(-95, -100)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
driveTimed(-50, -50, 150)
binGrabUp()
def goToTaterBin():
print("goToTaterBin")
moveArm(c.armMid, 10)
drive(95, 100)
moveOutrigger(c.outriggerApproach, 20)
while not onBlack(c.STARBOARD_TOPHAT):
pass
drive(25, 30)
while not atArmLength():
pass
stop()
def grabCube():
print("grabCube")
moveArm(c.armUp, 10)
drive(0, -100)
crossBlack(c.LINE_FOLLOWER)
moveClaw(c.clawOpen, 15)
msleep(500)
stop()
moveArm(c.armFront, 15)
driveTimed(100, 100, 1100)
binGrabUp()
moveClaw(c.clawClose, 10)
def loop():
x = joy.leftY()
r = joy.rightX()
drive.drive(x, r)
s = joy.rightTrigger() - joy.leftTrigger()
if s > 0.05:
intake.succ(s)
elif s < -0.05:
intake.spit(s)
else:
intake.stop()
def scoreCube():
print "scoreCube"
drive(50, 50)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
msleep(500)
driveTimed(45, 50, 700)
moveClaw(c.clawOpen, 25)
msleep(300)
moveArm(c.armUp, 10)
driveTimed(100, 100, 1100)
def grabMiddlePile():
print("grabMiddlePile")
moveClaw(c.clawOpen, 25)
moveArm(c.armFront, 25)
drive(100, 100)
msleep(300)
timedLineFollowLeft(c.STARBOARD_TOPHAT, 3)
moveClaw(c.clawMid, 10)
drive(60, 60)
moveClaw(c.clawClose, 5)
moveArm(c.armMid, 25)
stop()
deliverPoms()
def grabSouthPile():
print ("grabSouthPile")
moveClaw(c.clawOpen, 10)
moveArm(c.armFront, 15)
moveArm(c.armShovel, 10)
timedLineFollowLeft(c.STARBOARD_TOPHAT, 5)
drive(50, 50) #now same on both
moveArm(c.armFront, 50)
moveClaw(c.clawClose, 5)
msleep(500)
stop()
moveArm(c.armMid, 15)
deliverPoms()
moveOutrigger(c.outriggerFindLine, 25)
def correction():
sonicarray = [sonic.read("L"), sonic.read("M"), sonic.read("R")]
obstacle = cam.objectDetection()
areaL = 0
areaR = 0
factor = 1
for element in obstacle:
ocor = obstacle[element]
if ocor[0] >= 140:
areaR += (ocor[2]*ocor[3])
else:
areaL += (ocor[2]+ocor[3])
if areaL > areaR and sonicarray != [0, 0, 0]:
#nach rechts fahren abhängig von flächengröße
factor = areaR/areaL
drive.drive("L",speed + corrF*factor)
drive.drive("R",speed - corrF*factor)
if areaR > areaL and sonicarray != [0, 0, 0]:
#nach links fahren abhängig von flächengröße
factor = areaL/areaR
drive.drive("R",50 + corrF*factor)
drive.drive("L",50 - corrF*factor)
def grabNorthPile():
print("grabNorthPile")
drive(93, 100)#90
moveClaw(c.clawMid, 10)
msleep(1000)
drive(54, 50)#45,50
moveClaw(c.clawClose, 4)
msleep(500)
stop()
#moveArm(c.armMid, 15)
#msleep(500)
moveClaw(c.clawOpen, 10)
msleep(500)
moveClaw(c.clawClose, 10)
'''driveTimed(-50, -50, 1000)
def goToWestPile():
print("goToWestPile")
moveClaw(c.clawOpen, 20)
msleep(300)
driveTimed(95, 100, 500)
moveClaw(c.clawClose, 15)
msleep(300)
drive(95, 100)
msleep(1000)
moveArm(c.armBump, 20)
msleep(500)
print("armBump")
driveTimed(95,100, 1250)
moveArm(c.armFront,10)
moveClaw(c.clawWiggle, 20) #clawOpen
driveTimed(95, 100, 3000)
def teleopPeriodic(self):
print(self.switch1.get())
if self.writeAutoChooser.getSelected() == 1:
auto.Autonomous.writeCode(self)
else:
drive.drive(self,
self.Joystick.getRawAxis(4), self.Joystick.getY(),
self.Sec_Joystick.getRawButton(1),
self.Sec_Joystick.getRawButton(2),
self.Sec_Joystick.getRawButton(3),
self.Sec_Joystick.getRawButton(4),
self.Sec_Joystick.getRawButton(5),
self.Sec_Joystick.getRawButton(6),
self.Sec_Joystick.getRawButton(7),
self.Sec_Joystick.getRawButton(8))
def goToNorthernPile():
print("goToNorthernPile")
moveClaw(c.clawOpen, 30)
moveArm(c.armMid, 20)
drive(100, 90)
while not onBlack(c.STARBOARD_TOPHAT):
pass
driveTimed(100, 100, 150)
moveOutrigger(c.outriggerTurn, 20)
drive(100, -20) #was drive(100, -20)
while not onBlack(c.STARBOARD_TOPHAT):
pass
moveArm(c.armFront, 10)
if c.isClone:
driveTimed(0, 100, 200) #300
stop()
driveTimed(0, 100, 300)
def readCode(self, robotMain, recTime):
timeVar = time.time()
fileName = "/home/lvuser/auto.txt"
#debugFileName = "/home/lvuser/debug.txt"
#debugFile = open(debugFileName, "a")
startLine = 0
endLine = 0
completeTime=12.730631828308105
count = 0
recTimeString = "[" + str(recTime)
with open(fileName) as file:
for line in file:
count += 1
if recTimeString in line:
startLine = count
if startLine != 0 and endLine == 0 and "]" in line:
endLine = count
break
count = 1
while wpilib.DriverStation.getInstance().isEnabled() and timeVar + 15 > time.time() and count + startLine < endLine - 1:
count += (completeTime / 15)
line = linecache.getline(fileName, int(round(count + startLine)))
lineList = line.split(',')
drive.drive(robotMain, float(lineList[0]), float(lineList[1]), bool(int(lineList[2])), bool(int(lineList[3])), bool(int(lineList[4])), bool(int(lineList[5])), bool(int(lineList[6])), bool(int(lineList[7])), bool(int(lineList[8])), bool(int(lineList[9])))
#debugFile.write(str(time.time() - timeVar) + "\n")
#debugFile.close()
timeEndVar = time.time() - timeVar
while wpilib.DriverStation.getInstance().isEnabled() and timeVar + 15 > time.time():
drive.drive(robotMain, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
print(timeEndVar)
linecache.clearcache()
def main(argv=None):
FLAGS.output_path = os.path.join(FLAGS.root_dir, 'output', FLAGS.version)
if FLAGS.mode == 'train':
from train import train_model
train_model(FLAGS)
elif FLAGS.mode == 'drive':
from drive import drive
drive(FLAGS)
elif FLAGS.mode == 'eval':
from evaluate import eval_model
eval_model(FLAGS)
elif FLAGS.mode == 'save':
from save_model import save_model_with_weights
save_model_with_weights(FLAGS)
def writeCode(robotMain):
timeVar = time.time()
fileName = "/home/lvuser/auto.txt"
file = open(fileName, "a")
line = "[" + str(timeVar) + "\n"
file.write(line)
while wpilib.DriverStation.getInstance().isEnabled() and timeVar + 15 > time.time():
line = str(robotMain.Joystick.getRawAxis(4)) + "," + str(robotMain.Joystick.getY()) + "," + str(int(robotMain.Sec_Joystick.getRawButton(1))) + "," + str(int(robotMain.Sec_Joystick.getRawButton(2))) + "," + str(int(robotMain.Sec_Joystick.getRawButton(3))) + "," + str(int(robotMain.Sec_Joystick.getRawButton(4))) + "," + str(int(robotMain.Sec_Joystick.getRawButton(5))) + "," + str(int(robotMain.Sec_Joystick.getRawButton(6))) + "," + str(int(robotMain.Sec_Joystick.getRawButton(7))) + "," + str(int(robotMain.Sec_Joystick.getRawButton(8))) + "\n"
file.write(line)
drive.drive(robotMain, robotMain.Joystick.getX(), robotMain.Joystick.getY(), robotMain.Sec_Joystick.getRawButton(1), robotMain.Sec_Joystick.getRawButton(2), robotMain.Sec_Joystick.getRawButton(3), robotMain.Sec_Joystick.getRawButton(4), robotMain.Sec_Joystick.getRawButton(5), robotMain.Sec_Joystick.getRawButton(6), robotMain.Sec_Joystick.getRawButton(7), robotMain.Sec_Joystick.getRawButton(8))
while wpilib.DriverStation.getInstance().isEnabled() and timeVar + 15 < time.time():
drive.drive(robotMain, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
robotMain.My_Robot.tankDrive(0, 0)
file.write("]\n")
file.close()
def approachCubeCam(code, timeout=10):
min_dist = 140
s_per_deg = 0.2
mk = None
print("approachCubeCam")
t0 = R.time()
while R.time() < t0 + timeout:
markers = R.see()
for m in markers:
if m.info.code == code:
mk = m
break
if mk is None:
return False
print("Dist:", mk.dist * 1000)
left = ultrasound.getDistance(0)
right = ultrasound.getDistance(1)
print("US", left, right)
print("Switch", R.ruggeduinos[0].digital_read(2))
if not left is None and not right is None:
if min(left, right) < 60 or mk.dist < 0.145 or R.ruggeduinos[
0].digital_read(2):
#R.sleep(0.5)
drive.driveStraight(0)
return True
if mk.rot_y > -0.5 and mk.rot_y < 0.5:
print("Straight")
drive.driveStraight(20)
elif mk.rot_y > 0:
print("Right")
drive.drive(15, 10, -1)
elif mk.rot_y < 0:
print("Left")
drive.drive(10, 15, -1)
print("Timed out!")
return False
def returnToBase():
print ("returntobase")
moveArm(c.armBlockBack, 10)
driveTimed(-100, 0, 1000)
drive(-100, -87) #-85
while not onBlack(c.STARBOARD_TOPHAT):
pass
driveTimed(-100, 0, 300)
timedLineFollowBack(c.STARBOARD_TOPHAT, 3)#2
driveTimed(-80, -100, 1000)
drive(-90, -100)
msleep(3500);
moveOutrigger(c.outriggerBaseReturn, 20)
msleep(1000)
while(onBlack(c.STARBOARD_TOPHAT)):
pass
msleep(10)
while(onBlack(c.STARBOARD_TOPHAT)):
pass
msleep(10)
while(onBlack(c.STARBOARD_TOPHAT)):
pass
stop()
def main():
wrist = arm(0.005, "wrist", 8.0, 1.0, 134.0, 0.7, 0.71,
18730.0 * 2.0 * math.pi / 60.0,
1.0 / 3.0 * 10.0 * 0.454 * ((17.0 * 0.0254)**2), 833.33)
"""q_1 = 1.0/((math.pi / 90.0)**2)
q_2 = 1.0/((math.pi / 90.0)**2)
Q_c = numpy.matrix([[q_1, 0],
[0, q_2]])
r_1 = 1.0 / (0.1**2)
R_c = numpy.matrix([[r_1]])
A_d, B_d, Q_d, R_d = controls.c2d(numpy.asmatrix(wrist.A_c), numpy.asmatrix(wrist.B_c),0.005, Q_c, R_c)
K = controls.place(A_d, B_d, (0.9, 0.003))
L = controls.dkalman(A_d, wrist.C, Q_d, R_d)
wrist.set_K(K)
wrist.set_L(L)"""
#wrist.run_test(numpy.matrix([[0.0],[0]]), numpy.matrix([[90.0],[0.0]]), True, False, True, 400)
#wrist.run_pid_test([5.23, 0.0,0.0],numpy.matrix([[0.0],[0]]), numpy.matrix([[90.0],[0]]), True, True, 400)
#wrist.run_custom_test(numpy.matrix([[0.0],[0]]), numpy.matrix([[90.0],[0]]), max_func, True, False, True, 400)
wrist.get_stats()
elev = elevator(0.005, "elev", 4.0, 4.0, 134.0, 0.7, 0.71,
18730.0 * 2.0 * math.pi / 60.0, 40.62, 1.751 * 0.0254,
10.0 * 0.454 + 2.5) #11.59)
#elev.run_pid_test([1.2, 0.0,0.0],numpy.matrix([[0.0],[0]]), numpy.matrix([[60.0],[0]]), True, True, 400)
#elev.run_custom_test(numpy.matrix([[0.0],[0]]), numpy.matrix([[60.0],[0]]), max_func, True, False, True, 400)
elev.get_stats()
drive_gear_ratio = (60.0 / 12.0) * (34.0 / 24.0) * (34.0 / 32.0)
distribution_radius = 0.45
mass = 154.0 * 0.454
j = distribution_radius * mass
print(drive_gear_ratio)
drivetrain = drive(0.005, "drivetrain", 4.0, 3.0, 89.0, 3.0, 1.41,
5840 * 2.0 * math.pi / 60.0, drive_gear_ratio, j,
3.0 * 0.0254, 28.0 * 0.0254 * 0.5, 154.0 * 0.454)
#drivetrain.run_pid_test([1.2, 0.0,.1],numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[12.0],[8.0], [0.0],[0.0]]), True, True, 4000)
#drivetrain.run_custom_test(numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[12.0],[12.0], [0.0],[0.0]]), full_volts, True, False, True, 400)
drivetrain.get_stats()
intake = shooter(0.005, "intake", 4.0, 2.0, 134.0, 0.7, 0.71,
18730.0 * 2.0 * math.pi / 60.0, 0.5 * 0.0254**2 * 10.0,
6.67)
intake.run_pid_test([0.05, 0.07, 0.0], numpy.matrix([[0.0]]),
numpy.matrix([[2000.0]]), True, True, 400)
#intake.run_custom_test(numpy.matrix([[0.0]]), numpy.matrix([[1000]]), max_func, True, False, True, 400)
intake.get_stats()
def main():
drive_authorization = auth(SCOPES, args.credentials)
drive_credentials = None
try:
drive_credentials = drive_authorization.get_credentials(
args.no_localhost)
drive_service = drive(drive_credentials)
drive_service.download_file(args.id, args.name, args.chunk_size,
args.output_directory)
except FileNotFoundError as fnfe:
print('\'' + args.credentials +
'\' was not found or is not a valid client secret file',
file=sys.stderr)
except RefreshError as rfe:
print(
'An error occured whilst refreshing your credentials. If this issue persists, delete the \'token.pickle\' file and try again.',
file=sys.stderr)
def start():
while GPIO.input(conf.tasterStop):
line = cam.lineDetection()
try:
difference = int(line[2])
except TypeError:
continue
control = pid(difference)
if difference >= 0:
drive.drive("R", 50 - control / 2)
drive.drive("L", 50 + control / 2)
else:
drive.drive("L", 50 - control / 2)
drive.drive("R", 50 + control / 2)
if conf.debug:
print("T3 Control:{}, {}".format(control, delta))
def goToCenterAgain():
print("goCenterAgain")
driveTimed(95, 100, 3000)
DEBUG()
driveTimed(100, 60, 3000)
drive(100, 100)
while not onBlack(c.LINE_FOLLOWER):
pass
drive(100, 0)
while onBlack(c.LINE_FOLLOWER):
pass
stop()
timedLineFollowRight(c.LINE_FOLLOWER, 2)
drive(100, 100)
moveClaw(c.clawOpen, 25)
lineFollowUntilEndRight(c.LINE_FOLLOWER)
driveTimed(100, 0, 150)
msleep(400)
driveTimed(100, 100, 1500)
def goToCenter():
print("goToCenter")
if c.isPrime:
driveTimed(95, 100, 4000)
driveTimed(100, 60, 3000) #3000
else:
driveTimed(90, 100, 4000)
driveTimed(100, 60, 3000)
drive(100, 100)
while not onBlack(c.LINE_FOLLOWER):
pass
drive(100, 0)
while onBlack(c.LINE_FOLLOWER):
pass
stop()
timedLineFollowRight(c.LINE_FOLLOWER, 1)
drive(100, 100)
moveClaw(c.clawOpen, 25)
lineFollowUntilEndRight(c.LINE_FOLLOWER)
driveTimed(90, 50, 200)
moveArm(c.armShovel, 15)
msleep(400)
driveTimed(100, 100, 1500)
def none(self): while wpilib.DriverStation.getInstance().isEnabled() and time.time() - self.startTime < 15: drive.drive(robotMain, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
PWML = PWML - 0.5
if PWML < 0:
PWML = PWML + 0.5
deltavR = speed - r.rpm[1]
if deltavR < 0:
PWMR = PWMR - 0.5
if PWMR > 100:
PWMR = PWMR - 0.5
elif deltavR >= 0:
PWMR = PWMR + 0.5
if PWMR < 0:
PWMR = PWMR + 0.5
d.drive("F", "F", PWML, PWMR)
print(r.rpm, end='\r')
print("deltavL:", deltavL, end='\r')
print("PWML:", PWML, end='\r')
print("deltavR:", deltavR, end='\r')
print("PWMR:", PWMR, end='\r')
#if s.readDistance(cf.EchoM, cf.TriggerL) < 30 and s.readDistance(cf.EchoM, cf.TriggerL) > 20:
#speed = speed - 30
#if s.readDistance(cf.EchoM, cf.TriggerL) < 20 and s.readDistance(cf.EchoM, cf.TriggerL) > 10:
#speed = speed - 50
## print("s to start, e to exit, a to abort, r to return (only after start)")
## x = raw_input()
## if x == 's':
## d.drive("B", "F", 50, 50)
try:
if args.mode == 'drive':
from drive import drive
driver_conf_module = __import__(args.driver_config)
driver_conf = driver_conf_module.configDrive()
driver_conf = parse_drive_arguments(
args,
driver_conf,
attributes=[
'carla_config', 'host', 'path', 'noise', 'type_of_driver',
'interface', 'number_screens', 'scale_factor'
])
driver_conf.positions_file = args.positions_file
drive(args.experiment_name, driver_conf, args.name,
float(args.memory))
elif args.mode == 'train':
from train import train
train(args.experiment_name, args.memory)
elif args.mode == 'test_input':
from test_input import test_input
test_input(args.gpu)
elif args.mode == 'test_train':
from test_train import test_train
test_train(args.gpu)
else:
raise ValueError()
except:
raise
# **MAINE LOOP**
print "Sasha rises!"
print "Press Start to enable."
try:
while True:
if isStarted:
### Drive Command ###
if speedToggle != 0:
if speedToggle == 2:
if abs(j.leftX()) <= .10 and abs(j.leftY()) <= .21:
drive.drive(0, 0)
else:
drive.drive(j.leftX(), j.leftY())
else:
if abs(j.leftX()) <= .15 and abs(j.leftY()) <= .23:
drive.drive(0, 0)
else:
drive.drive(j.leftX(), j.leftY() * .4)
else:
drive.drive(0, 0)
### Speed toggle & Drive Control ###
if j.Back():
if pressedBack == False:
if j.X():
#print "FULL SPEED AHEAD!"
def main():
rod_weight = 4.0 * 0.293 * 0.453592
radius = 3.0 / 16.0 * 0.0254
pitch = 16.0/ (1 * 0.0254)
load = calculate_load_torque(40. * 4.4482, 2.0 * radius, pitch, 0.19)
test = linear_actuator(0.005, "test", 1.0, 1.0, 105.0, 1.8, 2.6, 5676 * 2.0 * math.pi / 60.0,
0.5 * rod_weight * radius * radius, 1.0, pitch, load)
#test.run_custom_test(numpy.matrix([[0.0],[0]]), numpy.matrix([[24.0],[0]]), max_func, True, False, True, 800)
#test.get_stats()
best_G = calculate_ideal_gear_ratio(load, 1.3)
print(best_G)
#Neo @ 1:3 overdrive running at 10 volts works best
drive_gear_ratio = 7.083 * 24.0 / 60.0 * 64.0 / 20.0;#1.13333#5.10#50.0 /34.0 2.16 ratio spread 54 : 30
j = 6.0
#2778 2000 2907 3269 3211 3099 3287 3573 2705
#drivetrain = drive(0.005, "drivetrain", 4.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0,
# drive_gear_ratio, j, 3.0 * 0.0254, 28.0 * 0.0254 * 0.5, 154.0 * 0.454)
#radius base might be 25.5 * 0.0254 * 0.5, but not likely, more like 0.45
drivetrain = drive(0.005, "drivetrain", 3.0, 2.0, 131.0, 2.7, 2.41, 5330 * 2.0 * math.pi / 60.0,
drive_gear_ratio, j, 3.0 * 0.0254, 25.5 * 0.0254 * 0.5 , (125.0) * 0.454)
#drivetrain.run_pid_test([1.0, 0.0,0],numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[55.0],[55.0], [0.0],[0.0]]), True, True, 4000)
#drivetrain.run_custom_test(numpy.matrix([[0.0],[0],[0],[0]]), numpy.matrix([[12.0],[12.0], [0.0],[0.0]]), full_volts, True, False, True, 2000)
drivetrain.get_stats(True)
driveQ_c = numpy.matrix([[1.0 / (0.14)**2.0 , 0, 0, 0],
[0, 1.0 / (0.14)**2.0, 0, 0],
[0, 0, 1.0 / (1.0)**2.0, 0],
[0, 0, 0, 1.0 / (1.0)**2.0]])
driveR_c = numpy.matrix([[0.0001**2, 0.0],
[0.0, 0.0001 **2]])
# [0 , 1.0 / (.1 * 0.0254)**2]])
driveA_d, driveB_d, driveQ_d, driveR_d = controls.c2d(drivetrain.A_c, drivetrain.B_c, 0.005, driveQ_c, driveR_c)
driveK = controls.dlqr(driveA_d, driveB_d, driveQ_d, driveR_d)
print(driveK)
elev = elevator(0.005, "elev", 2.0, 2.0, 134.0, 0.7, 0.71, 18730.0 * 2.0 * math.pi / 60.0,
63.0, 3.0 * (0.8755 - 0.16) * 0.0254, 18.8* 0.454)#11.59)
#elev.export("../python")
elevQ_c = numpy.matrix([[1.0 / (0.0254 * 0.01)**2.0 , 0],
[0 , 1.0 / (.1 * 0.0254)**2]])
elevR_c = numpy.matrix([[1.0 / 13.0**2]])
elevA_d, elevB_d, elevQ_d, elevR_d = controls.c2d(elev.A_c, elev.B_c, 0.005, elevQ_c, elevR_c)
elevK = controls.dlqr(elevA_d, elevB_d, elevQ_d, elevR_d)
elevL = controls.dkalman(elevA_d, elevB_d, elevQ_d, elevR_d)
#print(elevK)
elev.run_test([[0.],[0.]],[[0.0],[60.0]])
def test():
drive.drive(0.5, 0)
except:
raise
# **MAINE LOOP**
print "Sasha rises!"
print "Press Start to enable."
try:
while True:
if isStarted:
### Drive Command ###
if speedToggle != 0:
if speedToggle == 2:
if abs(j.leftX()) <= .10 and abs(j.leftY()) <= .21:
drive.drive(0, 0)
else:
drive.drive(j.leftX(), j.leftY())
else:
if abs(j.leftX()) <= .15 and abs(j.leftY()) <= .23:
drive.drive(0, 0)
else:
drive.drive(j.leftX(), j.leftY() * .4)
else:
drive.drive(0, 0)
### Speed toggle & Drive Control ###
if j.Back():
if pressedBack == False:
if j.X():
#print "FULL SPEED AHEAD!"
def start():
while GPIO.input(conf.tasterStop):
drive.drive("L")
drive.drive("R")
return True
def run_tests():
# Init
# Ros node initalization
rospy.init_node('position', anonymous=True)
# Create driver for receiving mavros msg
driver = mavros_driver.mavros_driver()
lock = threading.Lock()
go = drive.drive(lock)
# Pose publisher rate
rate = 20
# Signal flag for running threads
run_event = threading.Event()
run_event.set()
move_t = threading.Thread(target=move_pub, args=(rate, go, run_event))
move_t.start()
# SET MODE
driver.set_mode("OFFBOARD")
# ARM
driver.arm(True)
# Wait for drone to reach hover position
time.sleep(2.0)
print "Start Altitude = 2 meter"
# UI loop
usage()
while 1:
# READ USER INPUT
user_input = sys.stdin.readline()
# SPLIT INPUT
args = user_input.split()
# SET POSITION
if str(args[0]) == "p":
# Reser relative pose
reset(pose_rel)
# Set new relative position
if len(args[1:]) > 3:
print "too many arguments"
usage()
elif len(args[1:]) < 3:
print "not enough arguments"
usage()
else:
# Starting position
start_pose = driver.get_pose()
# Set starting pose
pose_rel[0] = start_pose.pose.position.x
pose_rel[1] = start_pose.pose.position.y
pose_rel[2] = start_pose.pose.position.z
print "Start position set"
time.sleep(0.5)
# Set destination pose
des_pose.pose.position.x = float(args[1])
des_pose.pose.position.y = float(args[2])
des_pose.pose.position.z = float(args[3])
print "Waypoint set"
time.sleep(0.5)
# Set distance
distance = go.dist2wp(des_pose)
print"Distance set"
print distance
time.sleep(0.5)
# Calculate distance step
dx = ((float(args[1]) - start_pose.pose.position.x) / (distance * 1000))
dy = ((float(args[2]) - start_pose.pose.position.y) / (distance * 1000))
dz = ((float(args[3]) - start_pose.pose.position.z) / (distance * 1000))
print "Step size set"
time.sleep(0.5)
pose_rel2[0] = float(args[1])
pose_rel2[1] = float(args[2])
pose_rel2[2] = float(args[3])
go.set_orient(pose_rel2)
while distance > 0.5:
alt_correction = pose_rel[2]
if (des_pose.pose.position.z-pose_rel[2]) < 0.2:
alt_correction = pose_rel[2] + go.obstacle_height()
pose_rel[0] = pose_rel[0] + dx
pose_rel[1] = pose_rel[1] + dy
pose_rel[2] = alt_correction + dz
go.set_msg(pose_rel)
time.sleep(0.001)
distance = go.dist2wp(des_pose)
print "Destination reached"
usage()
# Dont waste cpu!
time.sleep(1)
def leftTurn(time=5):
drive.drive("L", 50, 2, 0)
drive.drive("R", 50, 2, 1)
time.sleep(time)
drive.stop()
# /usr/bin/env python
# Download the twilio-python library from twilio.com/docs/libraries/python
from flask import Flask, request
from twilio.twiml.messaging_response import MessagingResponse, Message
from drive import drive
app = Flask(__name__)
d = drive()
@app.route("/sms", methods=['GET', 'POST'])
def sms_ahoy_reply():
"""Respond to incoming messages with a friendly SMS."""
# Start our response
resp = MessagingResponse()
cmd = request.form['Body']
print(cmd)
cmd = cmd.split()
if cmd[0] == 'S':
d.forward(0)
resp.message("Command Received: Stopped")
elif cmd[0] == 'F':
try:
speed = float(cmd[1])
d.forward(speed)
resp.message("Command Received: Forward")
except:
d.forward(0)
resolution = []
resolution.append(int(res_string[0]))
resolution.append(int(res_string[1]))
try:
if args.mode == 'drive':
if args.game == 'Elektra': #can later be clubbed with general case(handle drive_config object)
driver_conf_module = __import__(args.driver_config)
driver_conf = driver_conf_module.configDrive()
driver_conf = parse_drive_arguments(args, driver_conf)
drive_elektra(args.experiment_name, driver_conf,
args.input_method, args.name)
else:
drive(args.host, int(args.port), args.gpu, args.path,
args.show_screen, resolution, args.noise,
args.carla_config, args.driver, args.experiment_name,
args.city, args.game, args.name)
elif args.mode == 'train':
#from config import *
#config_main = configMain()
train(args.gpu, args.experiment_name)
elif args.mode == 'evaluate':
evaluate(args.gpu, args.experiment_name)
elif args.mode == 'test_input':
test_input(args.gpu)
elif args.mode == 'test_train':
test_train(args.gpu)
else: # mode == evaluate
evaluate.evaluate(args.gpu)
def start():
while GPIO.input(conf.tasterStop):
lane = linehold.line()
if lane[0] == 1 and lane[1] == 1:
#geradeaus
drive.drive("L", 75, 2)
drive.drive("R", 75, 2)
elif lane[0] == 0 and lane[1] == 1:
#nach rechts korrigieren
drive.drive("L", speedA, 2)
drive.drive("R", speedI, 2)
elif lane[0] == 1 and lane[1] == 0:
#nach links korrigieren
drive.drive("L", speedI, 2)
drive.drive("R", speedA, 2)
def process_collect(list_of_configs, port, gpu,
tag, generated_config_cache_path, template_path, driver_config, TownName):
print("port is ", port, "!!!!!!!!!!!!!!!!!!")
port = int(port)
count = 5 # to flag that initially we need to start the server
#os.environ["CUDA_VISIBLE_DEVICES"] = str(gpu)
threads = []
for config, weather in list_of_configs:
# generate this config
this_name = config_naming(tag, config, weather)
config_fname = os.path.join(generated_config_cache_path, this_name + ".ini")
config_change_attrs(template_path, config_fname,
[("CARLA/Sensor", config[0], config[1]),
("CARLA/LevelSettings", "WeatherId", str(weather))])
driver_conf_module = __import__(driver_config)
driver_conf = driver_conf_module.configDrive()
driver_conf.carla_config = config_fname
driver_conf.weather = str(weather)
driver_conf.port = port
driver_conf.path = "/scratch/yang/aws_data/carla_collect/"+args.townname + "_" + args.mode + "/" # If path is set go for it , if not expect a name set
driver_conf.city_name = args.townname
if args.mode == "normal":
driver_conf.reset_period = 960
driver_conf.num_images_to_collect = 600 * 20 * 3
driver_conf.noise_intensity = 7.5
driver_conf.parking_position_file = "town03_intersections/positions_file_Exp_Town.parking.txt"
driver_conf.extra_explore_prob = 0.0
# driver_conf.extra_explore_position_file = "town03_intersections/positions_file_Exp_Town.parking_attract.txt"
elif args.mode.startswith("park"):
driver_conf.reset_period = 960 // 80
driver_conf.num_images_to_collect = 600 * 20 * 3 // 5
driver_conf.noise_intensity = 5.0
if args.mode == "park_withcar":
driver_conf.parking_position_file = "town03_intersections/positions_file_Exp_Town.parking.txt"
elif args.mode == "park_nocar":
driver_conf.parking_position_file = None
else:
raise ValueError()
driver_conf.extra_explore_prob = 1.0
driver_conf.extra_explore_position_file = "town03_intersections/positions_file_Exp_Town.parking_attract.txt"
elif args.mode == "shoulder":
driver_conf.reset_period = 960 // 200
driver_conf.num_images_to_collect = 600 * 20 * 3 // 5
driver_conf.noise_intensity = 5.0
driver_conf.parking_position_file = None
driver_conf.extra_explore_prob = 1.0
driver_conf.extra_explore_position_file = "town03_intersections/position_file_Exp_Town.shoulder.v4.merge.txt"
else:
raise ValueError()
if not os.path.exists(driver_conf.path):
os.makedirs(driver_conf.path)
# experiment_name & memory not used for human
while True:
count += 1
if count >= 2:
count = 0
if use_docker:
cmd = ["docker run -p %d-%d:%d-%d --runtime=nvidia -e NVIDIA_VISIBLE_DEVICES=%d %s /bin/bash CarlaUE4.sh %s -carla-server -benchmark -fps=5 -carla-world-port=%d" % (port, port+2, port, port+2, gpu, docker_path, town_within_path, port)]
else:
cmd = ['bash', '-c', " '%s %s -carla-server -benchmark -fps=5 -carla-world-port=%d' " % (CARLA_PATH, town_within_path, port)]
print(" ".join(cmd))
print("before spawnling")
t = threading.Thread(target=lambda: os.system(" ".join(cmd)))
t.start()
threads.append(t)
time.sleep(60)
if drive("", driver_conf, this_name, 0):
count = 0
break
time.sleep(1)
print("finished one setting, sleep for 3 seconds")
time.sleep(3)
os.system('pkill -f -9 "CarlaU.*port=%d"' % port)
global reverseRight
# Bewegen der Kamera entsprechend des über MQTT empfangenen Winkels
cameraMove(app_angle)
# Senden der Daten für Winkel und Distanz an die App mittels MQTT
client.publish(topicRadar, radarQ.get())
# Falls das selbständige Fahren aktiviert ist, so werden die Steuerbefehle der Motoren aus dem Skript für das automatische Fahren geholt
if autoDrive == True:
forwardLeft = autodrive.getForwardLeft()
forwardRight = autodrive.getForwardRight()
reverseLeft = autodrive.getReverseLeft()
reverseRight = autodrive.getReverseRight()
objectReached = autodrive.getObjectReached()
# Überprüfen, ob das Objekt mittels Autodrive-Funktion erreicht wurde
if objectReached == True:
client.publish(topic, "arrived at home")
autodrive.setAutoDrive(False)
autoDrive = False
autodrive.reset()
# Übermittlung der Steuerbefehle an die Motoren
drive(forwardLeft, forwardRight, reverseLeft, reverseRight)
def getAutoDriveQ():
global autoDriveQ
return autoDriveQ