def shapeGen(): x, y = yield (x,y) = yield vibes.drawArrow([x-1, y-1], [x+1, y+1], 0.6, 'k[k]') (x,y) = yield vibes.drawAUV(x,y,45,2, 'k[y]') (x,y) = yield vibes.drawBox(x-1,x+1,y-2,y+2, "[b]") (x,y) = yield vibes.drawBoxesUnion([[x-1,x,y-2,y], [x-0.5,x+1,y-0.5,y+2]], "[r]") (x,y) = yield vibes.drawCircle(x,y,1, "g[m]") (x,y) = yield vibes.drawEllipse(x,y,2,1,45, color="r[darkCyan]") (x,y) = yield vibes.drawLine([[x-0.5, y-1], [x+1, y+0.5]], "k") (x,y) = yield vibes.drawPie( (x-2, y-2), (1,2.5), [20,70] , "y[cyan]") (x,y) = yield vibes.drawPolygon([[x-1,y-1], [x, y+1], [ x+1, y-1]], "k[orange]") (x,y) = yield vibes.drawRing(x,y, 1,2, '[red]') # (x,y) = yield vibes.drawPie( (x, y), (1,2), [0,360] , "y[cyan]") (x,y) = yield vibes.drawVehicle( x, y, 20, 1, "[darkBlue]") (x,y) = yield vibes.drawPoint( x,y, 1, "[k]") (x,y) = yield vibes.drawText( x,y, "Text", 1)
def shapeGen():
x, y = yield
(x, y) = yield vibes.drawArrow([x - 1, y - 1], [x + 1, y + 1], 0.6, 'k[k]')
(x, y) = yield vibes.drawAUV(x, y, 45, 2, 'k[y]')
(x, y) = yield vibes.drawBox(x - 1, x + 1, y - 2, y + 2, "[b]")
(x, y) = yield vibes.drawBoxesUnion(
[[x - 1, x, y - 2, y], [x - 0.5, x + 1, y - 0.5, y + 2]], "[r]")
(x, y) = yield vibes.drawCircle(x, y, 1, "g[m]")
(x, y) = yield vibes.drawEllipse(x, y, 2, 1, 45, color="r[darkCyan]")
(x, y) = yield vibes.drawLine([[x - 0.5, y - 1], [x + 1, y + 0.5]], "k")
(x, y) = yield vibes.drawPie((x - 2, y - 2), (1, 2.5), [20, 70], "y[cyan]")
(x,
y) = yield vibes.drawPolygon([[x - 1, y - 1], [x, y + 1], [x + 1, y - 1]],
"k[orange]")
(x, y) = yield vibes.drawRing(x, y, 1, 2, '[red]')
# (x,y) = yield vibes.drawPie( (x, y), (1,2), [0,360] , "y[cyan]")
(x, y) = yield vibes.drawVehicle(x, y, 20, 1, "[darkBlue]")
(x, y) = yield vibes.drawPoint(x, y, 1, "[k]")
(x, y) = yield vibes.drawText(x, y, "Text", 1)
print(vibes.channel, vibes.current_fig)
vibes.beginDrawing()
vibes.newFigure("test")
vibes.drawBox(0,1,4,6,color='[#FF12FFA0]', figure='test')
vibes.drawText(12, 12, 'My Text', 0.1, 'b[r]')
vibes.axisAuto()
vibes.drawBox(0.5,2,4,7,color='[#00FFFF66]', figure='test')
vibes.drawBox(-1,0,-4,-6,color='r', figure='test')
vibes.drawLine([[0,0], [4,4]])
vibes.drawEllipse(3, 5, 1, 3, 45)
vibes.drawEllipse(5, 5, 3, 3, 0, angles=[30,60], color='g')
vibes.drawCircle(0, 0, 5)
vibes.drawAUV(0,0, 2, 3, color='r[yellow]')
vibes.drawPie([0,0], [3,4], [45, 90])
vibes.newGroup("Pie", figure="test", format='[cyan]')
vibes.drawPie([0,0], [5,9], [-120, -40], group="Pie")
# vibes.drawPie([0,0], [5,9], [-120, -40], "[b]")
vibes.drawPie([5,2], [1,2], [160, 220]) #, 'g[y]')
# vibes.clearGroup("Pie", figure="test")
vibes.drawPie([0,0], [1,2], [160, 220]) #, 'g[y]')
for i in range(0,1):
listRobots.append(Robot(-4.0,45,0,0,h,i,'b'))
vibes.beginDrawing()
vibes.clearFigure()
vibes.drawLine([[-4.4,47.8],[-3.8,47.8],[-2.3,47.2],[-2,46.6],[-1.3,44.4],[-2.0,43.3],[-3.6,43.5],[-2.8,43.4]])
for t in frange(0,100,h):
time.sleep(0.1)
#vibes.clearFigure()
time1 = time.time()
for rob in listRobots:
#Draw circle of position
# print(rob.X[0])
# print(rob.X[1])
vibes.drawAUV(rob.X[0], rob.X[1], 0.2, rob.theta*180/math.pi, rob.couleur)
#Determine next point
delta = 2*rob.numero*math.pi/len(listRobots)
nextX, nextV = traj(t,delta)
vibes.drawCircle(nextX[0],nextX[1], 0.005, 'r')
# print(nextX)
# print(nextV)
rob.setObjectifs(nextX, nextV)
#Calculate the new command
rob.controlCommande()
#Move the robot
rob.subMove()
print(vibes.channel, vibes.current_fig)
vibes.beginDrawing()
vibes.newFigure("test")
vibes.drawBox(0, 1, 4, 6, color='[#FF12FFA0]', figure='test')
vibes.drawText(12, 12, 'My Text', 0.1, 'b[r]')
vibes.axisAuto()
vibes.drawBox(0.5, 2, 4, 7, color='[#00FFFF66]', figure='test')
vibes.drawBox(-1, 0, -4, -6, color='r', figure='test')
vibes.drawLine([[0, 0], [4, 4]])
vibes.drawEllipse(3, 5, 1, 3, 45)
vibes.drawEllipse(5, 5, 3, 3, 0, angles=[30, 60], color='g')
vibes.drawCircle(0, 0, 5)
vibes.drawAUV(0, 0, 2, 3, color='r[yellow]')
vibes.drawPie([0, 0], [3, 4], [45, 90])
vibes.newGroup("Pie", figure="test", format='[cyan]')
vibes.drawPie([0, 0], [5, 9], [-120, -40], group="Pie")
# vibes.drawPie([0,0], [5,9], [-120, -40], "[b]")
vibes.drawPie([5, 2], [1, 2], [160, 220]) #, 'g[y]')
# vibes.clearGroup("Pie", figure="test")
vibes.drawPie([0, 0], [1, 2], [160, 220]) #, 'g[y]')
# exit()
vibes.newFigure("test2")
sep = SepQInterProjF(seps)
sep.q = 0
# init drawing area
vibes.beginDrawing()
vibes.newFigure('Result')
vibes.setFigureProperties({'x': 0, 'y': 0, 'width': 1000, 'height': 1000})
#configure pySIVIA output
params = {'color_in': '#888888[#444444]', 'color_out':
'#888888[#DDDDDD]', 'color_maybe': '#888888[w]', 'use_patch' : True}
# create the initial box X0 = [-10, 10] x [-10, 10]
X0 = IntervalVector([[-12, 11], [-6, 17]]) # '#888888[#DDDDDD]'
# run SIVIA
(res_in, res_out, res_y) = pySIVIA(X0, sep, 0.1, **params)
vibes.drawAUV(robot[0], robot[1], 1, np.rad2deg(0.3))
for (x, y), d in zip(landmarks, dist):
vibes.drawCircle(x,y, 0.1, "[k]")
vibes.drawCircle(x,y, d.lb(), "k")
vibes.drawCircle(x,y, d.ub(), "k")
#equalize axis lenght
vibes.axisEqual()
vibes.endDrawing()
# init drawing area
vibes.beginDrawing()
vibes.newFigure('Result')
vibes.setFigureProperties({'x': 0, 'y': 0, 'width': 1000, 'height': 1000})
#configure pySIVIA output
params = {
'color_in': '#888888[#444444]',
'color_out': '#888888[#DDDDDD]',
'color_maybe': '#888888[w]',
'use_patch': True
}
# create the initial box X0 = [-10, 10] x [-10, 10]
X0 = IntervalVector([[-12, 11], [-6, 17]]) # '#888888[#DDDDDD]'
# run SIVIA
(res_in, res_out, res_y) = pySIVIA(X0, sep, 0.1, **params)
vibes.drawAUV(robot[0], robot[1], 1, np.rad2deg(0.3))
for (x, y), d in zip(landmarks, dist):
vibes.drawCircle(x, y, 0.1, "[k]")
vibes.drawCircle(x, y, d.lb(), "k")
vibes.drawCircle(x, y, d.ub(), "k")
#equalize axis lenght
vibes.axisEqual()
vibes.endDrawing()
from vibes import vibes
print(vibes.channel, vibes.current_fig)
vibes.beginDrawing()
vibes.newFigure("test")
vibes.drawBox(0, 1, 4, 6, color="[#FF12FFA0]", figure="test")
vibes.drawBox(0.5, 2, 4, 7, color="[#00FFFF66]", figure="test")
vibes.drawBox(-1, 0, -4, -6, color="r", figure="test")
vibes.drawLine([[0, 0], [4, 4]])
vibes.drawEllipse(3, 5, 1, 3, 45)
vibes.drawEllipse(5, 5, 3, 3, 0, angles=[30, 60], color="g")
vibes.drawCircle(0, 0, 5)
vibes.drawAUV(0, 0, 2, 3, color="r[yellow]")
vibes.drawPie([0, 0], [3, 4], [45, 90])
vibes.drawPie([0, 0], [5, 9], [-120, -40], "[b]")
vibes.drawPie([0, 0], [1, 2], [160, 220], "g[y]")
vibes.drawPie([5, 2], [1, 2], [160, 220], "g[y]")
vibes.newFigure("test2")
# vibes.drawBox(-1,0,-4,-6,color='[r]', figure='test2')
# vibes.selectFigure("test")
# vibes.clearFigure()
# INITIALISATION : bateaux (positions, vitesse)
T = 100
h = 0.05
nombreRobots = 1
listRobots = []
for i in range(0,nombreRobots):
listRobots.append(Robot(-4.0,45.0,0.0,0.0,i,'b',nombreRobots))
vibes.beginDrawing()
vibes.clearFigure()
vibes.drawLine([[-4.4,47.8],[-3.8,47.8],[-2.3,47.2],[-2,46.6],[-1.3,44.4],[-2.0,43.3],[-3.6,43.5],[-2.8,43.4]])
for t in frange(0,T,h):
# DEROULEMENT pour la simulation:
# u = Commande renvoyée par chaque régulateur de chaque bateau
# Une itération de la simulation grâce à u
# On change les positions des bateaux
# On recommence
for rob in listRobots:
vibes.drawAUV(float(rob.X[0]), float(rob.X[1]), 0.2, rob.thetaReel*180/math.pi, rob.couleur)
pwm = rob.regulate(t)
X,V,theta = simule(rob,pwm,h)
rob.setNewState(X,V,theta)
time.sleep(0.1)
vibes.endDrawing()
