AI 모빌리티 공학 캠프 (대학)
제공 코드3일차 ②, PID 클래스 (출력 제한과 적분 고정)
| 언어 | Python |
|---|---|
| 설명 | 카메라 자율주행 실습 코드 control.py 발췌 |
def clamp(value, low, high):
return max(low, min(high, value))
class PID:
def __init__(self, kp=0.55, ki=0.04, kd=0.08, output_limit=0.48):
self.kp, self.ki, self.kd = kp, ki, kd
self.limit = output_limit
self.integral = 0.0
self.previous = None
def reset(self):
self.integral = 0.0
self.previous = None
def update(self, error, dt):
if dt <= 0:
raise ValueError('dt must be positive')
derivative = 0 if self.previous is None else (error-self.previous)/dt
trial = clamp(self.integral + error*dt, -2.0, 2.0)
raw = self.kp*error + self.ki*trial + self.kd*derivative
value = clamp(raw, -self.limit, self.limit)
# Freeze integration only when it would push farther into saturation.
if abs(raw) <= self.limit or error*raw < 0:
self.integral = trial
self.previous = error
return valueAI 모빌리티 공학 캠프 (대학)
제공 코드3일차 ②, 4일차 ①, 상태 판단: 센서 정지, 양보, 재출발
| 언어 | Python |
|---|---|
| 설명 | control.py의 Controller.step 발췌. 충전과 열 관리 부분은 생략 |
def step(self, perception, telemetry, dt):
p, t = perception, telemetry
# stale frame or weak lane -> stop
if p.frame_age > 0.25 or p.lane_confidence < 0.35:
self.clear_time = 0.0
return self.stop('SENSOR_STOP')
hazard = p.red_light or p.pedestrian_in_path
if hazard:
self.clear_time = 0.0
self.hazard_latched = True
elif self.hazard_latched:
self.clear_time = self.clear_time + dt if p.path_clear else 0.0
# restart only after the path stays clear for 1.5 s
if self.hazard_latched and self.clear_time < 1.5:
return self.stop('YIELD')
self.hazard_latched = False
target = self.cruise / (1.0 + 2.8*abs(p.curvature))
if p.lane_confidence < 0.65:
target = min(target, 0.35)
desired_steer = self.pid.update(p.lane_error, dt)
# Rate limiting prevents sudden steering changes.
self.steer += clamp(desired_steer-self.steer, -0.6*dt, 0.6*dt)
return Command('FOLLOW', target, self.steer)AI 모빌리티 공학 캠프 (대학)
제공 코드3일차 ②, ROI 차선 중심 오차
| 언어 | Python (OpenCV) |
|---|---|
| 설명 | perception.py의 LaneDetector.predict 발췌 |
def predict(self, frame):
height, width = frame.shape[:2]
polygon = (self.roi*np.array([width, height])).astype(np.int32)
roi_mask = np.zeros((height, width), np.uint8)
cv2.fillPoly(roi_mask, [polygon], 255)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
white = cv2.inRange(hsv, (0, 0, self.white_v), (179, 80, 255))
yellow = cv2.inRange(hsv, (15, 65, 80), (40, 255, 255))
mask = cv2.bitwise_and(white | yellow, roi_mask)
clean = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))
edges = cv2.Canny(cv2.GaussianBlur(clean, (5, 5), 0), *self.canny)
segments = cv2.HoughLinesP(edges, 1, np.pi/180, 35, minLineLength=25, maxLineGap=30)
# ... split segments into left/right by x position, fit x = a*y + b ...
sample_y = .78*height
lx, rx = np.polyval(l, sample_y), np.polyval(r, sample_y)
error = ((lx+rx)/2 - width/2) / (width/2) # -1(left) .. +1(right)
return dict(error=float(error), confidence=min(1.0, (len(left)+len(right))/40))AI 모빌리티 공학 캠프 (대학)
제공 코드2일차 ①, 가변저항으로 관절 각도 제어
| 언어 | Arduino C++ |
|---|---|
| 설명 | 로봇팔 키트 예제 Potentiometer_control.ino 발췌. 2025 로봇팔 수업 배포 코드 |
#include <Servo.h>
Servo servo1, servo2, servo3, servo4, servo5;
void setup() {
servo1.attach(9); // base
servo2.attach(6);
servo3.attach(5);
servo4.attach(3);
servo5.attach(11); // gripper
}
void loop() {
// potentiometer 0..1023 -> servo angle
servo1.write(map(analogRead(0), 0, 1023, 0, 180));
servo2.write(map(analogRead(1), 0, 1023, 0, 180));
servo3.write(map(analogRead(2), 0, 1023, 0, 180));
servo4.write(map(analogRead(3), 0, 1023, 0, 180));
servo5.write(map(analogRead(6), 0, 1023, 60, 160)); // narrower gripper range
delay(50);
}AI 모빌리티 공학 캠프 (대학)
제공 코드4일차 ②, 집기와 분류 상태 기계
| 언어 | Python |
|---|---|
| 설명 | 대기, 집기, 이동, 복귀 상태와 실패 시 재시도, 정지를 담은 코드 골격입니다. arm, camera 객체는 팀 장비에 맞춰 구현 |
MIN_CONF = 0.5 # chosen from handout 5
MAX_RETRY = 2
def run_cycle(arm, camera, to_xy, trays):
state, retry = "DETECT", 0
while True:
if state == "DETECT":
dets = [d for d in camera.detect() if d.conf >= MIN_CONF]
if not dets:
retry += 1
state = "DETECT" if retry <= MAX_RETRY else "DONE"
continue
target, retry = dets[0], 0
state = "PICK"
elif state == "PICK":
x, y = to_xy(target.cx, target.cy) # px -> mm
ok = arm.approach(x, y) and arm.grip()
if ok:
state = "PLACE"
else:
retry += 1
state = "PICK" if retry <= 1 else "ERROR"
elif state == "PLACE":
arm.move_to(*trays[target.label])
arm.release()
arm.home()
state = "DETECT"
elif state == "ERROR":
arm.home()
return "ERROR" # operator checks, then restarts
elif state == "DONE":
arm.home()
return "DONE"AI 모빌리티 공학 캠프 (대학)
제공 코드3일차 ①, 화면 좌표를 작업대 좌표로 보정
| 언어 | Python (OpenCV) |
|---|---|
| 설명 | 기준점 4쌍으로 원근 변환 행렬을 구해 픽셀 중심을 mm로 바꾼다(활동지 4와 함께 사용) |
import cv2
import numpy as np
# reference marks on the calibration board: pixel (u, v) -> table (x, y) in mm
px = np.float32([[102, 88], [538, 92], [530, 410], [110, 402]])
mm = np.float32([[0, 0], [300, 0], [300, 220], [0, 220]])
H = cv2.getPerspectiveTransform(px, mm)
def to_xy(u, v):
p = H @ np.array([u, v, 1.0])
return p[0]/p[2], p[1]/p[2]
# check with a point that was not used for calibration
print(to_xy(320, 250)) # compare with a ruler measurement