NHK学生ロボコン2026ロボット紹介

/*—————————–

 R1制御 ESP32-WROOM              //v3.3.4

——————————–*/

#include “DJIMotorCtrlESP.hpp”   //v2.0.5

#include “HXC_TWAI.hpp”          //v1.0.5

#include <ESP32Servo.h>          //v3.0.9

#include <IcsBaseClass.h>        //v3

#include <IcsHardSerialClass.h>  //v3

#include <Wire.h>                //標準

#include <MPU6050_tockn.h>       //v1.5.2

#include <PS4Controller.h>       //1.1.0

// ===== MPU6050 角度計算タスク（Core 0で永続ループ）=====

MPU6050 mpuA(Wire), mpuB(Wire);

volatile float stableYaw = 0;  // マルチタスクで使う変数はvolatile

float prevYawA = 0, prevYawB = 0;

const float THRESHOLD_NOISE = 5.0;

TaskHandle_t IMUTaskHandle;  // タスクハンドル

// ===== 角度計算タスク（Core 0で永続ループ） =====

void imuUpdateTask(void *pvParameters) {

  while (true) {

    mpuA.update(), mpuB.update();

    float currentA = mpuA.getGyroAngleZ(), currentB = mpuB.getGyroAngleZ();  // 90度倒し

    float diffA = abs(currentA – prevYawA), diffB = abs(currentB – prevYawB);

    if (diffA > THRESHOLD_NOISE && diffB < THRESHOLD_NOISE) stableYaw += (currentB – prevYawB);  // 冗長化ロジック

    else if (diffB > THRESHOLD_NOISE && diffA < THRESHOLD_NOISE) stableYaw += (currentA – prevYawA);

    else stableYaw += ((currentA – prevYawA) + (currentB – prevYawB)) / 2.0;

    prevYawA = currentA, prevYawB = currentB;

    vTaskDelay(10 / portTICK_PERIOD_MS);  // 10ms間隔で更新

  }

}

Servo A1;                       //ノーマルサーボ宣言

Servo M5;                       //通信用PWM宣言

HXC_TWAI CAN_BUS(17, 16);       //CAN

M3508_P19 MOTOR1(&CAN_BUS, 1);  //ロボマス

M3508_P19 MOTOR2(&CAN_BUS, 2);

M3508_P19 MOTOR3(&CAN_BUS, 3);

M3508_P19 MOTOR4(&CAN_BUS, 4);

M3508_P19 MOTOR5(&CAN_BUS, 5);  //Z軸

M2006_P36 MOTOR6(&CAN_BUS, 6);  //Y軸

HardwareSerial SerialICS(1);    //近藤サーボ

const int ICS_PIN = 12;

IcsHardSerialClass krs(&Serial1, 23, 115200, 8);              // 7応答時間 timeout

const int A1Pin = 14;                                         //武器回収根本

const int M5Pin = 5;                                          //M5通信

const int MOSFET = 19;                                        //空気圧

const int startSW = 13;                                       //スタートボタン

const int dis = 35, BRdis = 27, BLdis = 26;                   //距離センサー（前方）（右後方）（左後方）

const int rLED = 33, gLED = 25, bLED = 32;                    //フルカラーLED

const int MOTOR_SIGN[4] = { -1, +1, -1, +1 };                 //モーター取り付け符号

double currentX = 0, currentY = 0, targetX = 0, targetY = 0;  //座標,目標

long lastEnc[4] = { 0, 0, 0, 0 };                             //エンコーダ

const double PULSE_TO_MM = 0.01;

const double SERVO_MIN_DEG = -137.0, SERVO_MAX_DEG = 43.0;  //サーボ可動域(実角度)

const int SERVO_MIN = 3500, SERVO_MAX = 11500;              //サーボ可動域（指令値）

double driveTheta = 0;                                      //状態保持

int hou = 0;

const int A1angle = 75;     //初期値

const int maxSpeed = 400;   //操縦時最高速

const int rev = 8192 * 19;  //M3508が1回転

const float ACCEL = 8.0;

long Z_HOME = 0, Z_UP = rev * 11.1, Z_Little = rev / 3;  // Z軸

bool zUpState = false;                                   // 今上がっているか

long motor5_target = 0;

long motor6_target = 0;                                                      // MOTOR6目標位置

bool clawOpen = 1, prevSquare = false, prevRight = false, prevLeft = false;  // 前回のボタン状態

int field = 0;                                                               //青コート１、赤コート２

int d;

int z = 3.25;                    //ポール回収y軸修正

bool isWeaponReleased = false;  // 武器を離した状態かどうかのフラグ

bool prevShareState = false;    // 前回のボタン状態（連打防止用）

bool lastUp = false, lastDown = false, lastRight = false, lastLeft = false;

const int SERVO5_CLOSE_VAL = 5300;  //  サーボ5のブック保持の値(値を小さくすると強く保持)———————————————– 元5500

const int SERVO5_OPEN_VAL = 8700;   //　サーボ5のブック離す値

const int SERVO6_CLOSE_VAL = 7500;  //  サーボ6のポール固定する値(値を小さくすると強く保持)

const int SERVO6_OPEN_VAL = 8000;   //　サーボ6のポール固定解除の値

const int SERVO7_CLOSE_VAL = 7500;  //  サーボ7のポール回収の値(値を小さくすると強く保持)

const int SERVO7_OPEN_VAL = 8100;   //　サーボ7のポールを離す値

const int SERVO8_AVOID_VAL = 4600;  //  サーボ8のシリンダー避けるときの値(値を小さくすると押し付けを弱める)

const int SERVO8_SET_VAL = 7400;    //　サーボ8のいシリンダーセットの値

void setup() {

  set();            //各種設定

  StartPosition();  //スタートボタン押されるまで待機

  weaponCatch();    //ポール回収

  PS4doujyou();     //槍組み立て(半自動)

  A1.write(52);     //干渉回避

}

void loop() {

  if (PS4.isConnected()) {

    ;                     //PS4コントローラー接続せよ

    PS4main();            //手動操作

  } else {                //接続切れたらライブラリ内のコードが作動する可能性

    MOTOR1.set_speed(0);  // モーター停止

    MOTOR2.set_speed(0);

    MOTOR3.set_speed(0);

    MOTOR4.set_speed(0);

    digitalWrite(2, millis() % 500 < 100);  // LED点滅

    digitalWrite(rLED, HIGH);

    digitalWrite(gLED, 0);

    digitalWrite(bLED, 0);

  }

  if (PS4.PSButton()) {  //操縦から再開

    StartPosition();

    A1.write(52);  //干渉回避

  }

}

void set() {

  Serial.begin(9600);

  PS4.begin(“ec:c9:ff:e3:23:1e”);  //ps4コントローラーMACアドレス

  A1.attach(A1Pin);

  M5.attach(M5Pin);

  M5.write(5);

  pinMode(startSW, INPUT_PULLUP);

  pinMode(MOSFET, OUTPUT);

  pinMode(2, OUTPUT);

  pinMode(rLED, OUTPUT);

  pinMode(gLED, OUTPUT);

  pinMode(bLED, OUTPUT);

  pinMode(dis, INPUT);

  pinMode(BRdis, INPUT);

  pinMode(BLdis, INPUT);

  digitalWrite(rLED, LOW);

  digitalWrite(gLED, LOW);

  digitalWrite(bLED, LOW);

  Wire.begin(21, 22);  //ジャイロピン

  delay(250);          //これがないとジャイロが不安定になる?

  mpuA.begin(), mpuB.begin();

  Serial.println(“Fast Calibrating (Mode)…”);

  const int samples = 500;

  // 最頻値カウント用の配列（バケット）

  // 例：-250〜249（分解能0.1なら -25.0 〜 +24.9°/s）をカバー

  #define BUCKET_OFFSET 250

  #define BUCKET_SIZE 500

  int bucketAZ[BUCKET_SIZE] = { 0 };

  int bucketBZ[BUCKET_SIZE] = { 0 };

  for (int i = 0; i < samples; i++) {

    digitalWrite(2, millis() % 250 < 50);

    digitalWrite(rLED, millis() % 250 < 50);

    mpuA.update(), mpuB.update();

    // 小数点第1位で丸めるために10倍し、四捨五入（intキャスト）

    int valAZ = (int)round(mpuA.getGyroZ() * 10.0) + BUCKET_OFFSET;

    int valBZ = (int)round(mpuB.getGyroZ() * 10.0) + BUCKET_OFFSET;

    // 配列の範囲内に収まるかチェックしてカウント

    if (valAZ >= 0 && valAZ < BUCKET_SIZE) bucketAZ[valAZ]++;

    if (valBZ >= 0 && valBZ < BUCKET_SIZE) bucketBZ[valBZ]++;

    delay(1);

  }

  // 最頻値（一番多くカウントされた値）を探す

  int maxCountA = 0, modeIdxA = BUCKET_OFFSET;

  int maxCountB = 0, modeIdxB = BUCKET_OFFSET;

  for (int i = 0; i < BUCKET_SIZE; i++) {

    if (bucketAZ[i] > maxCountA) {

      maxCountA = bucketAZ[i];

      modeIdxA = i;

    }

    if (bucketBZ[i] > maxCountB) {

      maxCountB = bucketBZ[i];

      modeIdxB = i;

    }

  }

  // インデックスを実際のfloat値に戻す

  float modeAZ = (float)(modeIdxA – BUCKET_OFFSET) / 10.0;

  float modeBZ = (float)(modeIdxB – BUCKET_OFFSET) / 10.0;

  mpuA.setGyroOffsets(0, 0, modeAZ), mpuB.setGyroOffsets(0, 0, modeBZ);

  Serial.println(“Calibration Done.”);

  xTaskCreatePinnedToCore(imuUpdateTask, “IMUTask”, 4096, NULL, 1, &IMUTaskHandle, 0);

  CAN_BUS.setup();

  MOTOR1.set_speed_pid(1.0, 0.0, 0.1, 0.0, 10000);

  MOTOR2.set_speed_pid(1.0, 0.0, 0.1, 0.0, 10000);

  MOTOR3.set_speed_pid(1.0, 0.0, 0.1, 0.0, 10000);

  MOTOR4.set_speed_pid(1.0, 0.0, 0.1, 0.0, 10000);

  MOTOR5.set_location_pid(2.0, 0.05, 0.5, 0.0, 13000);

  MOTOR6.set_location_pid(0.8, 0.01, 0.1, 0.01, 13000);

  MOTOR1.setup();

  MOTOR2.setup();

  MOTOR3.setup();

  MOTOR4.setup();

  MOTOR5.setup();

  MOTOR6.setup();  //-7.6 伸ばす

  Z_HOME = MOTOR5.get_location();

  MOTOR6.set_location(MOTOR6.get_location() + 8000);  //　y軸余裕を少し持つ

  SerialICS.begin(115200, SERIAL_8N1, -1, 12);

  krs.begin();

  digitalWrite(rLED, LOW);

  stableYaw = 0;

  delay(100);

}

