-// July 30th 10:17 Updated

-

-// === 1번 모터 핀 정의 ===

-#define DIR_1 8      // DIR1 (드라이버 갈색 2번, 점퍼선 주황색)

-#define ST_1 9       // START/STOP1 (드라이버 핑크 4번, 점퍼선 초록색)

-#define PWM_1 45     // SPEED_IN1 (드라이버 주황색 6번, 점퍼선 노란색)

-#define ENCA_1 2     // A상 (드라이버 보라색, 점퍼선 파란색)

-#define ENCB_1 3     // B상 (드라이버 회색, 점퍼선 흰색)

-

-// === 2번 모터 핀 정의 ===

-#define DIR_2 22    // DIR2 (드라이버 갈색 16번, 점퍼선 주황색)

-#define ST_2 24     // START/STOP2 (드라이버 핑크 17번, 점퍼선 초록색)

-#define PWM_2 44    // SPEED_IN2 (드라이버 주황색 8번, 점퍼선 노란색)

-#define ENCA_2 18   // A상 (드라이버 보라색, 점퍼선 파란색)

-#define ENCB_2 19   // B상 (드라이버 회색, 점퍼선 흰색)

-

-// === 목표 RPM (부호 포함) ===

-int setRPM1 = 100;

-int setRPM2 = 0;

-

-// === PID 계수 (모터별 개별 설정) ===

-double kp1 = 0.5, ki1 = 3.0, kd1 = 0.085;

-double kp2 = 0.0, ki2 = 0.0, kd2 = 0.0;

-

-// === 모터 상태 변수 ===

-// 모터 1

-volatile long encoderPos1 = 0;

-double rpm1 = 0;

-int calPWM1 = 0;

-double error1 = 0, prevError1 = 0, integral1 = 0, derivative1 = 0;

-

-// 모터 2

-volatile long encoderPos2 = 0;

-double rpm2 = 0;

-int calPWM2 = 0;

-double error2 = 0, prevError2 = 0, integral2 = 0, derivative2 = 0;

-

-// 제어 주기

-const unsigned long interval = 100;    // 단위: ms

-unsigned long prevTime = 0;

-double dt = interval / 1000.0;          // ms → 초(s) 단위로 변경

-

-// === 인터럽트 핸들러 ===

-// 모터 1

-void doEncoderA1() {

-  if (digitalRead(ENCB_1) == LOW) encoderPos1++;

-  else encoderPos1--;

-}

-void doEncoderB1() {

-  if (digitalRead(ENCA_1) == HIGH) encoderPos1++;

-  else encoderPos1--;

-}

-

-// 모터 2

-void doEncoderA2() {

-  if (digitalRead(ENCB_2) == LOW) encoderPos2--;

-  else encoderPos2++;

-}

-void doEncoderB2() {

-  if (digitalRead(ENCA_2) == HIGH) encoderPos2--;

-  else encoderPos2++;

-}

-

-// === RPM 계산 함수 ===

-double calculateRPM(long counts) {

-  return ((double)counts / (4096.0 * 2)) * (60.0 / dt);

-}

-

-// === PID 계산 함수 (모터별 계수 전달) ===

-int computePID(double currentRPM, double &error, double &prevError,

-               double &integral, double &derivative, int targetRPM,

-               double kp, double ki, double kd) {

-  error = targetRPM - currentRPM;

-  integral += (error * dt);

-  derivative = (error - prevError) / dt;

-

-  double output = (kp * error) + (ki * integral) + (kd * derivative);

-  prevError = error;

-  return (int)output;

-}

-

-// === 실시간 입력 (R숫자 L숫자 형식으로 입력, 음수 가능) ===

