Skip to content

Commit da4fe09

Browse files
iarameriaramer
committed
robotics lab3
1 parent f1228a3 commit da4fe09

File tree

4 files changed

+276
-0
lines changed

4 files changed

+276
-0
lines changed

‎python/mipt/mipt_robotics/op1/control_updated.py

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
#!/usr/bin/env python
2+
3+
import asyncio
4+
import websockets
5+
import json
6+
7+
from pid import PID
8+
9+
# TODO: Initialize the pid variable.
10+
#steering_pid = PID(0.053, 0.0, 0.0)
11+
12+
13+
#steering_pid = PID(0.03, 0.00004, 40.0) # pozdno povorachivaet
14+
#steering_pid = PID(0.03, 0.00004, 30.0) # nachal povorachivat
15+
#steering_pid = PID(0.03, 0.00004, 25.0) - # huzhe
16+
17+
#steering_pid = PID(0.03, 0.00006, 30.0) # povernul best
18+
#steering_pid = PID(0.03, 0.00006, 35.0) # ne povernul
19+
#steering_pid = PID(0.03, 0.00006, 27.0) # povernul
20+
#steering_pid = PID(0.045, 0.00006, 40.0) # povernul
21+
steering_pid = PID(0.1, 0.00001, 50.0)
22+
23+
# Checks if the SocketIO event has JSON data.
24+
# If there is data the JSON object in string format will be returned,
25+
# else the empty string "" will be returned.
26+
def getData(message):
27+
try:
28+
start = message.find("[")
29+
end = message.rfind("]")
30+
return message[start:end+1]
31+
except:
32+
return ""
33+
34+
async def handleTelemetry(websocket, msgJson):
35+
cte = msgJson[1]["cte"]
36+
speed = msgJson[1]["speed"]
37+
angle = msgJson[1]["steering_angle"]
38+
39+
print("CTE: ", cte, ", speed: ", speed, ", angle: ", angle)
40+
41+
42+
# TODO: Calculate steering value here, remember the steering value is
43+
# [-1, 1].
44+
# NOTE: Feel free to play around with the throttle and speed.
45+
# Maybe use another PID controller to control the speed!
46+
47+
steering_pid.UpdateError(cte)
48+
steer_value = steering_pid.TotalError()
49+
50+
if steer_value < -1:
51+
steer_value = -1
52+
if steer_value > 1:
53+
steer_value = 1
54+
55+
response = {}
56+
57+
response["steering_angle"] = steer_value
58+
59+
# TODO: Play around with throttle value
60+
61+
throttle = 0.2
62+
63+
speed, cte = float(speed), float(cte)
64+
65+
#if speed > 25.0:
66+
#throttle = 0.1
67+
if abs(cte) > 1.2:
68+
throttle = 0.1
69+
70+
#response["throttle"] = 0.3
71+
response["throttle"] = throttle
72+
73+
print("throttle ", throttle, "Steer value ", steer_value, "cte ", cte)
74+
75+
msg = "42[\"steer\"," + json.dumps(response) + "]"
76+
77+
await websocket.send(msg)
78+
79+
80+
async def echo(websocket, path):
81+
async for message in websocket:
82+
if len(message) < 3 \
83+
or message[0] != '4' \
84+
or message[1] != '2':
85+
return
86+
87+
s = getData(message);
88+
msgJson = json.loads(s)
89+
90+
event = msgJson[0]
91+
if event == "telemetry":
92+
await handleTelemetry(websocket, msgJson)
93+
else:
94+
msg = "42[\"manual\",{}]"
95+
await websocket.send(msg)
96+
97+
98+
def main():
99+
start_server = websockets.serve(echo, "localhost", 4567)
100+
101+
asyncio.get_event_loop().run_until_complete(start_server)
102+
asyncio.get_event_loop().run_forever()
103+
104+
105+
if __name__ == "__main__":
106+
main()

‎python/mipt/mipt_robotics/op1/pid_updated.py

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
#!/usr/bin/env python
2+
3+
class PID:
4+
5+
# TODO: Complete the PID class. You may add any additional desired functions
6+
7+
def __init__(self, Kp, Ki=0.0, Kd=0.0, K_imax=30):
8+
# TODO: Initialize PID coefficients (and errors, if needed)
9+
10+
self.K_p = Kp
11+
self.K_i = Ki
12+
self.K_d = Kd
13+
self.K_imax = K_imax
14+
self.p_error = 0.0
15+
self.i_error = 0.0
16+
self.d_error = 0.0
17+
18+
19+
def UpdateError(self, cte):
20+
# TODO: Update PID errors based on cte
21+
cte = float(cte)
22+
self.d_error = cte - self.p_error
23+
self.p_error = cte
24+
self.i_error += cte
25+
if abs(self.i_error) > self.K_imax:
26+
self.i_error = self.K_imax
27+
28+
def TotalError(self):
29+
# TODO: Calculate and return the total error
30+
31+
return -self.K_p * self.p_error - self.K_i * self.i_error - self.K_d * self.d_error