void PS4doujyou() {

  int stage1 = 320;  // 第一段階

  int stage2 = 370;  //第二段階

  int stageB = 495;

  if (field == 1 || field == 2) {

    int basespeed = 40;  //自動前進の速度

    int taegetYaw;       //自動補正目標角

    if (field == 1) {

      taegetYaw = -90;

    } else {

      taegetYaw = 90;

    }

    int def = 0, p = 10;  //角度補正　p値は補正強さ

    int Lspeed, Rspeed;   //角度補正基準値

    int count = 0, count2 = 0, count3 = 0, count4 = 0;

    if (!PS4.isConnected()) return;  //PS4コントローラー接続せよ

    while (1) {

      digitalWrite(2, millis() % 1500 < 100);                //一定周期での点灯

      analogWrite(rLED, (millis() % 1500 < 500) ? 255 : 0);  //オレンジ

      analogWrite(gLED, (millis() % 1500 < 500) ? 200 : 0);  //緑

      analogWrite(bLED, 0);//消灯

      krs.setPos(8, SERVO8_SET_VAL);  //シリンダーセット

      if (PS4.Square()) break;

      //if ((count = (analogRead(dis) > stageB ? count + 1 : 0)) >= 5) break;  //組み立て距離 475

      if ((count2 = (analogRead(dis) > stage1 ? count2 + 1 : 0)) >= 5) basespeed = 20;  //減速第一段階

      if ((count3 = (analogRead(dis) > stage2 ? count3 + 1 : 0)) >= 5) basespeed = 10;  //減速第二段階

      // — やり直し動作 —　　

      if (PS4.Down()) {

        basespeed = 40;       //下がったので再度最高速に

        krs.setPos(1, 9590);  // 左前

        krs.setPos(2, 9590);  // 右前

        krs.setPos(3, 9590);  // 左後

        krs.setPos(4, 9590);  // 右後

        for (int i = 0; i < 100; i++) {

          MOTOR1.set_speed(i);

          MOTOR2.set_speed(-i);

          MOTOR3.set_speed(i);

          MOTOR4.set_speed(-i);

          delay(1);

        }

        for (int i = 100; i > 0; i–) {

          MOTOR1.set_speed(i);

          MOTOR2.set_speed(-i);

          MOTOR3.set_speed(i);

          MOTOR4.set_speed(-i);

          delay(1);

        }

      }

      // — 縦合わせ手動操作 —

      int step_z = rev * 0.07;                                              // 1回の移動量（調整可）

      if (PS4.R1() && motor5_target < rev * 11.1) motor5_target += step_z;  // R1 → 減少

      else if (PS4.L1() && motor5_target > 0) motor5_target -= step_z;      // L1 → 増加

      MOTOR5.set_location(motor5_target);

      // — 横合わせ手動操作 —

      if (PS4.LStickX() > 20) d = map(PS4.LStickX(), 20, 128, 0, -1650);    // -1900

      else if (PS4.LStickX() < -20) d = map(PS4.LStickX(), -20, -128, 0, 1650); // 1900

      else d = 0;

      krs.setPos(1, 9590 + d);

      krs.setPos(2, 9590 + d);

      krs.setPos(3, 9590 + d);

      krs.setPos(4, 9590 + d);

      // — Yaw角補正動作 —

      def = (stableYaw – taegetYaw) * p;  //(現在角-目標値)×補正強さ

      Lspeed = basespeed + def;           //基準速度+補正値

      Rspeed = basespeed – def;

      MOTOR1.set_speed(-Lspeed);

      MOTOR2.set_speed(Rspeed);

      MOTOR3.set_speed(-Lspeed);

      MOTOR4.set_speed(Rspeed);

      delay(50);

    }

    digitalWrite(rLED, HIGH);  //紫

    digitalWrite(gLED, LOW);

    digitalWrite(bLED, HIGH);

    MOTOR1.set_speed(0);  //停止

    MOTOR2.set_speed(0);

    MOTOR3.set_speed(0);

    MOTOR4.set_speed(0);

    MOTOR5.set_location(rev * 0.23);  //上げる

    //krs.setPos(6, SERVO6_OPEN_VAL);   //弱める

    delay(100);

    digitalWrite(MOSFET, HIGH);       //組み立て

    krs.setPos(6, SERVO6_CLOSE_VAL);  //閉じる

    delay(250);

    digitalWrite(MOSFET, LOW);

    delay(250);

    //krs.setPos(6, SERVO6_OPEN_VAL);  //弱める

    /*delay(100);*/

    digitalWrite(MOSFET, HIGH);       //組み立て

    krs.setPos(6, SERVO6_CLOSE_VAL);  //閉じる

    delay(250);

    digitalWrite(MOSFET, LOW);

    delay(250);

    krs.setPos(8, SERVO8_AVOID_VAL);  //避ける

    MOTOR5.set_location(rev * 2.2);   //上げる

    delay(350);

    MOTOR1.set_speed(250);  //後進

    MOTOR2.set_speed(-250);

    MOTOR3.set_speed(250);

    MOTOR4.set_speed(-250);

    delay(300);

    MOTOR1.set_speed(0);  //停止

    MOTOR2.set_speed(0);

    MOTOR3.set_speed(0);

    MOTOR4.set_speed(0);

    MOTOR5.set_location(Z_HOME);

    delay(800);  //Z軸戻る時間

  }

}

//—–メイン操作関数—–
int sub, ave, mspeed;
bool right_old = false;
bool left_old = false;
void PS4main() {
krs.setPos(6, SERVO6_CLOSE_VAL); //閉じる
analogWrite(rLED, (millis() % 1500 < 500) ? 255 : 0); //黄色 analogWrite(gLED, (millis() % 1500 < 500) ? 255 : 0); analogWrite(bLED, 0); digitalWrite(2, millis() % 1500 < 100); krs.setPos(8, SERVO8_AVOID_VAL); //よける if (!PS4.isConnected()) return; //—–足回り—– int rx = PS4.RStickX(), ry = -PS4.RStickY(), lx = -PS4.LStickX(); if (abs(lx) > 20) rotateByStick(lx); // 左スティック → 旋回
else if (abs(ry) > 20 || abs(rx) > 20) driveByStick(-ry, -rx); // 右スティック → 移動
else if (PS4.L2Value() > 20) { //横半自動送り
mspeed = map(PS4.L2Value(), 20, 255, 10, 400);
krs.setPos(1, 5590);
krs.setPos(2, 5590);
krs.setPos(3, 5590);
krs.setPos(4, 5590);
MOTOR1.set_speed(mspeed);
MOTOR2.set_speed(-mspeed); //3500-11150
MOTOR3.set_speed(mspeed);
MOTOR4.set_speed(-mspeed);
} else if (PS4.R2Value() > 35) { //横半自動送り
mspeed = map(PS4.R2Value(), 35, 255, 10, 400);
krs.setPos(1, 5590);
krs.setPos(2, 5590);
krs.setPos(3, 5590);
krs.setPos(4, 5590);
MOTOR1.set_speed(-mspeed);
MOTOR2.set_speed(mspeed);
MOTOR3.set_speed(-mspeed);
MOTOR4.set_speed(mspeed);
} else {
MOTOR1.set_speed(0);
MOTOR2.set_speed(0);
MOTOR3.set_speed(0);
MOTOR4.set_speed(0);
}
// ===== 十字キー右 =====
bool right_now = PS4.Right();
if (right_now && !right_old) {
krs.setPos(7, SERVO7_OPEN_VAL); // ピッキングハンド開く
A1.write(A1angle); // アーム上げる
delay(500);
krs.setPos(7, SERVO7_CLOSE_VAL); // ピッキングハンド保持
delay(300);
}
right_old = right_now;
// ===== 十字キー左 =====
bool left_now = PS4.Left();
if (left_now && !left_old) {
krs.setPos(7, SERVO7_OPEN_VAL); // ピッキングハンド開く
delay(500);
A1.write(A1angle – 23); // アーム下げる
delay(300);
}
left_old = left_now;
//—–Z軸(モーター5)—–
if (PS4.R1()) MOTOR5.set_location(Z_UP);
else if (PS4.L1()) MOTOR5.set_location(Z_HOME);
else MOTOR5.set_location(MOTOR5.get_location());
//—–ボタン操作—–
if (PS4.Triangle()) getBox(400); // △ボタン 400mm
else if (PS4.Circle()) getBox(200); // 〇ボタン 200mm
else if (PS4.Cross()) getBox(0); // ✕ボタン 0mm
if (PS4.Square()) { // □ボタン　ハンド
clawOpen = !clawOpen;
delay(300); // 連続切替防止
}
if (clawOpen == 1) krs.setPos(5, SERVO5_OPEN_VAL); //開く
else krs.setPos(5, SERVO5_CLOSE_VAL); //閉じる 元3800
//—–Y軸(モーター6)—–
int stepy = rev * 0.3; // 1回の移動量（調整可）
if (PS4.Up() && motor6_target > -rev * 7.6) motor6_target -= stepy; // → 減少
else if (PS4.Down() && motor6_target < 0) motor6_target += stepy; // L1 → 増加 MOTOR6.set_location(motor6_target); delay(10); if (PS4.Touchpad()) { //武器離す krs.setPos(7, SERVO7_OPEN_VAL); //ピッキングハンド開く A1.write(A1angle); //アーム上げる delay(500); krs.setPos(7, SERVO7_CLOSE_VAL); //ピッキングハンド保持 delay(300); krs.setPos(6, SERVO6_OPEN_VAL); //筒形ハンド開く delay(300); for (int i = A1angle; i <= A1angle + 70; i += 10) { A1.write(i); //アーム下げる delay(200); } krs.setPos(7, SERVO7_OPEN_VAL); //ピッキングハンド開く delay(500); krs.setPos(7, SERVO7_CLOSE_VAL); //ピッキングハンド保持 A1.write(A1angle); //アーム少し上げる delay(500); A1.write(A1angle – 23); //アーム上げる } // シェアボタンが「押された瞬間」だけ判定 if (PS4.Share() && !prevShareState) { if (!isWeaponReleased) { // — 1回目：武器を離す動作 — krs.setPos(7, SERVO7_OPEN_VAL); // ピッキングハンド開く A1.write(A1angle); // アーム上げる delay(500); krs.setPos(7, SERVO7_CLOSE_VAL); // ピッキングハンド保持 delay(300); krs.setPos(6, SERVO6_OPEN_VAL); // 筒形ハンド開く delay(300); A1.write(A1angle + 45); // アーム下げる delay(300); krs.setPos(6, SERVO6_OPEN_VAL); // 筒形ハンド開く delay(300); isWeaponReleased = true; // 状態を「離した」に更新 } else { // — 2回目：アームを戻して準備する動作 — krs.setPos(6, SERVO6_OPEN_VAL); // 筒形ハンド開く delay(300); A1.write(A1angle); // アーム上げる delay(2000); //バウンド収まり待ち krs.setPos(6, SERVO6_CLOSE_VAL); // 筒形ハンド閉じる delay(300); krs.setPos(7, SERVO7_OPEN_VAL); // ピッキングハンド開く delay(300); A1.write(A1angle – 23); // アーム上げる delay(300); // isWeaponReleased = false; // 状態を「戻した」にリセット } } prevShareState = PS4.Share(); // ボタン状態を保存 } //—–直進関数—– void driveByStick(int rx, int ry) { double theta = atan2(ry, rx) * 180.0 / PI; //方向計算 int hou = 0; if (theta < SERVO_MIN_DEG || theta > SERVO_MAX_DEG) theta += (theta > 0) ? -180.0 : 180.0, hou = 1;
int Kangle = map((int)theta, SERVO_MIN_DEG, SERVO_MAX_DEG, SERVO_MIN, SERVO_MAX); //サーボ角変換
Kangle = constrain(Kangle, SERVO_MIN, SERVO_MAX);
for (int id = 1; id <= 4; id++) krs.setPos(id, Kangle);
double power = sqrt(rx * rx + ry * ry); // 速度計算
double speed = map(power, 20, 128, 0, 500);
speed = constrain(speed, 0, maxSpeed);
int dir = (hou == 0) ? 1 : -1;
MOTOR1.set_speed(speed * dir * MOTOR_SIGN[0]);
MOTOR2.set_speed(speed * dir * MOTOR_SIGN[1]);
MOTOR3.set_speed(speed * dir * MOTOR_SIGN[2]);
MOTOR4.set_speed(speed * dir * MOTOR_SIGN[3]);
}
//—–旋回関数—–
void rotateByStick(int lx) { //旋回
krs.setPos(1, 9590 – 2000); // 左前 サーボをハの字にする
krs.setPos(2, 3500 + 20); // 右前
krs.setPos(3, 3500 + 20); // 左後
krs.setPos(4, 9590 – 2000); // 右後
int turnSpeed = map(abs(lx), 20, 128, 0, 400); //スティックから回転速度
if (lx < 0) turnSpeed = -turnSpeed;
MOTOR1.set_speed((-turnSpeed) * MOTOR_SIGN[0]);
MOTOR2.set_speed((-turnSpeed) * MOTOR_SIGN[1]);
MOTOR3.set_speed((turnSpeed)MOTOR_SIGN[2]); MOTOR4.set_speed((turnSpeed)MOTOR_SIGN[3]);
}

int pickupPoint = 0;