-void serialSetRPM() {

-  if (Serial.available()) {

-    String input = Serial.readStringUntil('\n');

-    input.trim();

-

-    int rIndex = input.indexOf('R');

-    int lIndex = input.indexOf('L');

-

-    if (rIndex == -1 && lIndex == -1) {

-      Serial.println("입력 오류: 'R' 또는 'L'이 포함되어야 합니다. 예: R100 L-80");

-      return;

-    }

-

-    bool validInput = false;

-

-    // === R 처리 ===

-    if (rIndex != -1) {

-      int end = (lIndex != -1 && lIndex > rIndex) ? lIndex : input.length();

-      String rValStr = input.substring(rIndex + 1, end);

-      rValStr.trim();

-

-      if (rValStr.length() == 0) {

-        Serial.println("오류: R 뒤에 숫자가 없습니다. 예: R100");

-      } else {

-        int rpmVal = rValStr.toInt();

-        if (String(rpmVal) != rValStr) {

-          Serial.print("오류: R에 잘못된 숫자 형식 → "); Serial.println(rValStr);

-        } else if (abs(rpmVal) > 400) {

-          Serial.println("오류: R 값은 -400 ~ 400 사이여야 합니다.");

-        } else {

-          setRPM1 = rpmVal;

-          Serial.print("모터1 (R) 목표 RPM → "); Serial.println(setRPM1);

-          validInput = true;

-        }

-      }

-    }

-

-    // === L 처리 ===

-    if (lIndex != -1) {

-      int end = (rIndex != -1 && rIndex > lIndex) ? rIndex : input.length();

-      String lValStr = input.substring(lIndex + 1, end);

-      lValStr.trim();

-

-      if (lValStr.length() == 0) {

-        Serial.println("오류: L 뒤에 숫자가 없습니다. 예: L-80");

-      } else {

-        int rpmVal = lValStr.toInt();

-        if (String(rpmVal) != lValStr) {

-          Serial.print("오류: L에 잘못된 숫자 형식 → "); Serial.println(lValStr);

-        } else if (abs(rpmVal) > 400) {

-          Serial.println("오류: L 값은 -400 ~ 400 사이여야 합니다.");

-        } else {

-          setRPM2 = rpmVal;

-          Serial.print("모터2 (L) 목표 RPM → "); Serial.println(setRPM2);

-          validInput = true;

-        }

-      }

-    }

-

-    if (!validInput) {

-      Serial.println("R숫자 L숫자 형식으로 입력해주세요. 예: R100 L-80");

-    }

-  }

-}

-

-

-void setup() {

-  Serial.begin(57600);

-

-  // 모터 1 핀 설정

-  pinMode(DIR_1, OUTPUT); pinMode(PWM_1, OUTPUT); pinMode(ST_1, OUTPUT);

-  pinMode(ENCA_1, INPUT_PULLUP); pinMode(ENCB_1, INPUT_PULLUP);

-  attachInterrupt(digitalPinToInterrupt(ENCA_1), doEncoderA1, RISING);

-  attachInterrupt(digitalPinToInterrupt(ENCB_1), doEncoderB1, RISING);

-

-  // 모터 2 핀 설정

-  pinMode(DIR_2, OUTPUT); pinMode(PWM_2, OUTPUT); pinMode(ST_2, OUTPUT);

-  pinMode(ENCA_2, INPUT_PULLUP); pinMode(ENCB_2, INPUT_PULLUP);

-  attachInterrupt(digitalPinToInterrupt(ENCA_2), doEncoderA2, RISING);

-  attachInterrupt(digitalPinToInterrupt(ENCB_2), doEncoderB2, RISING);

-

-  // 브레이크 해제

-  digitalWrite(ST_1, LOW);

-  digitalWrite(ST_2, LOW);

-

-  prevTime = millis();

-

-  Serial.println("R±RPM | L±RPM 입력");

-}

-