‎python/mipt/mipt_robotics/op2/control_updated.py

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
#!/usr/bin/env python
2+
import asyncio
3+
import websockets
4+
import json
5+
6+
from pid_updated import PID
7+
8+
# TODO: Initialize the pid variable.
9+
# steering_pid = PID(0.053, 0.0, 0.0)
10+
11+
12+
# steering_pid = PID(0.03, 0.00004, 40.0) # pozdno povorachivaet
13+
# steering_pid = PID(0.03, 0.00004, 30.0) # nachal povorachivat
14+
# steering_pid = PID(0.03, 0.00004, 25.0) - # huzhe
15+
16+
# steering_pid = PID(0.03, 0.00006, 30.0) # povernul best
17+
# steering_pid = PID(0.03, 0.00006, 35.0) # ne povernul
18+
# steering_pid = PID(0.03, 0.00006, 27.0) # povernul
19+
# steering_pid = PID(0.045, 0.00006, 40.0) # povernul
20+
pid = PID(0.1, 0.00001, 50.0)
21+
pid_throttle = PID(0.6, 0, 1)
22+
23+
24+
# Checks if the SocketIO event has JSON data.
25+
# If there is data the JSON object in string format will be returned,
26+
# else the empty string "" will be returned.
27+
def get_data(message):
28+
try:
29+
start = message.find("[")
30+
end = message.rfind("]")
31+
return message[start:end + 1]
32+
except:
33+
return ""
34+
35+
36+
async def handle_telemetry(websocket, msg_json):
37+
cte = msg_json[1]["cte"]
38+
speed = msg_json[1]["speed"]
39+
angle = msg_json[1]["steering_angle"]
40+
41+
cte = float(cte)
42+
speed = float(speed)
43+
angle = float(angle)
44+
45+
print("CTE: ", cte, ", speed: ", speed, ", angle: ", angle)
46+
47+
# TODO: Calculate steering value here, remember the steering value is
48+
# [-1, 1].
49+
# NOTE: Feel free to play around with the throttle and speed.
50+
# Maybe use another PID controller to control the speed!
51+
52+
pid.UpdateError(cte)
53+
steer_value = pid.get_control()
54+
55+
max_speed = 80
56+
max_angle = 25
57+
max_throttle = 0.3
58+
target_speed = max(0.0, max_speed * (1.0 - abs(angle / max_angle * cte) / 4))
59+
target_speed = min(100.0, target_speed)
60+
pid_throttle.UpdateError(speed - target_speed)
61+
throttle_value = min(max_throttle, 0.7 + pid_throttle.get_control())
62+
63+
response = {
64+
'steering_angle': steer_value,
65+
'throttle': throttle_value
66+
}
67+
68+
print("throttle ", throttle_value, "Steer value ", steer_value, "cte ", cte)
69+
msg = "42[\"steer\"," + json.dumps(response) + "]"
70+
71+
await websocket.send(msg)
72+
73+
74+
async def echo(web_socket, path):
75+
async for message in web_socket:
76+
if len(message) < 3 \
77+
or message[0] != '4' \
78+
or message[1] != '2':
79+
return
80+
81+
s = get_data(message)
82+
msg_json = json.loads(s)
83+
84+
event = msg_json[0]
85+
if event == "telemetry":
86+
await handle_telemetry(web_socket, msg_json)
87+
else:
88+
msg = "42[\"manual\",{}]"
89+
await web_socket.send(msg)
90+
91+
92+
def main():
93+
start_server = websockets.serve(echo, "localhost", 4567)
94+
print('Server started at localhost:4567')
95+
96+
asyncio.get_event_loop().run_until_complete(start_server)
97+
asyncio.get_event_loop().run_forever()
98+
99+
100+
if __name__ == "__main__":
101+
main()

‎python/mipt/mipt_robotics/op2/pid_updated.py

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
#!/usr/bin/env python
2+
3+
class PID:
4+
5+
# TODO: Complete the PID class. You may add any additional desired functions
6+
7+
def __init__(self, Kp, Ki=0.0, Kd=0.0, K_imax=30):
8+
# TODO: Initialize PID coefficients (and errors, if needed)
9+
10+
self.K_p = Kp
11+
self.K_i = Ki
12+
self.K_d = Kd
13+
self.K_imax = K_imax
14+
self.p_error = 0.0
15+
self.i_error = 0.0
16+
self.d_error = 0.0
17+
18+
def UpdateError(self, cte):
19+
# TODO: Update PID errors based on cte
20+
cte = float(cte)
21+
self.d_error = cte - self.p_error
22+
self.p_error = cte
23+
self.i_error += cte
24+
# if abs(self.i_error) > self.K_imax:
25+
# self.i_error = self.K_imax
26+
27+
def TotalError(self):
28+
# TODO: Calculate and return the total error
29+
return self.K_p * self.p_error + self.K_i * self.i_error + self.K_d * self.d_error
30+
31+
def get_control(self):
32+
control_value = - self.K_p * self.p_error - self.K_d * self.d_error - self.K_i * self.i_error
33+
if control_value < -1:
34+
control_value = -1
35+
elif control_value > 1:
36+
control_value = 1
37+
38+
return control_value

0 commit comments

Comments
 (0)