void Select() {

  if (pickupPoint == 0) {             //初期設定(左から２番目)

    if (field == 1) pickupPoint = 2;  //青コート

    if (field == 2) pickupPoint = 3;  //赤コート

  }

  bool nowRight = PS4.Right();

  bool nowLeft = PS4.Left();

  // — 右移動 —

  if (nowRight && !lastRight) {

    if (pickupPoint % 4 != 0) pickupPoint++;  // ４の倍数でなければ右にいける

  }

  // — 左移動 —

  if (nowLeft && !lastLeft) {

    if (pickupPoint % 4 != 1) pickupPoint–;  // ４で割って1余る数でなければ左にいける

  }

  lastRight = nowRight;  //前回の状態を代入

  lastLeft = nowLeft;

  if (field == 1) M5.write((pickupPoint + 4) * 10);       //青コートPWM送信(5,6,7,8)

  else if (field == 2) M5.write((pickupPoint + 8) * 10);  //赤コートPWM送信(9,10,11,12)

  else {

    pickupPoint = 0;

    M5.write(4 * 10);  //無選択PWM送信(4)

  }

}

void StartPosition() {

  while (digitalRead(startSW) == 1 && PS4.Square() == 0) {

    Select();

    krs.setPos(1, 9590 – 4000);

    krs.setPos(2, 9590 – 4000);

    krs.setPos(3, 9590 – 4000);

    krs.setPos(4, 9590 – 4000);

    krs.setPos(5, SERVO5_OPEN_VAL);  //開く

    krs.setPos(6, SERVO6_OPEN_VAL);  //開く

    krs.setPos(7, SERVO7_OPEN_VAL);  //開く

    krs.setPos(8, SERVO8_AVOID_VAL);  //よける

    A1.write(A1angle);

    digitalWrite(MOSFET, LOW);

    digitalWrite(2, LOW);

    Serial.printf(” ポール回収位置 = %d”, pickupPoint);

    Serial.printf(” 方向 = %.2f”, stableYaw);

    Serial.printf(” 前方 = %d”, analogRead(dis));

    Serial.printf(” 右後 = %d”, analogRead(BRdis));

    Serial.printf(” 左後 = %d\n”, analogRead(BLdis));

    digitalWrite(gLED, millis() % 1500 < 1000);

    if (PS4.Touchpad()) field = 0; //手動操作

    if (PS4.Share()) field = 1;  //青コート

    if (field == 1) digitalWrite(bLED, millis() % 1500 >= 1000);

    else digitalWrite(bLED, LOW);

    if (PS4.Options()) field = 2;  //赤コート

    if (field == 2) digitalWrite(rLED, millis() % 1500 >= 1000);

    else digitalWrite(rLED, LOW);

  }

  digitalWrite(2, HIGH);

}

/* ============

箱回収

============ */

void getBox(int high) {

  digitalWrite(rLED, HIGH);  //紫

  digitalWrite(gLED, LOW);

  digitalWrite(bLED, HIGH);

  A1.write(A1angle – 23);          //アーム少し下げる

  krs.setPos(5, SERVO5_OPEN_VAL);  //開く

  switch (high) {

    case 0:

      MOTOR5.set_location(rev * 0);  //上げる

      delay(500);

      MOTOR6.set_location(rev * -6.4);  //伸ばす

      delay(500);

      MOTOR6.set_location_pid(0.8, 0.01, 0.1, 0.01, 5000);  //速度変換

      delay(10);

      MOTOR6.set_location(rev * -7.4);  //伸ばす

      delay(500);

      MOTOR6.set_location_pid(0.8, 0.01, 0.1, 0.01, 13000);  //速度変換

      delay(10);

      krs.setPos(5, SERVO5_CLOSE_VAL);  //つかむ

      delay(500);

      MOTOR5.set_location(rev * 9.90);  //+1上げる

      delay(500);

      break;

    case 200:

      MOTOR5.set_location(rev * 4.56);  //rev * 4.56

      delay(2000);

      MOTOR6.set_location(rev * -6.4);  //伸ばす

      delay(500);

      MOTOR6.set_location_pid(0.8, 0.01, 0.1, 0.01, 5000);  //速度変換

      delay(10);

      MOTOR6.set_location(rev * -7.4);  //伸ばす

      delay(500);

      MOTOR6.set_location_pid(0.8, 0.01, 0.1, 0.01, 13000);  //速度変換

      delay(10);

      krs.setPos(5, SERVO5_CLOSE_VAL);  //つかむ

      delay(500);

      MOTOR5.set_location(rev * 9.90);  //+1上げる

      delay(500);

      break;

    case 400:

      MOTOR5.set_location(rev * 8.80);  //rev * 8.80

      delay(2000);

      MOTOR6.set_location(rev * -6.4);  //伸ばす

      delay(500);

      MOTOR6.set_location_pid(0.8, 0.01, 0.1, 0.01, 5000);  //速度変換

      delay(10);

      MOTOR6.set_location(rev * -7.4);  //伸ばす

      delay(500);

      MOTOR6.set_location_pid(0.8, 0.01, 0.1, 0.01, 13000);  //速度変換

      delay(10);

      krs.setPos(5, SERVO5_CLOSE_VAL);  //つかむ

      delay(500);

      MOTOR5.set_location(rev * 9.90);  //+1上げる

      delay(500);

      break;

  }

  MOTOR6.set_location(0);  //縮む

  delay(500);

  zUpState = true;

  digitalWrite(rLED, LOW);

  digitalWrite(gLED, LOW);

  digitalWrite(bLED, LOW);

  clawOpen = false;

}

/* =====================================================

オドメトリ（進行方向でXY積分）

===================================================== */

void updateOdometry() {

  long enc[4] = {

    MOTOR1.get_location(),

    MOTOR2.get_location(),

    MOTOR3.get_location(),

    MOTOR4.get_location()

  };

  double sum = 0;

  for (int i = 0; i < 4; i++) {

    long diff = enc[i] – lastEnc[i];

    lastEnc[i] = enc[i];

    sum += diff * MOTOR_SIGN[i];

  }

  double d = (sum / 4.0) * PULSE_TO_MM;  // ここで物理単位に変換している場合、distも物理単位

  double rad = driveTheta * PI / 180.0;

  currentX += d * cos(rad);

  currentY += d * sin(rad);

}

/* =====================================================

目標座標へ移動（ステアリング待機ロジック実装版）

サーボ速度: 125RPM (750 deg/sec) を基準に計算

===================================================== */

void moveTo(double x, double y) {

  x *= 51.5, y *= 51.5;  // 座標系のスケーリング

  targetX = x, targetY = y;

  static double currentServoTheta = 0;  // 最後に「待機」を判断した基準角度

  static double v = 0;

  static unsigned long lastMicros = 0;

  static unsigned long steeringWaitUntil = 0;

  bool arrived = false;

  // — パラメータ設定 —

  const double V_MAX = 500.0;

  const double A_MAX = 250.0;

  const double STOP_DIST = 100.0;

  const double STOP_V = 20.0;

  const double SERVO_DEG_PER_SEC = 750.0;

  const unsigned long MARGIN_MS = 25;

  const double THETA_THRESHOLD = 5.0;  // 何度以上の変化で「一旦止まって待つ」か

  v = 0;

  lastMicros = micros();

  while (1) {

    /* ===== 1. 時間更新とオドメトリ ===== */

    unsigned long now = micros();

    double dt = (now – lastMicros) / 1000000.0;

    lastMicros = now;

    if (dt <= 0) dt = 0.001;

    updateOdometry();

    /* ===== 2. 目標計算 ===== */

    double dx = targetX – currentX;

    double dy = targetY – currentY;

    double dist = sqrt(dx * dx + dy * dy);

    // 停止判定

    if (dist < STOP_DIST && v < STOP_V) arrived = true;

    if (arrived) {

      MOTOR1.set_speed(0);

      MOTOR2.set_speed(0);

      MOTOR3.set_speed(0);

      MOTOR4.set_speed(0);

      currentX = targetX;

      currentY = targetY;

      break;

    }

    /* ===== 3. ステアリング角度決定 (スイッチバック対応) ===== */

    double theta = atan2(dy, dx) * 180.0 / PI;

    int nextHou = 0;

    if (theta < SERVO_MIN_DEG || theta > SERVO_MAX_DEG) {

      theta += (theta > 0) ? -180.0 : 180.0;

      nextHou = 1;

    }

    /* ===== 4. サーボ指令（常に送信）と待機判定 ===== */

    // A. サーボ指令の作成と送信（毎ループ実行）

    int Kangle = map((int)theta, SERVO_MIN_DEG, SERVO_MAX_DEG, SERVO_MIN, SERVO_MAX);

    Kangle = constrain(Kangle, SERVO_MIN, SERVO_MAX);

    for (int id = 1; id <= 4; id++) {

      krs.setPos(id, Kangle);  // 指令は常に送る

    }

    // B. 大幅な角度変化があった場合のみ、駆動ロック（待機）を設定

    // これを毎回やると steeringWaitUntil が更新され続けて走行不能になる

    double angleDiff = fabs(theta – currentServoTheta);

    if (angleDiff > THETA_THRESHOLD || nextHou != hou) {

      unsigned long travelTimeMs = (unsigned long)((angleDiff / SERVO_DEG_PER_SEC) * 1000.0);

      steeringWaitUntil = millis() + travelTimeMs + MARGIN_MS;

      currentServoTheta = theta;  // 基準角度を更新

      hou = nextHou;

      v = 0;  // 急旋回時は速度をリセット

    }

    driveTheta = theta;  // オドメトリ用の角度計算用

    /* ===== 5. 駆動ロック処理 ===== */

    if (millis() < steeringWaitUntil) {

      MOTOR1.set_speed(0);

      MOTOR2.set_speed(0);

      MOTOR3.set_speed(0);

      MOTOR4.set_speed(0);

      delay(1);

      continue;  // ロック中は以下の加速・出力処理をスキップ

    }

    /* ===== 6. 速度制御ロジック（走行中） ===== */

    double v_limit_dist = sqrt(0.5 * A_MAX * (dist / (STOP_DIST / 10.0)));

    double v_target = min(V_MAX, v_limit_dist);

    double dv = v_target – v;

    double max_dv = A_MAX * dt;

    if (dv > max_dv) dv = max_dv;

    if (dv < -max_dv) dv = -max_dv;

    v += dv;

    /* ===== 7. モータ出力 ===== */

    int dir = (hou == 0) ? 1 : -1;

    int speed = constrain((int)v, 0, (int)V_MAX);

    MOTOR1.set_speed(speed * dir * MOTOR_SIGN[0]);

    MOTOR2.set_speed(speed * dir * MOTOR_SIGN[1]);

    MOTOR3.set_speed(speed * dir * MOTOR_SIGN[2]);

    MOTOR4.set_speed(speed * dir * MOTOR_SIGN[3]);

    delay(1);

  }

}

void turnTo(double targetAngle) {

  // — 1. サーボを旋回用の「ハの字」に変形(指令１回だと不安定) —

  for (int i = 0; i <= 5; i++) {

    krs.setPos(1, 9590 – 2000);  // 左前

    krs.setPos(2, 3500 + 20);    // 右前

    krs.setPos(3, 3500 + 20);    // 左後

    krs.setPos(4, 9590 – 2000);  // 右後

  }

  // — 2. 制御パラメータ —

  const double Kp = 5.0;            // 比例ゲイン（ズレに対する強さ）

  const double TURN_V_MAX = 200.0;  // 最大速度制限

  const double TURN_V_MIN = 15.0;   // 最低速度（摩擦で止まらないための底上げ）

  const double TOLERANCE = 0.5;     // 許容誤差（度）

  int stableCount = 0;              // 安定判定用カウンタ

  // — 3. 旋回ループ —

  while (true) {

    double currentAngle = stableYaw;            // Core 0 の imuUpdateTask で更新されている最新の角度を取得

    double error = targetAngle – currentAngle;  // 最短経路での誤差計算（180度以上回らないように補正）

    while (error > 180.0) error -= 360.0;

    while (error < -180.0) error += 360.0;

    if (fabs(error) < TOLERANCE) {  // 終了判定：誤差が TOLERANCE 以内ならカウント

      stableCount++;                // 10ms周期で10回連続（＝0.1秒間）安定したらループ脱出

      if (stableCount >= 10) break;

    } else {

      stableCount = 0;  // 範囲外に出たらカウンタリセット

    }

    double turnSpeed = error * Kp;                              // P制御による速度計算

    turnSpeed = constrain(turnSpeed, -TURN_V_MAX, TURN_V_MAX);  // 速度を MAX_RPM (100) 以内に制限

    // 振動防止と死に体対策

    if (fabs(error) < TOLERANCE) turnSpeed = 0;  // 範囲内なら出力を切る（ハンチング防止）

    else {                                       // 範囲外かつ計算結果が小さすぎる場合、最低速度を保証して動かす

      if (fabs(turnSpeed) < TURN_V_MIN) {

        turnSpeed = (error > 0) ? TURN_V_MIN : -TURN_V_MIN;

      }

    }

    // — 4. モーター出力 —

    // MOTOR_SIGN を掛けて取り付け向きを補正し、全輪で旋回トルクを生む

    // 旋回時は「前進/後退」の向きではなく「回転」の向きを揃える

    int out = (int)turnSpeed;

    MOTOR1.set_speed(-out * MOTOR_SIGN[0]);

    MOTOR2.set_speed(-out * MOTOR_SIGN[1]);

    MOTOR3.set_speed(out * MOTOR_SIGN[2]);

    MOTOR4.set_speed(out * MOTOR_SIGN[3]);

    delay(10);  // ループ周期（100Hz）

  }

  MOTOR1.set_speed(0);  // — 5. 停止処理 —

  MOTOR2.set_speed(0);

  MOTOR3.set_speed(0);

  MOTOR4.set_speed(0);

  driveTheta = stableYaw;  // オドメトリ用の保持角度を更新して終了

}

// ===== ポール回収 =====

void weaponCatch() {

  int xb = 1500;  //後方判定r 1460 l 1630 元　1490

  double dx, dy;  //移動座標

  //stableYaw = 0;     //角度0に再定義

  int w = 0;

  while (w == 0) {

    if (xSemaphoreTake(i2cMutex, portMAX_DELAY) == pdTRUE) {

      w = 1;

      // ======================================

      stableYaw = 0;

      prevYawA = 0;

      prevYawB = 0;

      mpuA.update();

      mpuB.update();

      prevYawA = mpuA.getGyroAngleZ();

      prevYawB = mpuB.getGyroAngleZ();

      // =======================================

      xSemaphoreGive(i2cMutex);  // 終わったら安全に解放

    }

  }

  if (field == 2) {  //赤コート——————————————————————————————

    digitalWrite(rLED, HIGH);

    digitalWrite(gLED, LOW);

    digitalWrite(bLED, HIGH);

    krs.setPos(6, SERVO6_OPEN_VAL);  //開く

    krs.setPos(7, SERVO7_OPEN_VAL);  //開く

    if (pickupPoint == 4) {          //R1側から1番目

      dx = 60;

      dy = 186 + z;

    } else if (pickupPoint == 3) {  //R1側から2番目OK

      dx = 60;

      dy = 206.5 + z;

    } else if (pickupPoint == 2) {  //R1側から3番目

      dx = 60;

      dy = 227.5 + z;

    } else if (pickupPoint == 1) {  //R1側から4番目

      dx = 60;

      dy = 248.5 + z;

    }

    moveTo(dx, dy);  //回収地点へ

    delay(100);

    turnTo(0);                                                    //角度ズレを補正

    for (int i = 0; i < 10; i++) krs.setPos(7, SERVO7_OPEN_VAL);  //開く

    A1.write(A1angle + 75);                                       //75(初期値)

    while (1) {                                                   //後進(センサー使用)

      dx -= 5;

      if (analogRead(BRdis) > xb && pickupPoint != 4) break;  //右センサー(一番近いポールじゃないとき)//値不安定

      if (analogRead(BLdis) > xb && pickupPoint != 1) break;  //左センサー(一番遠いポールじゃないとき)

      krs.setPos(7, SERVO7_OPEN_VAL);                         //開く

      if (PS4.LStickX() > 20) d = map(PS4.LStickX(), 20, 128, 1, -5);

      else if (PS4.LStickX() < -20) d = map(PS4.LStickX(), -20, -128, 1, 5);

      else d = 0;

      dy += d;

      moveTo(dx, dy);

    }

    for (int i = 0; i < 100; i++) krs.setPos(7, SERVO7_CLOSE_VAL);  //ピッキングハンド保持 delay(5)

    A1.write(A1angle);                                              //アーム上げる

    moveTo(dx + 23, 198);                                           //前

    delay(300);

    krs.setPos(6, SERVO6_CLOSE_VAL);  //筒形ハンド保持

    turnTo(88);                       //R2の方を向く

    krs.setPos(6, SERVO6_CLOSE_VAL);  //筒形ハンド保持

    krs.setPos(7, SERVO7_OPEN_VAL);   //ピッキングハンド開く

    delay(800);

    A1.write(A1angle – 23);  //アーム少し下げる

    delay(250);

    krs.setPos(8, SERVO8_SET_VAL);  //シリンダーセット

  } else if (field == 1) {          //青コート——————————————————————————————

    digitalWrite(rLED, HIGH);

    digitalWrite(gLED, LOW);

    digitalWrite(bLED, HIGH);

    krs.setPos(6, SERVO6_OPEN_VAL);  //開く

    krs.setPos(7, SERVO7_OPEN_VAL);  //開く

    if (pickupPoint == 1) {          //R1側から1番目

      dx = 65;                       //

      dy = 188 + z;                  //

    } else if (pickupPoint == 2) {   //R1側から2番目OK

      dx = 65;                       //

      dy = 209 + z;                  //

    } else if (pickupPoint == 3) {   //R1側から3番目

      dx = 70;                       //

      dy = 230.5 + z;                //

    } else if (pickupPoint == 4) {   //R1側から4番目

      dx = 80;                       //

      dy = 249.5 + z;                //

    }

    moveTo(dx, -dy);  //回収地点へ

    delay(100);

    turnTo(0);                                                    //角度ズレを補正

    for (int i = 0; i < 10; i++) krs.setPos(7, SERVO7_OPEN_VAL);  //開く

    A1.write(A1angle + 75);                                       //75(初期値)

    while (1) {                                                   //後進(センサー使用)

      dx -= 5;

      if (analogRead(BRdis) > xb && pickupPoint != 4) break;  //右センサー(一番遠いポールじゃないとき)

      if (analogRead(BLdis) > xb && pickupPoint != 1) break;  //左センサー(一番近いポールじゃないとき)

      krs.setPos(7, SERVO7_OPEN_VAL);                         //開く（閉じる7300）

      moveTo(dx, -dy);

    }

    for (int i = 0; i < 100; i++) krs.setPos(7, SERVO7_CLOSE_VAL);  //ピッキングハンド保持 delay(5)

    A1.write(A1angle);                                              //アーム上げる

    moveTo(dx + 23, -198);                                          //前

    delay(300);

    krs.setPos(6, SERVO6_CLOSE_VAL);  //筒形ハンド保持

    turnTo(-90);                      //R2の方を向く

    krs.setPos(6, SERVO6_CLOSE_VAL);  //筒形ハンド保持

    krs.setPos(7, SERVO7_OPEN_VAL);   //ピッキングハンド開く

    delay(800);

    krs.setPos(7, SERVO7_OPEN_VAL);  //ピッキングハンド開く

    A1.write(A1angle – 23);          //アーム少し下げる

    delay(250);

    krs.setPos(8, SERVO8_SET_VAL);  //シリンダーセット

  }

}

/*————————————————-

NHK Robocon 2026 R2

ESPwroom v3.3.4

・経路に応じてスタートボタンを押すと実行

・積荷センサーを覆いながらスタートすると武器回収なし

・アリーナ侵入後、リセットしないことでリトライ対応

————————————————-*/

#include “DJIMotorCtrlESP.hpp”   //ロボマス(v2.0.5)

#include “HXC_TWAI.hpp”          //CAN通信(v1.0.5)

#include <ESP32Servo.h>          //ノーマルサーボ(v3.0.9)

#include <IcsBaseClass.h>        //近藤科学サーボ(v3)

#include <IcsHardSerialClass.h>  //近藤科学サーボ(v3)

#include <Wire.h>                //ジャイロセンサー(標準)

#include <MPU6050_tockn.h>       //ジャイロセンサー(v1.5.2)

// ===== URM V5.0 PWM =====

#define URTRIG_F 18  //超音波出力ピン（共通）

#define URECHO_F 36  //超音波前方読み取りピン

#define URTRIG_B 18  //超音波出力ピン（共通）

#define URECHO_B 34  //超音波後方読み取りピン

// ===== ロボマス CAN =====

HXC_TWAI CAN_BUS(/*TX=*/17, /*RX=*/16);

M3508_P19 MOTOR1(&CAN_BUS, 1);  //速度制御(左履帯)

M3508_P19 MOTOR2(&CAN_BUS, 2);  //速度制御(右履帯)

M2006_P36 MOTOR3(&CAN_BUS, 3);  //速度制御(後方履帯)

// ===== MPU6050 冗長化設定 =====

MPU6050 mpuA(Wire);

volatile float stableYaw = 0;  // マルチタスクで使う変数はvolatile

float prevYawA = 0;

const float THRESHOLD_NOISE = 5.0;

TaskHandle_t IMUTaskHandle;  // タスクハンドル

// ===== 角度計算タスク（Core 0で永続ループ） =====

void imuUpdateTask(void *pvParameters) {

  while (true) {

    mpuA.update();                              // センサーの値を更新

    float currentA = mpuA.getGyroAngleX();      // 現在の角速度（または角度）を取得

    stableYaw += (currentA – prevYawA) * 0.5f;  // 前回の値との差分を計算して統合変数に加算

    prevYawA = currentA;                        // 次回計算用に現在の値を保存

    vTaskDelay(10 / portTICK_PERIOD_MS);        // 10ms間隔で更新

  }

}

// ===== 近藤科学サーボ UART =====

const int ICS_PIN = 12;

HardwareSerial SerialICS(2);

IcsHardSerialClass krs(&SerialICS, 23, 115200, 1);  //23ピンは後からSWピンとして上書き

// ===== GPIOピン =====

Servo A1, A2, FR1, BR2, FL3, BL4;                                                                //サーボ宣言

const int A1Pin = 14, A2Pin = 27, FRPin = 32, BRPin = 33, FLPin = 25, BLPin = 26;                //サーボピン

const int dis_PIN_R = 39, dis_PIN_L = 15, IR_PIN_R_Arm = 35, IR_PIN_L_Arm = 4, IR_PIN_Box = 19;  //センサー

const int LstartSW = 13, CstartSW = 0, RstartSW = 23;                                            //スイッチ

const int inputPin = 5;                                                                          //ラズパイ

const int FL_start = 146, FR_start = 35, BL_start = 105, BR_start = 62;                          //上昇機構サーボ初期値

const int A1_start = 100, A2_start = 75;                                                         //アームサーボ初期値

//梅花林エリア系

int results[7];   //受信データ代入配列

int masu = 0;     //現在地

int have = 0;     //持ってるブックの個数

int field = 1;    //0:通信不可,1:赤コート,2:青コート

int restart = 0;  //リスタート（1の時リスタート状態）

// ===== URM V5.0 読取関数 =====

unsigned int getDistanceCM(int trigPin, int echoPin) {

  digitalWrite(trigPin, LOW);

  delayMicroseconds(10);

  digitalWrite(trigPin, HIGH);

  unsigned long lowTime = pulseIn(echoPin, LOW, 50000);

  if (lowTime == 0 || lowTime >= 50000) return 999;

  return lowTime / 50;

}

void setup() {

  set();

  StartPosition();  //while文が中にある

  /*if (masu == 0) getBox_flat();       //Lボタン

  if (masu == 1) getOot();  //Cボタン

  if (masu == 2) getBox_down();               //Rボタン*/

  weapon_catch();  //道場

  baikarinn();     //梅花林

}

void loop() {

  arina();  //アリーナ

}

/*———

初期設定

———–*/

