Skip to content
Snippets Groups Projects
Commit d87c8d07 authored by Ouhouuhu's avatar Ouhouuhu
Browse files

calibration discretisation

parent a7c9e66a
Branches
No related tags found
No related merge requests found
import serial #Importation de la bibliothèque « pySerial »
import time
def get_ser():
ser = serial.Serial(port="COM3", baudrate=115200, timeout=0.1,write_timeout=0.2) #Création du port lié au COM6 a une vitesse de 115200 bauds et un timout d’une seconde
#time.sleep(3)
return ser
def start_ser(ser):
ser.close() #Cloture du port pour le cas ou il serait déjà ouvert ailleurs
ser.open() #Ouverture du port
def close_ser(ser):
ser.close()
def send_infos(ser, a,b,c):
n = f"['{a} {b} {c}']"
#print("input: "+str(n))
ser.write((str(n)+'\n').encode("utf-8"))
cc=str(ser.readline())
#print("output: "+cc[2:][:-5])
#print("Success")
return cc[2:][:-5]
\ No newline at end of file
import matplotlib.pyplot as plt
from shapely.geometry import Point, LineString
from time import sleep, time
from car_interface import get_ser, start_ser, close_ser, send_infos
import track_2_generator as tg
from pid_controller import PidController
from mesure_discret_to_adim import pol2xy
import json
def update_pos(x,y,cp):
m = tg.create_track(0.5,0,0, (0,0))[:-1]+[(1,0.5)]
c = tg.get_cone_map()
p = tg.checkpoints(cp)
plt.plot([x for x,y in m], [y for x,y in m], '--g')
plt.plot(x,y, "r+")
plt.plot([x for x,y in c], [y for x,y in c], 'ob')
plt.plot([x for x,y in p], [y for x,y in p], '--r')
plt.axis("equal")
plt.show()
return
if __name__=="__main__":
essai_number = input("Numero de l'essai")
p,i,d = 1000,100,1000
"""initilize PID"""
pid = PidController(p,i,d)
"""initialize CP"""
cp=0
data = {"PID": {"P":p,"I":i,"D":d}, 'run': []}
"""loop"""
t = True
while t:
step_data = {}
"""set new pos"""
r, theta = input('cercle, r, theta: d ').split(' ')
c = "d"
#c, r, theta = "d", "1.2", "0"
if c=="s":
break
"""loading communication"""
#ser = get_ser()
#start_ser(ser)
r, theta = float(r), float(theta)
step_data["input"] = {"cercle":c, "r":r, "theta":theta}
#x,y = input("x,y: ").split(' ')
x,y = pol2xy(c, r, theta)
step_data["pos"] = {"x":x, "y":y}
"""get distance"""
current_cp = LineString(tg.checkpoints(cp))
n_cp=cp+1
if cp==4:
n_cp=1
next_cp = LineString(tg.checkpoints(n_cp))
pos = Point(x,y)
d1 = current_cp.distance(pos)
d2 = next_cp.distance(pos)
d=d1
if d1>d2:
cp=n_cp
d = d2
step_data["dist"] = d
step_data["cp"] = cp
print(f"d: {d}")
"""display"""
update_pos(x,y,cp)
"""get new order"""
speed = 10
direction = pid.get_control(d)
g = tg.is_in_track(x, y)
if x<1:
g = not g
if not g:
direction*=-1
dec = 60
braquage = 150
direction = direction+dec#decallage
if abs(direction-dec)>braquage:
direction = direction/abs(direction)*braquage+dec
print("Max turn")
direction = int(direction)
step_data["order"] = {"speed":speed, "direction":direction}
#direction = 100
print(f"direction: {direction}")
"""set new order"""
t_end = time() + 3
while time() < t_end:
#a = send_infos(ser, speed, direction, 0)
#print(f"capteurs: {a}")
pass
"""stop"""
t_end = time() + 2
while time() < t_end:
#send_infos(ser, 0, direction, 0)
pass
data["run"].append(step_data)
#close_ser(ser)
with open(f'essai_{essai_number}.json', 'w') as outfile:
json.dump(data, outfile)
\ No newline at end of file
import math
def pol2xy(cercle,rayon,angle):
distance_norm=rayon/2.4 #18.25 distance entre les deux cercles, égale à 1 ici
print(distance_norm)
# Angle mesuré depuis le centre du cercle choisi vers le point (1,1) dans le sens Trigo
angle_rad=math.pi*angle/180
if cercle=="g":
x=0.5+distance_norm*math.cos(angle_rad)
y=0.5+distance_norm*math.sin(angle_rad)
else:
x=1.5-distance_norm*math.cos(angle_rad)
y=0.5-distance_norm*math.sin(angle_rad)
return(x,y)
if __name__=="__main__":
print(pol2xy("droite",1.2,-90))
\ No newline at end of file
import numpy as np
class PidController:
def __init__(self, p_gain, i_gain, d_gain, set_point=0):
self.p_gain = p_gain
self.i_gain = i_gain
self.d_gain = d_gain
self.set_point = set_point
self.integrated_error = 0
self.previous_error = 0
def get_control(self, process_value):
error = self.set_point - process_value
control = self.p_gain * error + self.i_gain * self.integrated_error + self.d_gain * (error - self.previous_error)
self.previous_error = error
self.integrated_error += error
return np.sign(control) * abs(control)
import keyboard
def get_command():
forward = 0
direction = 30
if keyboard.is_pressed('q'):
direction=-50
elif keyboard.is_pressed('d'):
direction=110
if keyboard.is_pressed('z'):
forward = 10
if keyboard.is_pressed('s'):
forward = -10
return forward, direction, 0
\ No newline at end of file
from math import cos, sin, pi
import matplotlib.pyplot as plt
from shapely.geometry import Point, Polygon
def create_track(r, start_point_distance_y, end_point_distance_y, tot_shift):
laps = 1 #nombre de tours
resolution_circle = 10 #resolution du cercle en degre
resolution_line = 10
#contruire les points qui vont former le chemin
points = [(0, start_point_distance_y)]
current_x, current_y = 0, start_point_distance_y
if current_y<0:
#faire les points de la première ligne droite
while current_y<0:
current_y += resolution_line
points.append((current_x, current_y))
#faire les points des cercles
current_angle = 0 #angle en degree
for i in range(laps):
while current_angle<720:
rad = current_angle/360*2*pi
if 0<current_angle<360:
current_x, current_y = r-r*cos(rad), r*sin(rad)
else:
current_x, current_y = -(r-r*cos(rad)), r*sin(rad)
current_angle+=resolution_circle
points.append((current_x, current_y))
if current_y<end_point_distance_y:
current_x=0
while current_y<end_point_distance_y:
current_y += resolution_line
points.append((current_x, current_y))
#on decale les points
shift_x = -min([x[0] for x in points])
shift_y = -min([x[1] for x in points])
points = [(x+shift_x+tot_shift[0],y+shift_y+tot_shift[1]) for x,y in points]
#print(points)
return points
def create_double_circles(r, left_center, right_center):
resolution_circle = 10 #resolution du cercle en degre
current_angle=0
l_l = []
l_r = []
while current_angle<360:
rad = current_angle/360*2*pi
current_x_left, current_y_left = left_center[0]+r*cos(rad), left_center[1]+r*sin(rad)
current_x_right, current_y_right = right_center[0]+r*cos(rad), right_center[1]+r*sin(rad)
#if current_x_left<(left_center[0]+right_center[0])/2:
l_l.append((current_x_left, current_y_left))
#if current_x_right>(left_center[0]+right_center[0])/2:
l_r.append((current_x_right, current_y_right))
current_angle+=resolution_circle
return l_l+l_r
def get_cone_map():
pixel_x_ext, pixel_y_ext = [242, 220, 165, 110, 63, 33, 22, 34, 63, 110, 165, 220, 243, 310, 334, 388, 443, 490, 521, 531, 520, 489, 443, 388, 333, 310], [76, 64, 52, 64, 95, 141, 196, 252, 298, 330, 340, 328, 318, 316, 328, 339, 329, 298, 251, 196, 142, 95, 64, 53, 64, 77]
pixel_x_int, pixel_y_int = [245, 238, 222, 196, 166, 134, 108, 91, 85, 90, 109, 134, 165, 196, 222, 239, 308, 314, 332, 358, 388, 419, 445, 462, 468, 462, 445, 419, 388, 359, 332, 314], [201, 167, 140, 123, 116, 123, 140, 165, 195, 228, 253, 270, 277, 270, 253, 227, 200, 226, 253, 270, 277, 270, 253, 228, 197, 166, 140, 122, 117, 123, 140, 166]
diametre = 225
centre_x, centre_y = 278,200
coord_ext = [((i-centre_x)/diametre+1 , (j-centre_y)/diametre+0.5) for i,j in zip(pixel_x_ext, pixel_y_ext)]
coord_int = [((i-centre_x)/diametre+1 , (j-centre_y)/diametre+0.5) for i,j in zip(pixel_x_int, pixel_y_int)]
return coord_int+coord_ext
def checkpoints(n):
points = create_track(0.5,0,0, (0,0))[:-1]+[(1,0.5)]
if n==0:
return points[:11]
elif n==1:
return points[11:29]
elif n==2:
return points[29:47]
elif n==3:
return points[47:65]
elif n==4:
return points[65:]+points[1:11]
return points
def is_in_track(x,y):
t = Polygon(create_track(0.5,0,0, (0,0))[:-1]+[(1,0.5)])
p = Point(x,y)
return t.contains(p)
if __name__ =="__main__":
points = create_track(0.5,0,0, (0,0))[:-1]+[(1,0.5)]
cp_points = checkpoints(1)
plt.plot([x for x,y in cp_points], [y for x,y in cp_points], 'or')
plt.plot([x for x,y in points], [y for x,y in points])
"""
#construire le svg
mid = ""
#prendre le code autour du chemin
f = open("basic_track.svg", "r")
t = f.read().split('CUT_HERE')
for i,point in enumerate(points):
mid += f'<circle \n id="path{i}" \n style="fill:#000000;stroke:none" \n cx="{point[0]}" \n cy="{point[1]}" \n r="0.1" /> \n'
t = t[0]+mid+t[1]
f = open("track_11.svg", "w")
f.write(t)
f.close()
"""
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment