/* G1 33 - Capstone Robot Gen 1 da che do Board: ESP32-S3 DevKit + TB6612FNG + HC-SR04 + 3 analog line sensors */ 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 LINE_LEFT = 4; const int LINE_CENTER = 6; const int LINE_RIGHT = 7; const int LINE_THRESHOLD = 1800; const bool LINE_IS_DARK = true; const float SAFE_FORWARD_CM = 18.0; const unsigned long SENSOR_INTERVAL_MS = 80; const unsigned long TELEMETRY_INTERVAL_MS = 500; enum Mode { MODE_HOLD, MODE_LINE, MODE_OBSTACLE, MODE_REMOTE }; enum Motion { MOTION_STOP, MOTION_FWD, MOTION_BACK, MOTION_LEFT, MOTION_RIGHT }; Mode currentMode = MODE_HOLD; Motion currentMotion = MOTION_STOP; unsigned long motionEndsAt = 0; unsigned long lastSensorMs = 0; unsigned long lastTelemetryMs = 0; bool demoActive = false; int demoStep = 0; unsigned long demoStepEndsAt = 0; float distanceCm = -1.0; int lineL = 0; int lineC = 0; int lineR = 0; int leftPwm = 0; int rightPwm = 0; const char *modeName(Mode mode) { switch (mode) { case MODE_HOLD: return "HOLD"; case MODE_LINE: return "LINE"; case MODE_OBSTACLE: return "OBSTACLE"; case MODE_REMOTE: return "REMOTE"; } return "UNKNOWN"; } const char *motionName(Motion motion) { switch (motion) { 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; } bool lineDetected(int value) { return LINE_IS_DARK ? value < LINE_THRESHOLD : value > LINE_THRESHOLD; } void updateSensors() { unsigned long now = millis(); if (now - lastSensorMs < SENSOR_INTERVAL_MS) return; lastSensorMs = now; distanceCm = readDistanceCm(); lineL = analogRead(LINE_LEFT); lineC = analogRead(LINE_CENTER); lineR = analogRead(LINE_RIGHT); } 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, Motion motion) { leftPwm = constrain(leftSpeed, -255, 255); rightPwm = constrain(rightSpeed, -255, 255); currentMotion = motion; setMotor(L_PWM, L_IN1, L_IN2, leftPwm); setMotor(R_PWM, R_IN1, R_IN2, rightPwm); } void stopRobot(const char *reason) { drive(0, 0, MOTION_STOP); motionEndsAt = 0; Serial.print("event=STOP reason="); Serial.println(reason); } void beep(int count) { for (int i = 0; i < count; i++) { digitalWrite(PIN_BUZZER, HIGH); delay(25); digitalWrite(PIN_BUZZER, LOW); delay(35); } } void setMode(Mode nextMode, const char *reason) { stopRobot(reason); currentMode = nextMode; digitalWrite(PIN_LED, currentMode == MODE_HOLD ? LOW : HIGH); Serial.print("event=MODE_CHANGE mode="); Serial.println(modeName(currentMode)); } void runLineMode() { bool left = lineDetected(lineL); bool center = lineDetected(lineC); bool right = lineDetected(lineR); if (center) drive(85, 85, MOTION_FWD); else if (left) drive(50, 100, MOTION_LEFT); else if (right) drive(100, 50, MOTION_RIGHT); else drive(-55, 55, MOTION_LEFT); } void runObstacleMode() { if (distanceCm > 0 && distanceCm <= SAFE_FORWARD_CM) drive(-70, 70, MOTION_LEFT); else drive(80, 80, MOTION_FWD); } void startRemoteMove(Motion motion, int speed, unsigned long durationMs) { if (currentMode != MODE_REMOTE) { stopRobot("MOVE_OUTSIDE_REMOTE"); beep(2); return; } speed = constrain(speed, 50, 130); durationMs = constrain(durationMs, 100UL, 1200UL); if (motion == MOTION_FWD && distanceCm > 0 && distanceCm <= SAFE_FORWARD_CM) { stopRobot("OBSTACLE_TOO_CLOSE"); beep(2); return; } if (motion == MOTION_FWD) drive(speed, speed, motion); else if (motion == MOTION_BACK) drive(-speed, -speed, motion); else if (motion == MOTION_LEFT) drive(-speed, speed, motion); else if (motion == MOTION_RIGHT) drive(speed, -speed, motion); motionEndsAt = millis() + durationMs; Serial.print("event=REMOTE_MOVE motion="); Serial.print(motionName(motion)); Serial.print(" speed="); Serial.print(speed); Serial.print(" duration_ms="); Serial.println(durationMs); } void updateRemoteMotion() { if (currentMotion != MOTION_STOP && motionEndsAt > 0 && millis() >= motionEndsAt) { stopRobot("TIME_DONE"); } if (currentMotion == MOTION_FWD && distanceCm > 0 && distanceCm <= SAFE_FORWARD_CM) { stopRobot("OBSTACLE_DURING_REMOTE"); beep(2); } } void startDemo() { demoActive = true; demoStep = 0; demoStepEndsAt = millis(); stopRobot("DEMO_START"); Serial.println("event=DEMO_START"); beep(3); } void updateDemo() { if (!demoActive || millis() < demoStepEndsAt) return; demoStep++; if (demoStep == 1) { setMode(MODE_LINE, "DEMO_LINE"); demoStepEndsAt = millis() + 7000; } else if (demoStep == 2) { setMode(MODE_HOLD, "DEMO_PAUSE_AFTER_LINE"); demoStepEndsAt = millis() + 1000; } else if (demoStep == 3) { setMode(MODE_OBSTACLE, "DEMO_OBSTACLE"); demoStepEndsAt = millis() + 7000; } else if (demoStep == 4) { setMode(MODE_HOLD, "DEMO_PAUSE_AFTER_OBSTACLE"); demoStepEndsAt = millis() + 1000; } else if (demoStep == 5) { setMode(MODE_REMOTE, "DEMO_REMOTE"); startRemoteMove(MOTION_FWD, 90, 500); demoStepEndsAt = millis() + 1200; } else if (demoStep == 6) { startRemoteMove(MOTION_LEFT, 80, 300); demoStepEndsAt = millis() + 900; } else { setMode(MODE_HOLD, "DEMO_DONE"); demoActive = false; Serial.println("event=DEMO_DONE"); beep(3); } } void printTelemetry() { unsigned long now = millis(); if (now - lastTelemetryMs < TELEMETRY_INTERVAL_MS) return; lastTelemetryMs = now; Serial.print("telemetry t="); Serial.print(now); Serial.print(" mode="); Serial.print(modeName(currentMode)); Serial.print(" motion="); Serial.print(motionName(currentMotion)); Serial.print(" dist_cm="); Serial.print(distanceCm, 1); Serial.print(" line="); Serial.print(lineL); Serial.print(","); Serial.print(lineC); Serial.print(","); Serial.print(lineR); Serial.print(" pwm="); Serial.print(leftPwm); Serial.print(","); Serial.println(rightPwm); } void printStatus() { Serial.print("status mode="); Serial.print(modeName(currentMode)); Serial.print(" motion="); Serial.print(motionName(currentMotion)); Serial.print(" demo="); Serial.print(demoActive ? "ON" : "OFF"); Serial.print(" distance_cm="); Serial.print(distanceCm, 1); Serial.print(" line="); Serial.print(lineL); Serial.print(","); Serial.print(lineC); Serial.print(","); Serial.println(lineR); } void handleSerial() { if (!Serial.available()) return; String line = Serial.readStringUntil('\n'); line.trim(); line.toUpperCase(); if (line == "STOP") { demoActive = false; setMode(MODE_HOLD, "USER_STOP"); } else if (line == "STATUS") { printStatus(); } else if (line == "DEMO START") { startDemo(); } else if (line == "MODE HOLD") { demoActive = false; setMode(MODE_HOLD, "MODE_CHANGE"); } else if (line == "MODE LINE") { demoActive = false; setMode(MODE_LINE, "MODE_CHANGE"); } else if (line == "MODE OBSTACLE") { demoActive = false; setMode(MODE_OBSTACLE, "MODE_CHANGE"); } else if (line == "MODE REMOTE") { demoActive = false; setMode(MODE_REMOTE, "MODE_CHANGE"); } else if (tokenAt(line, 0) == "MOVE") { demoActive = false; String direction = tokenAt(line, 1); int speed = tokenAt(line, 2).toInt(); unsigned long durationMs = tokenAt(line, 3).toInt(); if (direction == "FWD") startRemoteMove(MOTION_FWD, speed, durationMs); else if (direction == "BACK") startRemoteMove(MOTION_BACK, speed, durationMs); else if (direction == "LEFT") startRemoteMove(MOTION_LEFT, speed, durationMs); else if (direction == "RIGHT") startRemoteMove(MOTION_RIGHT, speed, durationMs); else { stopRobot("BAD_MOVE"); beep(2); } } else { stopRobot("UNKNOWN_COMMAND"); beep(2); } } 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); pinMode(LINE_LEFT, INPUT); pinMode(LINE_CENTER, INPUT); pinMode(LINE_RIGHT, INPUT); digitalWrite(PIN_STBY, HIGH); setMode(MODE_HOLD, "BOOT"); Serial.println("G1 33 capstone ready"); Serial.println("Commands: DEMO START | MODE LINE | MODE OBSTACLE | MODE REMOTE | MOVE FWD 90 500 | STOP | STATUS"); } void loop() { updateSensors(); handleSerial(); updateDemo(); if (currentMode == MODE_LINE) runLineMode(); else if (currentMode == MODE_OBSTACLE) runObstacleMode(); else if (currentMode == MODE_REMOTE) updateRemoteMotion(); printTelemetry(); }

Capstone Robot Gen 1 đa chế độ

Đăng nhập để học miễn phí