void set() {

  Serial.begin(115200);

  Wire.begin();

  delay(250);  //これがないとジャイロが不安定になる?

  pinMode(2, OUTPUT);

  digitalWrite(2, HIGH);

  pinMode(inputPin, INPUT);

  mpuA.begin();

  mpuA.calcGyroOffsets();

  digitalWrite(2, LOW);

  Serial.println(“Calibration Done.”);

  xTaskCreatePinnedToCore(imuUpdateTask, “IMUTask”, 4096, NULL, 1, &IMUTaskHandle, 0);  //キャリブレーション直後に計算タスクを開始

  analogReadResolution(12);

  analogSetAttenuation(ADC_11db);

  CAN_BUS.setup();

  MOTOR1.set_speed_pid(1.0, 0.0, 0.1, 0.0, 10000);

  MOTOR2.set_speed_pid(1.0, 0.0, 0.1, 0.0, 10000);

  MOTOR1.setup();

  MOTOR2.setup();

  MOTOR3.setup();

  A1.attach(A1Pin);

  A2.attach(A2Pin);

  FR1.attach(FRPin);

  BR2.attach(BRPin);

  FL3.attach(FLPin);

  BL4.attach(BLPin);

  SerialICS.begin(115200, SERIAL_8N1, 12, 12);

  krs.begin();

  delay(100);

  Serial1.begin(115200, SERIAL_8N1, 5, -1);

  pinMode(dis_PIN_R, INPUT);  //下距離センサー

  pinMode(dis_PIN_L, INPUT);  //左距離センサー

  pinMode(URTRIG_F, OUTPUT);  //前方距離センサー

  digitalWrite(URTRIG_F, HIGH);

  pinMode(URECHO_F, INPUT);

  pinMode(URTRIG_B, OUTPUT);  //後方距離センサー

  digitalWrite(URTRIG_B, HIGH);

  pinMode(URECHO_B, INPUT);

  pinMode(IR_PIN_R_Arm, INPUT);     //右アームセンサー

  pinMode(IR_PIN_L_Arm, INPUT);     //左アームセンサー

  pinMode(LstartSW, INPUT_PULLUP);  //スタートボタン

  pinMode(CstartSW, INPUT_PULLUP);

  pinMode(RstartSW, INPUT_PULLUP);

}

/*———————-

スタート前の状態＆デバック

————————*/

void StartPosition() {  //初期姿勢

  while (1) {

    if (digitalRead(LstartSW) == 0) {  //左

      if (digitalRead(IR_PIN_Box) == 0) restart = 1;//積荷センサーでリスタート判定

      masu = 0;

      break;

    } else if (digitalRead(CstartSW) == 0) {  //中央

      if (digitalRead(IR_PIN_Box) == 0) restart = 1;//積荷センサーでリスタート判定

      masu = 1;

      break;

    } else if (digitalRead(RstartSW) == 0) {  //右

      if (digitalRead(IR_PIN_Box) == 0) restart = 1;//積荷センサーでリスタート判定

      masu = 2;

      break;

    }

    readUARTData();

    if (results[0] != 0) field = results[0];  //コート選択

    FL3.write(FL_start + 10);

    FR1.write(FR_start – 10);

    BL4.write(BL_start – 35);

    BR2.write(BR_start + 35);

    A1.write(A1_start);

    A2.write(A2_start);

    krs.setPos(1, 6000);

    digitalWrite(2, LOW);

    Serial.print(” Yaw = “);

    Serial.print(stableYaw);

    Serial.print(” 前超音波 = “);

    Serial.print(getDistanceCM(URTRIG_F, URECHO_F));

    Serial.print(” 後超音波 = “);

    Serial.print(getDistanceCM(URTRIG_B, URECHO_B));

    Serial.print(” 下赤外線 = “);

    Serial.print(analogRead(dis_PIN_R));

    Serial.print(” 左赤外線 = “);

    Serial.print(analogRead(dis_PIN_L));

    Serial.print(” 左ハンド = “);

    Serial.print(digitalRead(IR_PIN_L_Arm));

    Serial.print(” 右ハンド = “);

    Serial.print(digitalRead(IR_PIN_R_Arm));

    Serial.print(” 積荷 = “);

    Serial.print(digitalRead(IR_PIN_Box));

    Serial.print(” have = “);

    Serial.print(have);

    Serial.println();

    delay(50);  //早すぎるとバッファが飽和してフリーズする

  }

}

/*———

移動（台形速度制御）

———–*/

void moveTo(float targetDistMM) {

  // 台形速度制御の設定

  const float MAX_RPM = 350.0, MIN_RPM = 20.0;  // 速度(rpm)

  const float ACCEL_DIST = 400.0;               // 加速に要する距離(mm)

  FL3.write(FL_start + 10);

  FR1.write(FR_start – 10);

  BL4.write(BL_start – 35);  //後輪接地

  BR2.write(BR_start + 35);

  float startLoc1 = MOTOR1.get_location();

  float startLoc2 = MOTOR2.get_location();

  float currentDistMM = 0;

  float direction = (targetDistMM >= 0) ? 1.0 : -1.0;

  float totalDistAbs = abs(targetDistMM);

  const float countsPerMM = (8192.0 * 19.0) / (100.0 * 3.14159265);  //1mmあたりの回転

  while (abs(currentDistMM) < totalDistAbs) {

    float loc1 = MOTOR1.get_location() – startLoc1;  // 現在の走行距離を計算

    float loc2 = MOTOR2.get_location() – startLoc2;

    currentDistMM = ((loc1 + (-loc2)) / 2.0) / countsPerMM;

    float currentDistAbs = abs(currentDistMM);

    float distLeft = totalDistAbs – currentDistAbs;

    float currentTargetRPM = MAX_RPM;                                                                                   // 台形速度制御の計算

    if (currentDistAbs < ACCEL_DIST) currentTargetRPM = MIN_RPM + (MAX_RPM – MIN_RPM) * (currentDistAbs / ACCEL_DIST);  // 加速区間

    if (distLeft < (ACCEL_DIST * 1.5f)) {                                                                               // 減速区間（加速の1.5倍の距離で減速開始）

      float decelRPM = MIN_RPM + (MAX_RPM – MIN_RPM) * (distLeft / (ACCEL_DIST * 1.5f));

      if (decelRPM < currentTargetRPM) currentTargetRPM = decelRPM;

    }

    currentTargetRPM = constrain(currentTargetRPM, MIN_RPM, MAX_RPM);

    float speed = currentTargetRPM * direction;  // モーター出力（補正なし、左右均等）

    MOTOR1.set_speed(speed);

    MOTOR2.set_speed(-speed);  // 逆相なのでマイナス

    MOTOR3.set_speed(-speed);

    delay(10);

  }

  MOTOR1.stop(false);

  MOTOR2.stop(false);

  MOTOR3.stop(false);

}

/*————————————————————

移動（箱を追いかける）//繰り返しの実行で有効。終了後、停止操作いる。

————————————————————-*/

void moveBox() {

  BL4.write(BL_start);  //後輪上げる

  BR2.write(BR_start);

  MOTOR3.set_speed(0);                            //後輪停止

  readUARTData();                                 //受信データ読む

  if (results[5] <= -35 && results[5] >= -180) {  // 【左に補正】

    MOTOR1.set_speed(60);                         //減速

    MOTOR2.set_speed(-80);

  } else if (results[5] >= 35 && results[5] <= 180) {  // 【右にに補正】

    MOTOR1.set_speed(80);

    MOTOR2.set_speed(-60);  //減速

  } else {                  //±40以内

    MOTOR1.set_speed(80);

    MOTOR2.set_speed(-80);

  }

  delay(100);

}

/*—————————–

turnTo(絶対角,後方アームの挙動);

——————————-*/

void turnTo(float targetAbsoluteAngle, int flag) {

  BL4.write(BL_start);

  BR2.write(BR_start);

  float MAX_RPM = 350.0, MIN_RPM = 20.0;

  float turnKp = 1.0;

  float turnKd = 0.5;  // ★Dゲイン（ブレーキの強さ）を追加

  float deadBand = 1.0;

  int stableCount = 0;

  float goalYaw = targetAbsoluteAngle;

  static float lastYaw = 0.0;  // 前回ループ時の角度を保存する変数

  lastYaw = stableYaw;         // 初回ループ時のみ現在の角度で初期化

  //足出さないときのパラメーター

  if (flag == 0) {

    MAX_RPM = 100.0;  // ★D制御を入れるので、最高速度を少し上げても大丈夫になります

    MIN_RPM = 15.0;

    turnKp = 1.7;   // ★遠くからでもしっかり加速させるためにPを上げる

    turnKd = 20.0;  // ★【要微調整】スピードが速いほど強烈に逆噴射をかける（2.0〜6.0で調整）

    deadBand = 1.0;

  }

  while (true) {

    float currentYaw = stableYaw;  // ループ先頭の角度（裏で同期されている前提）

    float error = goalYaw – currentYaw;

    // 1. 終了判定

    if (abs(error) < deadBand) stableCount++;

    else stableCount = 0;

    if (stableCount > 5) break;

    // 2. 角速度（10msあたりの角度変化）を計算

    // 右回りを正、左回りを負とすると、勢いよく回っているほどこの値が大きくなる

    float omega = currentYaw – lastYaw;

    lastYaw = currentYaw;  // 次回のために保存

    // 3. 出力計算（P出力 – D出力）

    // 目標へ向かう力(error * turnKp) から、現在の勢いを殺す力(omega * turnKd) を引き算する

    float speed = (error * turnKp) – (omega * turnKd);

    speed = constrain(speed, -MAX_RPM, MAX_RPM);

    // 4. 速度の下限・不感帯処理

    if (abs(error) < deadBand) {

      MOTOR1.stop(false);

      MOTOR2.stop(false);

      continue;

    } else {

      // 遠いときはしっかり回し、目標付近でD制御による「逆噴射」が計算された場合は

      // MIN_RPMによる上書きを無視して、そのまま逆噴射（負の出力など）を許容する

      if (abs(speed) < MIN_RPM && (error * speed > 0)) {

        // errorとspeedが同符号（＝まだ順方向に進もうとしている）の時だけ最低速度保証

        speed = (speed > 0) ? MIN_RPM : -MIN_RPM;

      }

    }

    // アーム制御

    if (abs(targetAbsoluteAngle – currentYaw) <= 3.8 && flag == 1) {

      BL4.write(BL_start – 35);

      BR2.write(BR_start + 35);

    } else if (flag == 0) {

      BL4.write(BL_start);

      BR2.write(BR_start);

    }

    MOTOR1.set_speed(-speed);

    MOTOR2.set_speed(-speed);

    MOTOR3.stop(false);

    delay(10);

  }

  MOTOR1.stop(false);

  MOTOR2.stop(false);

  MOTOR3.stop(false);

}

