# G1 29 - Command parser và kiểm tra lệnh an toàn ## Cần chuẩn bị | Nhóm | Linh kiện / dụng cụ | Số lượng | Ghi chú | |---|---|---:|---| | Board | ESP32-S3 DevKit | 1 | Chạy parser và nhận lệnh Serial | | Robot base | Chassis 2WD, 2 motor DC, bánh xe, caster | 1 bộ | Đã chạy được Serial/Web Control | | Driver motor | TB6612FNG | 1 | Điều khiển hai motor | | Cảm biến khoảng cách | HC-SR04 | 1 | Echo qua chia áp hoặc level shifter | | Phản hồi | LED GPIO2, active buzzer GPIO15 | 1 mỗi loại | Báo trạng thái armed/lỗi | | Nguồn | Pin robot đã sạc, công tắc nguồn motor | 1 bộ | Luôn thử parser khi robot kê bánh trước | | Sân thử | Sàn phẳng, hộp giấy, băng dính vùng an toàn | 1 khu vực | Dùng kiểm tra lệnh hợp lệ sau khi parser pass | | Phần mềm | Arduino IDE, Serial Monitor | 1 máy | Baud 115200, line ending `Newline` | ## Bài này giúp bạn làm được gì? G1 27 và G1 28 đã cho robot nhận lệnh qua Serial và web. Nhưng nếu mỗi nơi tự xử lý lệnh một kiểu, robot sẽ khó mở rộng và dễ có lỗi an toàn. Bài này viết một command parser rõ ràng: lệnh phải đúng cú pháp, đúng giới hạn, robot phải được `ARM` trước khi chạy, và mọi lệnh sai đều dừng robot. Kết quả cuối bài: **robot chỉ chạy khi nhận lệnh hợp lệ dạng `MOVE FWD 90 500`, từ chối lệnh sai, yêu cầu `ARM` trước khi chạy, và dừng khi có vật cản gần**. ## Mục tiêu bài học Sau bài này, bạn có thể: - Thiết kế grammar lệnh đơn giản cho robot. - Tách bước parse lệnh khỏi bước chạy motor. - Kiểm tra tốc độ, thời gian, hướng và trạng thái an toàn. - Dùng bảng test case để chứng minh parser đúng. - Chuẩn bị nền cho G1 30 chuyển mode giữa line, tránh vật cản và remote. ## Pin map dùng trong bài | Chức năng | Chân ESP32-S3 | Module | |---|---|---| | HC-SR04 Trig | GPIO5 | HC-SR04 | | HC-SR04 Echo | GPIO18 qua chia áp | HC-SR04 | | LED trạng thái | GPIO2 | LED onboard/ngoài | | Buzzer | GPIO15 | Active buzzer | | STBY | GPIO33 | TB6612FNG | | Motor trái PWM/IN1/IN2 | GPIO25/GPIO26/GPIO27 | TB6612FNG | | Motor phải PWM/IN1/IN2 | GPIO14/GPIO12/GPIO13 | TB6612FNG | ## Grammar lệnh Trong bài này, robot chỉ chấp nhận các lệnh sau: ```text ARM HOLD STOP STATUS MOVE FWD MOVE BACK MOVE LEFT MOVE RIGHT ``` Ví dụ hợp lệ: ```text ARM MOVE FWD 90 500 MOVE LEFT 80 300 STOP ``` Ví dụ bị từ chối: ```text MOVE FWD 200 500 MOVE JUMP 90 500 FWD 90 500 MOVE FWD 90 5000 ``` ## Thực hành từng bước 1. Kê robot lên, upload code và mở Serial Monitor. 2. Gõ `MOVE FWD 90 500` khi chưa `ARM`; robot phải từ chối. 3. Gõ `ARM`, LED bật để báo robot sẵn sàng nhận lệnh chạy. 4. Gõ `MOVE FWD 90 500`; bánh quay tiến rồi tự dừng. 5. Gõ các lệnh sai trong bảng test case, xác nhận robot không chạy. 6. Đặt hộp giấy gần cảm biến, gõ `MOVE FWD 90 500`; robot phải từ chối vì vật cản gần. 7. Đặt robot xuống sàn, chạy 4 lệnh hợp lệ trong vùng thử. 8. Ghi lại log của ít nhất 6 test case. ## Code mẫu hoàn chỉnh ```cpp /* G1 29 - Command parser va kiem tra lenh an toan Board: ESP32-S3 DevKit + TB6612FNG + HC-SR04 */ const int PIN_TRIG = 5; const int PIN_ECHO = 18; const int PIN_LED = 2; const int PIN_BUZZER = 15; const int PIN_STBY = 33; const int L_PWM = 25; const int L_IN1 = 26; const int L_IN2 = 27; const int R_PWM = 14; const int R_IN1 = 12; const int R_IN2 = 13; const int MIN_SPEED = 50; const int MAX_SPEED = 130; const unsigned long MIN_MS = 100; const unsigned long MAX_MS = 1200; const float SAFE_FORWARD_CM = 20.0; const unsigned long SENSOR_INTERVAL_MS = 80; enum Motion { MOTION_STOP, MOTION_FWD, MOTION_BACK, MOTION_LEFT, MOTION_RIGHT }; enum CommandType { CMD_INVALID, CMD_ARM, CMD_HOLD, CMD_STOP, CMD_STATUS, CMD_MOVE }; struct ParsedCommand { CommandType type; Motion motion; int speed; unsigned long durationMs; String reason; }; bool armed = false; Motion currentMotion = MOTION_STOP; unsigned long motionEndsAt = 0; unsigned long lastSensorMs = 0; float distanceCm = -1.0; const char *motionName(Motion m) { switch (m) { case MOTION_STOP: return "STOP"; case MOTION_FWD: return "FWD"; case MOTION_BACK: return "BACK"; case MOTION_LEFT: return "LEFT"; case MOTION_RIGHT: return "RIGHT"; } return "UNKNOWN"; } String tokenAt(const String &text, int index) { int current = 0; int start = 0; for (int i = 0; i <= text.length(); i++) { if (i == text.length() || text.charAt(i) == ' ') { if (i > start) { if (current == index) return text.substring(start, i); current++; } start = i + 1; } } return ""; } float readDistanceCm() { digitalWrite(PIN_TRIG, LOW); delayMicroseconds(2); digitalWrite(PIN_TRIG, HIGH); delayMicroseconds(10); digitalWrite(PIN_TRIG, LOW); unsigned long duration = pulseIn(PIN_ECHO, HIGH, 30000UL); if (duration == 0) return -1.0; float cm = duration * 0.0343 / 2.0; if (cm < 3.0 || cm > 150.0) return -1.0; return cm; } void updateSensor() { unsigned long now = millis(); if (now - lastSensorMs < SENSOR_INTERVAL_MS) return; lastSensorMs = now; distanceCm = readDistanceCm(); } void setMotor(int pwmPin, int in1, int in2, int speed) { speed = constrain(speed, -255, 255); digitalWrite(in1, speed >= 0 ? HIGH : LOW); digitalWrite(in2, speed >= 0 ? LOW : HIGH); analogWrite(pwmPin, abs(speed)); } void drive(int leftSpeed, int rightSpeed) { setMotor(L_PWM, L_IN1, L_IN2, leftSpeed); setMotor(R_PWM, R_IN1, R_IN2, rightSpeed); } void stopRobot(const char *reason) { drive(0, 0); currentMotion = MOTION_STOP; Serial.print("event=STOP reason="); Serial.println(reason); } void beep(bool ok) { int count = ok ? 1 : 2; for (int i = 0; i < count; i++) { digitalWrite(PIN_BUZZER, HIGH); delay(25); digitalWrite(PIN_BUZZER, LOW); delay(35); } } ParsedCommand rejectCommand(const String &reason) { ParsedCommand parsed; parsed.type = CMD_INVALID; parsed.motion = MOTION_STOP; parsed.speed = 0; parsed.durationMs = 0; parsed.reason = reason; return parsed; } ParsedCommand parseCommand(String line) { line.trim(); line.toUpperCase(); if (line == "ARM") return {CMD_ARM, MOTION_STOP, 0, 0, "OK"}; if (line == "HOLD") return {CMD_HOLD, MOTION_STOP, 0, 0, "OK"}; if (line == "STOP") return {CMD_STOP, MOTION_STOP, 0, 0, "OK"}; if (line == "STATUS") return {CMD_STATUS, MOTION_STOP, 0, 0, "OK"}; if (tokenAt(line, 0) != "MOVE") return rejectCommand("UNKNOWN_COMMAND"); String direction = tokenAt(line, 1); String speedText = tokenAt(line, 2); String timeText = tokenAt(line, 3); if (direction.length() == 0 || speedText.length() == 0 || timeText.length() == 0) { return rejectCommand("MISSING_FIELD"); } if (tokenAt(line, 4).length() > 0) return rejectCommand("TOO_MANY_FIELDS"); Motion motion = MOTION_STOP; if (direction == "FWD") motion = MOTION_FWD; else if (direction == "BACK") motion = MOTION_BACK; else if (direction == "LEFT") motion = MOTION_LEFT; else if (direction == "RIGHT") motion = MOTION_RIGHT; else return rejectCommand("BAD_DIRECTION"); int speed = speedText.toInt(); unsigned long durationMs = timeText.toInt(); if (speed < MIN_SPEED || speed > MAX_SPEED) return rejectCommand("BAD_SPEED"); if (durationMs < MIN_MS || durationMs > MAX_MS) return rejectCommand("BAD_DURATION"); ParsedCommand parsed; parsed.type = CMD_MOVE; parsed.motion = motion; parsed.speed = speed; parsed.durationMs = durationMs; parsed.reason = "OK"; return parsed; } void runMove(const ParsedCommand &cmd) { if (!armed) { stopRobot("NOT_ARMED"); beep(false); return; } updateSensor(); if (cmd.motion == MOTION_FWD && distanceCm > 0 && distanceCm <= SAFE_FORWARD_CM) { stopRobot("OBSTACLE_TOO_CLOSE"); beep(false); return; } currentMotion = cmd.motion; motionEndsAt = millis() + cmd.durationMs; if (cmd.motion == MOTION_FWD) drive(cmd.speed, cmd.speed); if (cmd.motion == MOTION_BACK) drive(-cmd.speed, -cmd.speed); if (cmd.motion == MOTION_LEFT) drive(-cmd.speed, cmd.speed); if (cmd.motion == MOTION_RIGHT) drive(cmd.speed, -cmd.speed); beep(true); Serial.print("event=MOVE_ACCEPT motion="); Serial.print(motionName(cmd.motion)); Serial.print(" speed="); Serial.print(cmd.speed); Serial.print(" duration_ms="); Serial.println(cmd.durationMs); } void printStatus() { Serial.print("status armed="); Serial.print(armed ? "YES" : "NO"); Serial.print(" motion="); Serial.print(motionName(currentMotion)); Serial.print(" distance_cm="); Serial.println(distanceCm, 1); } void executeCommand(const ParsedCommand &cmd) { switch (cmd.type) { case CMD_ARM: armed = true; digitalWrite(PIN_LED, HIGH); stopRobot("ARMED"); beep(true); break; case CMD_HOLD: armed = false; digitalWrite(PIN_LED, LOW); stopRobot("HOLD"); beep(true); break; case CMD_STOP: stopRobot("USER_STOP"); beep(true); break; case CMD_STATUS: printStatus(); break; case CMD_MOVE: runMove(cmd); break; case CMD_INVALID: stopRobot("BAD_COMMAND"); beep(false); Serial.print("reject="); Serial.println(cmd.reason); break; } } void printHelp() { Serial.println("Commands:"); Serial.println(" ARM | HOLD | STOP | STATUS"); Serial.println(" MOVE FWD 90 500 | MOVE BACK 80 400 | MOVE LEFT 90 300 | MOVE RIGHT 90 300"); } void setup() { Serial.begin(115200); pinMode(PIN_TRIG, OUTPUT); pinMode(PIN_ECHO, INPUT); pinMode(PIN_LED, OUTPUT); pinMode(PIN_BUZZER, OUTPUT); pinMode(PIN_STBY, OUTPUT); pinMode(L_PWM, OUTPUT); pinMode(L_IN1, OUTPUT); pinMode(L_IN2, OUTPUT); pinMode(R_PWM, OUTPUT); pinMode(R_IN1, OUTPUT); pinMode(R_IN2, OUTPUT); digitalWrite(PIN_STBY, HIGH); stopRobot("BOOT"); Serial.println("G1 29 safe command parser ready"); printHelp(); } void loop() { updateSensor(); if (Serial.available()) { ParsedCommand cmd = parseCommand(Serial.readStringUntil('\n')); executeCommand(cmd); } if (currentMotion == MOTION_FWD && distanceCm > 0 && distanceCm <= SAFE_FORWARD_CM) { stopRobot("OBSTACLE_DURING_FORWARD"); beep(false); } if (currentMotion != MOTION_STOP && millis() >= motionEndsAt) { stopRobot("TIME_DONE"); } } ``` ## Bảng test case bắt buộc | Test | Lệnh | Kết quả mong đợi | Pass? | |---:|---|---|---| | 1 | `MOVE FWD 90 500` trước `ARM` | Từ chối, robot không chạy | [ ] | | 2 | `ARM` | LED bật, robot vẫn dừng | [ ] | | 3 | `MOVE FWD 90 500` | Robot tiến ngắn rồi dừng | [ ] | | 4 | `MOVE FWD 200 500` | Từ chối `BAD_SPEED` | [ ] | | 5 | `MOVE JUMP 90 500` | Từ chối `BAD_DIRECTION` | [ ] | | 6 | `MOVE FWD 90 5000` | Từ chối `BAD_DURATION` | [ ] | | 7 | `HOLD` rồi `MOVE LEFT 90 300` | Từ chối `NOT_ARMED` | [ ] | | 8 | `STATUS` | In armed, motion, distance | [ ] | ## Cách đọc log | Log | Ý nghĩa | |---|---| | `event=MOVE_ACCEPT` | Parser pass và robot bắt đầu chạy | | `reject=BAD_SPEED` | Speed ngoài giới hạn | | `reject=BAD_DIRECTION` | Hướng không thuộc danh sách | | `reason=NOT_ARMED` | Chưa bật trạng thái cho phép chạy | | `reason=OBSTACLE_TOO_CLOSE` | HC-SR04 báo vật cản gần | ## Lỗi thường gặp và cách sửa | Lỗi | Nguyên nhân hay gặp | Cách sửa | |---|---|---| | Lệnh đúng nhưng bị từ chối | Chưa gửi `ARM` | Gửi `ARM`, xem LED bật | | Parser nhận sai token | Gõ thừa hoặc thiếu khoảng trắng | Gõ đúng dạng `MOVE FWD 90 500` | | Robot chạy dù lệnh sai | Code cũ chưa upload hoặc chưa gọi `stopRobot()` khi reject | Upload lại, kiểm tra log `BAD_COMMAND` | | Vật cản gần nhưng robot vẫn tiến | HC-SR04 đọc lỗi hoặc lắp lệch | Kiểm tra `distance_cm`, chỉnh hướng cảm biến | ## Checklist debug - [ ] Parser từ chối lệnh sai trước khi motor chạy. - [ ] Robot cần `ARM` trước khi chạy. - [ ] `HOLD` khóa lệnh chạy. - [ ] Speed và duration có giới hạn rõ ràng. - [ ] Có bảng test case kèm log thật. ## Thử thách nhỏ Thêm lệnh `MOVE SLOWFWD 60 400` bằng cách mở rộng parser. Sau đó giải thích vì sao thêm một hướng mới phải đi qua parser, không gọi motor trực tiếp trong `loop()`. ## Checklist hoàn thành - [ ] Robot chỉ chạy khi command parser trả về lệnh hợp lệ. - [ ] Mọi lệnh sai đều làm robot dừng. - [ ] Có ít nhất 8 test case parser. - [ ] Có video demo robot thật với `ARM`, `MOVE`, `STOP`, `HOLD`. - [ ] Sẵn sàng tích hợp parser vào mode switching ở G1 30. ## Bài tiếp theo G1 30 sẽ dùng parser an toàn này để chuyển giữa các mode: dò line, tránh vật cản và remote, sao cho robot không chạy bất ngờ khi đổi mode. --- ## Điều hướng bài học **Bài tiếp theo:** G1 30 - Chuyển mode giữa dò line, tránh vật cản và remote **Bài trước:** G1 28 - Tạo Web Control đơn giản trên ESP32

Command parser và kiểm tra lệnh an toàn

Ghi danh khóa học để mở bài này