-void loop() {

-  unsigned long now = millis();

-

-  if (now - prevTime >= interval) {

-    prevTime += interval;

-

-    // 모터 1

-    noInterrupts(); long currentPos1 = encoderPos1; encoderPos1 = 0; interrupts();

-    rpm1 = calculateRPM(currentPos1);

-    calPWM1 = computePID(rpm1, error1, prevError1, integral1, derivative1, abs(setRPM1), kp1, ki1, kd1);

-    digitalWrite(DIR_1, setRPM1 >= 0 ? HIGH : LOW);

-    calPWM1 = constrain(abs(calPWM1), 0, 255);

-    analogWrite(PWM_1, calPWM1);

-

-    // 모터 2

-    noInterrupts(); long currentPos2 = encoderPos2; encoderPos2 = 0; interrupts();

-    rpm2 = calculateRPM(currentPos2);

-    calPWM2 = computePID(rpm2, error2, prevError2, integral2, derivative2, abs(setRPM2), kp2, ki2, kd2);

-    digitalWrite(DIR_2, setRPM2 >= 0 ? HIGH : LOW);

-    calPWM2 = constrain(abs(calPWM2), 0, 255);

-    analogWrite(PWM_2, calPWM2);

-

-    // 시리얼 출력

-    // Serial.print("R:"); Serial.println(setRPM1);

-    // Serial.print("RPM:"); Serial.println(rpm1, 2);

-    // Serial.print("PWM:"); Serial.println(calPWM1);

-    // Serial.println(" ");

-    // Serial.print("L:"); Serial.println(setRPM2);

-    // Serial.print("RPM:"); Serial.println(rpm2, 2);

-    // Serial.print("PWM:"); Serial.println(calPWM2);

-    // Serial.println("--------------------");

-

-    // Serial Plotter용 출력 (탭으로 구분)

-    Serial.print(setRPM1); Serial.print('\t');

-    Serial.println(rpm1, 2);

-    // Serial.print(setRPM2); Serial.print('\t');

-    // Serial.println(rpm2, 2);

-  }

-

-  serialSetRPM();

-}

-//test motor with arduino, check hallsensor

-

-// test run motor with arduino

-

-const int PWM = 10;    // MD200T 6번 (SPEED_IN2) 노랑

-const int DIR = 8;    // MD200T 2번 (DIR2) 주황

-const int ST  = 9;    // MD200T 4번 (START/STOP2) 초록

-

-void setup() {

-  Serial.begin(9600);

-

-  pinMode(PWM, OUTPUT);

-  pinMode(DIR, OUTPUT);       // 회전 방향 (시계 방향 = HIGH)

-  pinMode(ST, OUTPUT);        // 브레이크 (해제 = LOW)  

-

-  digitalWrite(ST, HIGH);

-  delay(2000);                // 전원 안정화

-}

-

-void loop() {

-

-  digitalWrite(ST, LOW);      // 브레이크 해제

-  digitalWrite(DIR, LOW);     // 반시계 회전

-  analogWrite(PWM, 0);

-  // delay(10000);

-  // digitalWrite(ST, HIGH);     // 멈춤

-  // delay(4000);

-

-  // digitalWrite(ST, LOW);

-  // digitalWrite(DIR, HIGH);    // 시계 회전

-  // analogWrite(PWM, 125);

-  // delay(10000);

-  // digitalWrite(ST, HIGH);

-  // delay(4000);

-}

-

-

-// check hallsensor

-

-

-// 홀센서 핀 번호

-const int Hu = 4;

-const int Hv = 3;

-const int Hw = 2;

-

-const int PWM = 10;    // MD200T 6번 (SPEED_IN2) 노랑

-const int DIR = 8;    // MD200T 2번 (DIR2) 주황

-const int ST  = 9;    // MD200T 4번 (START/STOP2) 초록

-

-void setup() {

-  Serial.begin(9600);

-  

-  pinMode(Hu, INPUT);

-  pinMode(Hv, INPUT);

-  pinMode(Hw, INPUT);

-

-  pinMode(PWM, OUTPUT);

-  pinMode(DIR, OUTPUT);       // 회전 방향 (시계 방향 = HIGH)

-  pinMode(ST, OUTPUT);        

-  digitalWrite(ST, HIGH); // 브레이크 (해제 = LOW)  

-  delay(2000);            // 전원 안정화

-}

-

-//모터를 직접 손으로 돌리면서 체크

-void loop() {

-  int hu = digitalRead(Hu);

-  int hv = digitalRead(Hv);

-  int hw = digitalRead(Hw);

-

-  Serial.print("Hu: "); Serial.print(hu);

-  Serial.print(" | Hv: "); Serial.print(hv);

-  Serial.print(" | Hw: "); Serial.println(hw);

-

-  delay(200);  // 0.2초 간격 출력

-}(파일 끝에 줄바꿈 문자 없음)