/——— 道場 ———–/
void weapon_catch() {
stableYaw = 0;
const int open = 6750; //ハンド開く
const int close = 4750; //ハンド閉じる
const int waterlevel = 4840; //台に沿わせる
const int armlittleup_L = 5070; //アーム少し上4950
const int armlittleup_R = 5000; //アーム少し上4950
const int armup = 7500; //アーム上
const int armdoun = 4800; //アーム下
int trial = 1; //試行回数
digitalWrite(2, HIGH);
if (restart == 0) { //リスタートじゃないとき
switch (trial) {
case 1: //—–1回目：左アーム試行—–
krs.setPos(5, armlittleup_L); //左アーム前
krs.setPos(4, armup); //右アーム上
krs.setPos(2, open); //ハンド開く
krs.setPos(3, close); //右閉じる
moveTo(970); //前進
stableYaw = 0;
krs.setPos(5, waterlevel); //到着後台に沿わせる
delay(300);
krs.setPos(2, close); //左ハンド閉じる
delay(350);
krs.setPos(5, armup); //真上
delay(350);
if (digitalRead(IR_PIN_L_Arm) == LOW && field == 1) { // 左センサー反応あり(赤)
moveTo(-1400); //後進 ———–下すべて
krs.setPos(4, armdoun); //下げる
turnTo(90, 1); //回転
moveTo(-400); //後進
turnTo(180, 1); //回転
break;
} else if (digitalRead(IR_PIN_L_Arm) == LOW && field == 2) { // 左センサー反応あり(青)
moveTo(-1600); //後進
krs.setPos(4, armdoun); //下げる
turnTo(-90, 1); //回転
moveTo(-550); //後進
turnTo(-180, 1); //回転
break;
} else trial++; //試行回数加算
case 2: //—–2回目：右アーム試行—–
krs.setPos(5, armup); //真上
moveTo(-100); //後進
krs.setPos(3, open);
krs.setPos(4, armlittleup_R); //アーム前
moveTo(120); //前進
stableYaw = 0;
krs.setPos(4, waterlevel); //到着後台に沿わせる
delay(250);
krs.setPos(3, close); //右ハンド閉じる
delay(350);
krs.setPos(4, armup); //アーム上
delay(350);
if (digitalRead(IR_PIN_R_Arm) == LOW && field == 1) { // 右センサー反応あり(赤)
moveTo(-1400); //後進
krs.setPos(5, armdoun); //下げる
turnTo(90, 1); //回転
moveTo(-500); //400+250(左右差)
turnTo(180, 1); //回転
break;
} else if (digitalRead(IR_PIN_R_Arm) == LOW && field == 2) { // 右センサー反応あり(青)
moveTo(-1600); //後進
krs.setPos(5, armdoun); //下げる
turnTo(-90, 1); //回転
moveTo(-300); //400+250(左右差)
turnTo(-180, 1); //回転
break;
} else trial++; //試行回数加算
case 3: //—–3回目：移動後、左アーム試行—–
moveTo(-150); //後進
if (field == 1) turnTo(90, 1); //赤
else turnTo(-90, 1); //青
moveTo(-320); //後進
turnTo(0, 1);
delay(250);
krs.setPos(2, open); //ハンド開く
delay(300);
krs.setPos(5, armlittleup_L); //左アーム前
krs.setPos(4, 7490); //右アーム上
krs.setPos(3, close); //右閉じる
moveTo(280); //前進(ちょっと多く)
stableYaw = 0;
krs.setPos(5, waterlevel); //到着後台に沿わせる
delay(250);
krs.setPos(2, close); //左ハンド閉じる
delay(350);
krs.setPos(5, armup); //真上
delay(350);
if (digitalRead(IR_PIN_L_Arm) == LOW && field == 1) { // 左センサー反応あり(赤)
moveTo(-1400); //後進
krs.setPos(4, armdoun); //下げる
turnTo(90, 1); //回転
moveTo(-50); //400-320(横にシフトした量)-30(微調整？)
turnTo(180, 1); //回転
break;
} else if (digitalRead(IR_PIN_L_Arm) == LOW && field == 2) { // 左センサー反応あり(青)
moveTo(-1600); //後進
krs.setPos(4, armdoun); //下げる
turnTo(-90, 1); //回転
moveTo(-200); //400-320(横にシフトした量)-30(微調整？)
turnTo(-180, 1); //回転
break;
} else trial++; //試行回数加算
case 4: //—–4回目：移動後、右アーム試行—–
krs.setPos(5, armup); //真上
moveTo(-320); //後進
krs.setPos(3, open);
krs.setPos(4, armlittleup_R); //アーム前
moveTo(280); //前進
stableYaw = 0;
krs.setPos(4, waterlevel); //到着後台に沿わせる
delay(250);
krs.setPos(3, close); //右ハンド閉じる
delay(350);
krs.setPos(4, 7490); //アーム上
delay(350);
if (digitalRead(IR_PIN_R_Arm) == LOW && field == 1) { // 右センサー反応あり(赤)
moveTo(-1400); //後進
krs.setPos(5, armdoun); //下げる
turnTo(-90, 1); //回転
moveTo(-200); //400+250-320(横にシフトした量)-30(微調整？)
turnTo(-180, 1); //回転
break;
} else if (digitalRead(IR_PIN_R_Arm) == LOW && field == 2) { // 右センサー反応あり(青)
moveTo(-1600); //後進
krs.setPos(5, armdoun); //下げる
turnTo(90, 1); //回転
moveTo(-50); //400+250-320(横にシフトした量)-30(微調整？)
turnTo(180, 1); //回転
break;
} else trial++; //試行回数加算
case 5: //—–5回目：移動後、左アーム試行—–
moveTo(-150); //後進
if (field == 1) turnTo(-90, 1); //赤
else turnTo(90, 1); //青
moveTo(450); //前進
turnTo(0, 1);
delay(250);
krs.setPos(2, open); //ハンド開く
//delay(250);
krs.setPos(5, armlittleup_L); //左アーム前
krs.setPos(4, armup); //右アーム上
krs.setPos(3, close); //右閉じる
moveTo(280); //前進(ちょっと多く)
stableYaw = 0;
krs.setPos(5, waterlevel); //到着後台に沿わせる
delay(250);
krs.setPos(2, close); //左ハンド閉じる
delay(350);
krs.setPos(5, armup); //真上
delay(350);
if (digitalRead(IR_PIN_L_Arm) == LOW && field == 1) { // 左センサー反応あり(赤)
moveTo(-1400); //後進
krs.setPos(4, armdoun); //下げる
turnTo(-90, 1); //回転
moveTo(-340); //400-320-320(横にシフトした量)
turnTo(-180, 1); //回転
break;
} else if (digitalRead(IR_PIN_L_Arm) == LOW && field == 2) { // 左センサー反応あり(青)
moveTo(-1600); //後進
krs.setPos(4, armdoun); //下げる
turnTo(90, 1); //回転
moveTo(-500); //400-320-320(横にシフトした量)
turnTo(180, 1); //回転
break;
} else trial++; //試行回数加算
case 6: //—–6回目：移動後、右アーム試行—–
krs.setPos(5, armup); //真上
moveTo(-100); //後進
krs.setPos(3, open);
krs.setPos(4, armlittleup_R); //アーム前
moveTo(160); //前進
stableYaw = 0;
krs.setPos(4, waterlevel); //到着後台に沿わせる
delay(250);
krs.setPos(3, close); //右ハンド閉じる
delay(350);
krs.setPos(4, armup); //アーム上
delay(350);
if (digitalRead(IR_PIN_R_Arm) == LOW && field == 1) { // 右センサー反応あり(赤)
moveTo(-1400); //後進
krs.setPos(5, armdoun); //下げる
turnTo(-90, 1); //回転
moveTo(-500); //400+250-320-320(横にシフトした量)
turnTo(-180, 1); //回転
break;
} else if (digitalRead(IR_PIN_R_Arm) == LOW && field == 2) { // 右センサー反応あり(青)
moveTo(-1600); //後進
krs.setPos(5, armdoun); //下げる
turnTo(90, 1); //回転
moveTo(-340); //400+250-320-320(横にシフトした量)
turnTo(180, 1); //回転
break;
} else { //所持失敗
krs.setPos(3, open); //ハンド開く
moveTo(-1600); //後進
krs.setPos(3, close); //ハンド閉じる
krs.setPos(4, 3500); //下げる
krs.setPos(5, 3500); //下げる
if (field == 1) {
turnTo(-90, 1); //回転
moveTo(-520);
turnTo(-180, 1); //回転
} else {
turnTo(90, 1); //回転
moveTo(-520);
turnTo(180, 1); //回転
}
break;
}
}
BL4.write(BL_start – 35); //後輪接地
BR2.write(BR_start + 35);
delay(600);
float axA = mpuA.getAccX(), ayA = mpuA.getAccY(), azA = mpuA.getAccZ(), refA = sqrt(axA * axA + ayA * ayA + azA * azA);
float threshold = 1.17; // 反応する加速度の大きさ
while (1) { // 振動発生まで待機
mpuA.update();
float cAxA = mpuA.getAccX(), cAyA = mpuA.getAccY(), cAzA = mpuA.getAccZ(), curA = sqrt(cAxA * cAxA + cAyA * cAyA + cAzA * cAzA);
if (abs(curA – refA) > threshold) break;
}
delay(650);
krs.setPos(2, open); //ハンド開く
krs.setPos(3, open);
delay(2500);
krs.setPos(2, close); //ハンド閉じる
krs.setPos(3, close);
krs.setPos(4, 3500); //下げる
krs.setPos(5, 3500); //下げる
digitalWrite(2, LOW);
moveTo(-900); //後進
} else { //リスタート時————————————————————
if (field == 1) { //赤
turnTo(90, 1);
moveTo(-350);
turnTo(179, 1);
} else { //青
turnTo(-90, 1);
moveTo(-350);
turnTo(-179, 1);
}
}
BL4.write(BL_start – 25); //後輪下げる（接地せず）
BR2.write(BR_start + 25);
delay(200);
for (int i = 0; i < 80; i++) { //加速
MOTOR1.set_speed(-i); //少し下がって押し付ける
MOTOR2.set_speed(+i);
MOTOR3.set_speed(0);
delay(5);
}
MOTOR1.set_speed(-80); //少し下がって押し付ける
MOTOR2.set_speed(+80);
MOTOR3.set_speed(0);
delay(1500);
MOTOR1.set_speed(0); //リセット
MOTOR2.set_speed(0);
MOTOR3.set_speed(0);
delay(500);
stableYaw = 0;
//赤
if (field == 1) {
if (masu == 2) moveTo(3600); //Rボタン
else if (masu == 0) moveTo(1080); //Lボタン
}
//青
else {
if (masu == 2) moveTo(1080); //Rボタン -200 + 1100
else if (masu == 0) moveTo(3600); //Lボタン
}
//共通
if (masu == 1) moveTo(2200); //Cボタン
stableYaw = 0;
if (field == 1) turnTo(90, 1); //赤
else turnTo(-89, 1); //青
delay(200);
moveTo(200);
unsigned int distance; //前方距離
int stopcount = 0; //止まった回数
while (true) {
readUARTData();
if (results[4] == 1 && stopcount == 0) { //前方Fakeのとき
stopcount++;
MOTOR1.set_speed(0);
MOTOR2.set_speed(0);
MOTOR3.set_speed(0);
delay(10000); //R1の速度に依存
moveTo(1200); //少し進む
}
distance = getDistanceCM(URTRIG_F, URECHO_F);
if (distance <= 50) break; //近づいた
moveTo(80); //箱を追いかける
}
krs.setPos(1, 6000); //getBox前準備
MOTOR1.set_speed(0);
MOTOR2.set_speed(0);
MOTOR3.set_speed(0);
}

/*———

梅花林エリア

———–*/

int LRC[3][6] = {  //段差配列（赤基準）

  { 0, 200, 400, 200, 0, 0 },

  { 0, 0, 200, 400, 200, 0 },

  { 0, 200, 0, 200, 0, 0 }

};

void baikarinn() {

  if (field == 2) {  //青コート(配列を入れ替える)

    for (int i = 0; i < 6; i++) {

      int tmp = LRC[0][i];    // 1つ目の行のi番目をキープ

      LRC[0][i] = LRC[2][i];  // 1つ目の行に3つ目の行の値を上書き

      LRC[2][i] = tmp;        // 3つ目の行にキープしていた値を入れる

    }

  }

  for (int i = 0; i < 5; i++) {  //5回実行

    delay(300);

    readUARTData();  //画像認識データ更新(念のため２回)

    readUARTData();

    //【特例対応】

    if (masu == 1 && i == 0) {  //中央列、初めの段

      if (analogRead(dis_PIN_L) > 1400) {

        stableYaw = 0;

        getBox_flat();  //ブックある

      } else {          //ブックない

        moveTo(-100);

        i++;

        readUARTData();  //画像認識データ更新(念のため２回)

        readUARTData();

      }

    }

    //【前方ブックあり】

    if (results[3] != 2) {

      stableYaw = 0;

      if (have == 2) getOot();                                          //2つ持ってる

      if (LRC[masu][i + 1] – LRC[masu][i] == 200) getBox_up();          //前方上

      else if (LRC[masu][i + 1] – LRC[masu][i] == -200) getBox_down();  //前方下

      else getBox_flat();                                               //前方段差なし

    }

    //【左ブックあり】(Realブックあり && 保持１以下 && 前奥反応なし && 左レーンじゃない)

    if (results[1] == 0 && have != 2 && results[4] == 2 && masu != 0) {

      moveTo(400);

      turnTo(-25, 1);

      moveTo(-350);

      turnTo(90, 1);  //左向く

      moveTo(50);

      stableYaw = 0;                                                        //補正のためリセット

      if (LRC[masu – 1][i] – LRC[masu][i] == 200) getBox_up();              //自身の左が上方

      else if (LRC[masu – 1][i] – LRC[masu][i] == -200) { getBox_down(); }  //自身の左が下方

      moveTo(400);

      turnTo(25, 1);

      moveTo(-350);

      turnTo(-90, 1);  //前に向きなおす

    }

    //【右ブックあり】(Realブックあり && 保持１以下 && 前奥反応なし && 右レーンじゃない)

    if (results[2] == 0 && have != 2 && results[4] == 2 && masu != 2) {

      moveTo(400);

      turnTo(25, 1);

      moveTo(-350);

      turnTo(-90, 1);  //右向く

      moveTo(50);

      stableYaw = 0;                                                        //補正のためリセット

      if (LRC[masu + 1][i] – LRC[masu][i] == 200) getBox_up();              //自身の右が上方

      else if (LRC[masu + 1][i] – LRC[masu][i] == -200) { getBox_down(); }  //自身の右が下方

      moveTo(400);

      turnTo(-25, 1);

      moveTo(-350);

      turnTo(90, 1);  //前に向きなおす

    }

    //【次のマスへ】

    if ((LRC[masu][i + 1] – LRC[masu][i]) == 0) {

      moveTo(1200);  //段差なし

      stableYaw = 0;

    } else if ((LRC[masu][i + 1] – LRC[masu][i]) == 200) up();   //上り

    else if ((LRC[masu][i + 1] – LRC[masu][i]) == -200) down();  //下り

    delay(250);

  }

  if (field == 1) {  //赤コート

    turnTo(-90, 1);

    if (masu == 2) moveTo(800);        //R

    else if (masu == 1) moveTo(2200);  //C

    else if (masu == 0) moveTo(3500);  //L

    turnTo(0, 1);

    moveTo(2000);  //アリーナに進入

    turnTo(90, 1);

    moveTo(2000);

  } else {  //青コート

    turnTo(90, 1);

    if (masu == 0) moveTo(800);        //L

    else if (masu == 1) moveTo(2200);  //C

    else if (masu == 2) moveTo(3500);  //R

    turnTo(0, 1);

    moveTo(2000);  //アリーナに進入

    turnTo(-90, 1);

    moveTo(3000);

  }

}

/*———

段差登る

———–*/

void up() {

  // ===== 初期サーボ動作 =====

  digitalWrite(2, HIGH);

  krs.setPos(5, 4895);  //ヘッド回収機構の干渉防止

  krs.setPos(4, 4765);

  BL4.write(BL_start – 35);  //後輪接地

  BR2.write(BR_start + 35);

  delay(250);

  // ===== 1. 前方の段差に接近する動作 =====

  while (true) {

    unsigned int distance = getDistanceCM(URTRIG_F, URECHO_F);  //前方は条件満たしてる？

    if (distance <= 14) break;                                  //条件を満たした回数でbreak

    MOTOR1.set_speed(100);

    MOTOR2.set_speed(-100);

    MOTOR3.set_speed(-100);

    delay(10);

  }

  // ===== 2. 段差への乗り上げ開始（サーボ駆動） =====

  MOTOR1.set_speed(80);

  MOTOR2.set_speed(-80);

  MOTOR3.set_speed(-180);

  FL3.write(FL_start – 51);  //先に前アーム

  FR1.write(FR_start + 51);

  delay(100);//150

  BL4.write(BL_start – 78);

  BR2.write(BR_start + 78);

  delay(250);

  MOTOR1.set_speed(100);

  MOTOR2.set_speed(-100);

  MOTOR3.set_speed(-100);

  delay(550);

  // ===== 3. 後方壁検知で登りきりを確認 =====

  int n = 0;

  while (true) {

    unsigned int backDistance = getDistanceCM(URTRIG_B, URECHO_B);

    if (backDistance <= 12) n++;  //後方は条件満たしてる？

    if (n > 2) break;             //条件を満たした回数でbreak

    MOTOR1.set_speed(80);

    MOTOR2.set_speed(-80);

    MOTOR3.set_speed(-80);

    delay(30);

  }

  MOTOR1.set_speed(80);

  MOTOR2.set_speed(-80);

  MOTOR3.set_speed(-80);

  delay(250);

  MOTOR1.stop(false);

  MOTOR2.stop(false);

  MOTOR3.stop(false);

  FL3.write(FL_start);  //元の角度へ

  FR1.write(FR_start);

  BL4.write(BL_start);

  BR2.write(BR_start);

  delay(500);

  moveTo(350);  //要検討

  digitalWrite(2, LOW);

  stableYaw = 0;

}  // ここでup関数が終了

/*———

段差降りる

———–*/

void down() {

  // ===== 初期サーボ動作 =====

  digitalWrite(2, HIGH);

  krs.setPos(5, 4895);  //ヘッド回収機構の干渉防止

  krs.setPos(4, 4765);

  delay(100);

  FL3.write(FL_start – 51);  //95

  FR1.write(FR_start + 51);  //86

  delay(500);

  // ===== 床検知の動作 =====

  int n = 0;

  while (true) {

    unsigned int distance = getDistanceCM(URTRIG_F, URECHO_F);

    if (distance > 6) n++;  //前方は条件満たしてる？

    if (n > 3) break;       //条件を満たした回数でbreak

    MOTOR1.set_speed(80);

    MOTOR2.set_speed(-80);

    MOTOR3.set_speed(-80);

    delay(50);

  }

  // ===== 減速中の動作 =====

  FL3.write(FL_start – 111);  //35

  FR1.write(FR_start + 111);  //146

  BL4.write(BL_start – 35);

  BR2.write(BR_start + 35);

  delay(500);

  // ===== 前方壁検知まで走行 =====

  int m = 0;

  while (true) {

    unsigned int floorDistanceF = getDistanceCM(URTRIG_F, URECHO_F);

    if (floorDistanceF >= 45) m++;  //前方は条件満たしてる？

    unsigned int floorDistanceB = getDistanceCM(URTRIG_B, URECHO_B);

    if (floorDistanceB >= 7) m++;  //後方は条件満たしてる？

    if (m > 3) break;              //条件を満たした回数でbreak

    MOTOR1.set_speed(80);

    MOTOR2.set_speed(-80);

    MOTOR3.set_speed(-80);

    delay(25);

  }

  // ===== 前方床検知後、サーボの角度を徐々に変更 =====

  // 開始角度（今の姿勢）

  int FL_now = 35;

  int FR_now = 146;

  int BL_now = BL_start – 35;

  int BR_now = BR_start + 35;

  // 目標角度（次の姿勢）

  int FL_target = 95;

  int FR_target = 86;

  int BL_target = BL_start;

  int BR_target = BR_start;

  int steps = 60;

  for (int i = 0; i <= steps; i += 7) {

    float t = (float)i / steps;

    FL3.write(FL_now + (FL_target – FL_now) * t);

    FR1.write(FR_now + (FR_target – FR_now) * t);

    BL4.write(BL_now + (BL_target – BL_now) * t);

    BR2.write(BR_now + (BR_target – BR_now) * t);

    MOTOR1.set_speed(50);

    MOTOR2.set_speed(-50);

    MOTOR3.set_speed(-50);

    delay(50);

  }

  // ===== 最終停止処理 =====

  MOTOR1.set_speed(80);  //少し進む

  MOTOR2.set_speed(-80);

  MOTOR3.set_speed(-80);

  delay(200);

  BL4.write(BL_start – 25);  //後輪下げる（接地せず）

  BR2.write(BR_start + 25);

  delay(200);

  MOTOR1.set_speed(-80);  //少し下がって押し付ける

  MOTOR2.set_speed(80);

  MOTOR3.stop(false);

  delay(750);

  moveTo(150);

  digitalWrite(2, LOW);

  stableYaw = 0;  //押し付けたので角度リセット

  delay(300);

}

/—————————————— ブック回収 ２つ目のブックを落とす : getOot(); 同じ段から回収(0m) : getBox_flat(); 高い段から回収(200mm) : getBox_up(); 低い段から回収(-200mm) : getBox_down(); ——————————————/
int grip = 4500; //箱掴む
int lowgrip = 5000; //箱掴む(少し弱く)
int nongrip = 8700; //ハンド広げる
// ===== 2つ目のブックを落とす =====
void getOot() {
digitalWrite(2, HIGH);
have = 1; //2つ目落とすと必ず1
krs.setPos(1, nongrip);
delay(500);
for (int i = A1_start + 5; i >= A1_start – 70; i -= 10) A1.write(i); // 105 → 30
krs.setPos(1, grip);
delay(300);
for (int i = A2_start – 10; i >= A2_start – 40; i -= 5) A2.write(i); // 叩き落とす
krs.setPos(1, nongrip);
delay(300);
for (int i = A2_start – 40; i <= A2_start; i += 5) A2.write(i); // 40 → 75 for (int i = A1_start – 70; i <= A1_start; i += 5) A1.write(i); // 30 → 100 delay(300); digitalWrite(2, LOW); } // ===== 自分の前方同じ段から回収(0m) ==== void getBox_flat() { have = 1; //※平面上の回収は１つ目以外ありえない digitalWrite(2, HIGH); krs.setPos(1, nongrip); readUARTData(); if (abs(results[5]) > 15) { //絶対値15以上のとき発動
turnTo(map(results[5], -170, 170, 60, -60), 1);
moveTo(150);
turnTo(0, 1);
} else moveTo(150);
for (int i = A1_start; i >= A1_start – 60; i -= 10) A1.write(i); // 100 → 40
for (int i = A2_start; i <= A2_start + 90; i += 10) A2.write(i); // 75 → 165 moveTo(250); krs.setPos(1, grip); //ボックス保持中 delay(300); for (int i = A1_start – 60; i <= A1_start – 25; i += 5) A1.write(i); // 40 → 75 for (int i = A2_start + 90; i >= A2_start – 15; i -= 5) A2.write(i); // 165 → 60
for (int i = A2_start – 15; i <= A2_start; i += 4) A2.write(i); // 60 → 75 for (int i = A1_start – 25; i <= A1_start; i += 4) A1.write(i); // 75 → 100 A2.write(80); delay(450); krs.setPos(1, nongrip); A2.write(75); delay(200); digitalWrite(2, LOW); } // ===== 自分より高い段から回収(200mm) ===== void getBox_up() { digitalWrite(2, HIGH); krs.setPos(1, nongrip); readUARTData(); if (abs(results[5]) > 15) { //絶対値20以上のとき発動
turnTo(map(results[5], -170, 170, 60, -60), 0);
moveTo(150);
turnTo(0, 0);
} else moveTo(150);
moveTo(250);
moveTo(-400);
for (int i = A2_start; i <= A2_start + 60; i += 10) A2.write(i); // 75 → 135 for (int i = A1_start; i >= A1_start – 100; i -= 8) A1.write(i); // 100 → 0
moveTo(250);
for (int i = 0; i < 5; i++) moveTo(20); krs.setPos(1, grip); //ボックス保持中 delay(450); if (digitalRead(IR_PIN_Box) == LOW) { //１ボックス設置済み have = 2; for (int i = A1_start – 100; i <= A1_start – 45; i += 4) A1.write(i); // 0 → 55 for (int i = A2_start + 60; i >= A2_start + 3; i -= 2) A2.write(i); // 135 → 78
delay(300);
krs.setPos(1, nongrip);
delay(300);
for (int i = A1_start – 45; i <= A1_start – 30; i += 5) A1.write(i); // 55 → 70 for (int i = A2_start + 3; i >= A2_start – 15; i -= 5) A2.write(i); // 78 → 60
delay(300);
krs.setPos(1, grip);
delay(300);
for (int i = A2_start – 15; i <= A2_start + 3; i += 5) A2.write(i); // 60 → 78 for (int i = A1_start – 30; i <= A1_start – 12; i += 5) A1.write(i); // 70 → 88 krs.setPos(1, lowgrip); } else { //保持ボックスなし have = 1; for (int i = A1_start – 100; i <= A1_start – 25; i += 10) A1.write(i); // 0 → 75 for (int i = A2_start + 60; i >= A2_start – 15; i -= 10) A2.write(i); // 135 → 60
for (int i = A2_start – 15; i <= A2_start; i += 5) A2.write(i); // 60 → 75 for (int i = A1_start – 25; i <= A1_start; i += 5) A1.write(i); // 75 → 100 delay(300); krs.setPos(1, nongrip); delay(500); A2.write(A2_start – 15); //60 delay(450); krs.setPos(1, grip); delay(450); A2.write(A2_start + 5); //80 delay(450); krs.setPos(1, nongrip); A2.write(A2_start); //75 delay(200); } moveTo(80); moveTo(-400); digitalWrite(2, LOW); } // ===== 自分より低い段から回収(-200mm) ===== void getBox_down() { digitalWrite(2, HIGH); krs.setPos(1, nongrip); readUARTData(); if (abs(results[5]) > 15) { //絶対値20以上のとき発動
turnTo(map(results[5], -170, 170, 60, -60), 1);
moveTo(150);
turnTo(0, 1);
} else moveTo(150);
krs.setPos(5, 4895); //ヘッド回収機構の干渉防止(kokokaratuika)
krs.setPos(4, 4765);
delay(100);
FL3.write(FL_start – 51); //95
FR1.write(FR_start + 51); //86
BL4.write(BL_start – 35); //後輪接地
BR2.write(BR_start + 35);
delay(500);
// —– 床検知の動作 —–
int n = 0;
while (true) {
unsigned int distance = getDistanceCM(URTRIG_F, URECHO_F);
if (distance > 10) n++; //前方は条件満たしてる？
if (n > 2) break; //条件を満たした回数でbreak
MOTOR1.set_speed(50); //moveToは前足を上げてしまうため
MOTOR2.set_speed(-50);
MOTOR3.set_speed(-50);
delay(25);
}
moveTo(-200); //前進し過ぎるので一旦後進
// —– アーム展開 —–
for (int i = A1_start; i >= A1_start – 60; i -= 5) A1.write(i); // 100 → 40
for (int i = A2_start; i <= A2_start + 90; i += 5) A2.write(i); // 75 → 165 moveTo(225); //少し前進 krs.setPos(1, grip); //ボックス保持 delay(300); if (digitalRead(IR_PIN_Box) == LOW) { //1ボックス設置済み have = 2; for (int i = A1_start – 60; i <= A1_start – 12; i += 10) A1.write(i); // 40 → 88 for (int i = A2_start + 90; i >= A2_start – 9; i -= 3) A2.write(i); // 165 → 66
delay(450);
krs.setPos(1, nongrip);
for (int i = A1_start – 12; i >= A1_start – 37; i -= 2) A1.write(i); // 88 → 63
for (int i = A2_start – 9; i >= A2_start – 25; i -= 2) A2.write(i); // 66 → 50
delay(300);
krs.setPos(1, grip);
delay(300);
A1.write(A1_start – 45); //55
delay(100);
A2.write(A2_start – 10); //65
delay(100);
A1.write(A1_start – 12); //88
krs.setPos(1, lowgrip);
} else { //ボックス未設置
have = 1;
for (int i = A1_start – 60; i <= A1_start + 5; i += 10) A1.write(i); // 40 → 105 for (int i = A2_start + 90; i >= A2_start – 10; i -= 5) A2.write(i); // 165 → 65
for (int i = A1_start + 5; i <= A1_start + 20; i += 5) A1.write(i); // 105 → 120 delay(300); krs.setPos(1, nongrip); for (int i = A1_start + 20; i >= A1_start; i -= 10) A1.write(i); // 120 → 100
for (int i = A2_start – 10; i <= A2_start; i += 10) A2.write(i); // 65 → 75
krs.setPos(1, nongrip);
delay(450);
A2.write(A2_start – 5); //70
delay(450);
krs.setPos(1, grip);
delay(450);
A2.write(A2_start + 5); //80
delay(450);
krs.setPos(1, nongrip);
A2.write(75);
delay(200);
}
moveTo(-400);
digitalWrite(2, LOW);
}

/*———————–

ラズパイからの画像認識結果

　RaspberryPi5 – TX = 14

　ESP32wroom   – RX = 5

　M5stack      – RX = 36

————————*/

// results[5] は定義済み

void readUARTData() {

  static char rx_buf[64];

  static int rx_idx = 0;

  while (Serial1.available() > 0) {

    char c = Serial1.read();

    //Serial.write(c);  // 受信した文字をそのまま表示

    if (c == ‘\n’) {

      rx_buf[rx_idx] = ‘\0’;

      int r0, r1, r2, r3, r4, r5, r6;

      if (sscanf(rx_buf, “S,%d,%d,%d,%d,%d,%d”, &r0, &r1, &r2, &r3, &r4, &r5) == 6) {

        results[0] = r0;  //0:通信不可, 1:赤, 2:青

        results[1] = r1;  //左(1m以内)　0:Real, 1:Fake, 2:none

        results[2] = r2;  //右(1m以内)

        results[3] = r3;  //前(1m以内)

        results[4] = r4;  //前(1m以上)

        results[5] = r5;  //前中央箱の座標

      }

      rx_idx = 0;

    } else if (rx_idx < 63) {

      rx_buf[rx_idx++] = c;

    }

  }

}

/*———

ブック設置

———–*/

void Put_Box2() {  //2段目

  digitalWrite(2, HIGH);

  delay(2000);      //短くしたい

  if (have == 2) {  //2つ保持

    krs.setPos(1, 8700);

    A1.write(72);

    /*A2.write(75);

    for (int i = 100; i >= 72; i -= 4) A1.write(i);  // 100 → 78*/

    for (int i = 63; i >= 55; i -= 2) A2.write(i);  // 65 → 55

    krs.setPos(1, 4000);

    delay(300);

    for (int i = 72; i >= 0; i -= 3) A1.write(i);   // 100 → 78

    for (int i = 55; i <= 74; i += 2) A2.write(i);  // 55 → 70

    delay(50);

    krs.setPos(1, 8700);  //離す

    delay(300);

    for (int i = 0; i <= 20; i += 5) {

      float ratio = i / 20.0;

      int a1 = 0 + (20 – 0) * ratio;    // 0 → 20

      int a2 = 75 + (90 – 75) * ratio;  // 75 → 90

      A1.write(a1);

      A2.write(a2);

    }

    for (int i = 20; i <= 100; i += 5) A1.write(i);  // 20 → 100

    for (int i = 90; i >= 75; i -= 5) A2.write(i);   // 90 → 75

    delay(200);

  } else if (have == 1) {  //1つ保持

    krs.setPos(1, 8700);

    delay(300);

    A1.write(100);

    A2.write(75);

    delay(500);

    A1.write(115);

    delay(500);

    A2.write(65);

    krs.setPos(1, 4000);

    delay(300);

    for (int i = 78; i >= 0; i -= 3) A1.write(i);   // 78 → 0

    for (int i = 65; i <= 75; i += 3) A2.write(i);  // 65 → 75

    delay(50);

    krs.setPos(1, 8700);

    delay(100);

    A2.write(62);  //素早く再保持地点へ移動

    delay(300);

    krs.setPos(1, 4000);  //掴みなおす

    delay(500);

    for (int i = 62; i <= 75; i += 3) A2.write(i);  //60→70

    delay(500);

    krs.setPos(1, 8700);

    delay(500);

    for (int i = 0; i <= 100; i += 8) A1.write(i);  // 0 → 100

  }

  digitalWrite(2, LOW);

  have–;

}

void Put_Box3() {  //３段目

  digitalWrite(2, HIGH);

  delay(2000);      //短くしたい

  if (have == 2) {  //2つ保持

    //A1.write(100);

    //A2.write(75);

    krs.setPos(1, 8700);

    delay(300);

    for (int i = 72; i <= 73; i += 1) {

      A1.write(i);  // 72 → 76

      delay(3);

    }

    for (int i = 63; i >= 60; i -= 1) {

      A2.write(i);  // 78 → 55

      delay(3);

    }

    krs.setPos(1, 3700);

    delay(300);

    for (int i = 55; i <= 70; i += 7) A2.write(i);  // 55 → 70

    for (int i = 76; i >= 1; i -= 7) A1.write(i);   // 78 → 3

    delay(200);

    krs.setPos(1, 8700);  //離す

    delay(500);

    /*A2.write(59);

    krs.setPos(1, 4000);  //掴みなおす

    A2.write(74);

    delay(100);

    krs.setPos(1, 8700);

    delay(300);*/

    for (int i = 1; i <= 20; i += 5) {

      float ratio = i / 20.0;

      int a1 = 3 + (20 – 3) * ratio;    // 0 → 20

      int a2 = 70 + (90 – 70) * ratio;  // 75 → 90

      A1.write(a1);

      A2.write(a2);

    }

    for (int i = 20; i <= 100; i += 5) A1.write(i);  // 100 → 78

    for (int i = 90; i >= 75; i -= 5) A2.write(i);   // 75 → 65

    //delay(200);

  } else if (have == 1) {  //1つ保持

    krs.setPos(1, 8700);

    A1.write(100);

    A2.write(75);

    delay(500);

    A1.write(115);

    delay(500);

    A2.write(65);

    krs.setPos(1, 4000);

    delay(300);

    for (int i = 78; i >= 0; i -= 5) A1.write(i);   // 78 → 0

    for (int i = 65; i <= 73; i += 4) A2.write(i);  // 65 → 70

    delay(50);

    krs.setPos(1, 8700);

    delay(100);

    A2.write(67);  //素早く再保持地点へ移動

    delay(350);

    //krs.setPos(1, 4000);  //掴みなおす

    delay(500);

    for (int i = 67; i <= 75; i += 4) A2.write(i);  //65→75

    delay(500);

    krs.setPos(1, 8700);

    delay(500);

    for (int i = 0; i <= 100; i += 8) A1.write(i);  // 0 → 100

  }

  digitalWrite(2, LOW);

  have–;

}

/*———

アリーナ

———–*/

void arina() {

  krs.setPos(4, 3500);  //下げる

  krs.setPos(5, 3500);  //下げる

  FL3.write(FL_start);

  FR1.write(FR_start);

  BL4.write(BL_start);

  BR2.write(BR_start);

  MOTOR1.stop(false);

  MOTOR2.stop(false);

  MOTOR3.stop(false);

  // 1. 指定の範囲内かつボックスが載るまでループ

  int val = analogRead(dis_PIN_R);

  if (have != 0) {

    for (int i = 100; i >= 72; i -= 4) A1.write(i);  // 100 → 72

    for (int i = 75; i >= 63; i -= 2) A2.write(i);   // 75 → 63

    krs.setPos(1, 4800);                             //ハンド

  } else {

    A1.write(A1_start);

    A2.write(A2_start);

    krs.setPos(1, 8700);  //ハンド

  }

  while (true) {

    val = analogRead(dis_PIN_R);  //更新

    krs.setPos(2, 4900);          //ハンド閉じる

    krs.setPos(3, 4900);

    if (have == 0 && digitalRead(IR_PIN_Box) == 0) have = 1;  // 積荷センサーのチェック

    if ((val > 1200 && val < 2000) && have != 0) break;       //位置が範囲内&&ボックスがある

    delay(10);

  }

  int base_val = (analogRead(dis_PIN_R) + analogRead(dis_PIN_R) + analogRead(dis_PIN_R)) / 3;  // 3回の平均

  int low_count = 0, high_count = 0;                                                           // チャタリング防止用のカウンター

  // 2. モーション発動するまでループ

  while (true) {

    val = analogRead(dis_PIN_R);  //更新

    krs.setPos(1, 4800);          //ハンド半開き？

    krs.setPos(2, 6000);          //ハンド開く

    krs.setPos(3, 6000);

    // — 条件判定（カウント処理） —

    if (val < base_val – 350) low_count++;   //3段目（値が小さくなる側）の判定

    else low_count = 0;                      // 外れたらリセット

    if (val > base_val + 800) high_count++;  // 2段目（値が大きくなる側）の判定

    else high_count = 0;                     // 外れたらリセット

    // — 3回連続で満たしたら実行 —

    if (low_count >= 3) {

      //krs.setPos(1, 4800);

      Put_Box3();  // ３段目へ

      break;

    }

    if (high_count >= 3) {

      //krs.setPos(1, 4800);

      Put_Box2();  // ２段目へ

      break;

    }

    delay(10);

  }

}