プロジェクト | K*RISE / 九産大の学生による大学魅力発信プロジェクト Fri, 09 Oct 2026 00:16:00 +0000 ja hourly 1 https://wordpress.org/?v=7.1.3 /wp-content/uploads/2025/05/cropped-アセット-20-32x32.png プロジェクト | K*RISE / 32 32 キャチロボ2025 ロボット紹介 /%e3%82%ad%e3%83%a3%e3%83%81%e3%83%ad%e3%83%9c2025-%e3%83%ad%e3%83%9c%e3%83%83%e3%83%88%e7%b4%b9%e4%bb%8b/ Fri, 09 Oct 2026 00:16:00 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=1895

京都で開催されたキャチロボバトルコンテストに、ロボット工房から「ぶちょー」と「FTFT」の2チームで出場しました。

私はチーム「ぶちょー」としてロボットを製作しました。

このロボットについてご紹介します。

大会概要

一言でいうと、

「お菓子を陳列する競技」

今年のワークはプチアソートでした。

もちろん競技はチョコが入っている状態で行われるので、会場は甘い匂いが漂っていました。

試合は対戦形式で、赤コートと青コートがあります。

コートの色によって、お菓子の陳列先が自身の右側なのか左側なのかと変化します。

シューティングエリアにお菓子を置くと得点が入るわけですが、このようにパッケージが揃うように5つ陳列することで追加のボーナス得点を獲得できます。

つぶれたマーブル

このようにワークを破損させると減点されます。

共通エリアなんて要素もありますが今回はスルーしました。

詳しいルールは公式ホームページをご覧ください。

【公式】キャチロボバトルコンテストWEBサイト
キャチロボバトルコンテストはマテリアルハンドリング技術 (マテハン)に特化した学生限定の一般的なロボコンと一線を画すユニークな大会です。

結果報告

予選敗退でしたが、特別枠で紹介いただきました。

第15回キャチロボバトルコンテスト・ライブ配信
日程:2025年9月21日(日)12時~会場:京都国際会館イベントホール○キャチロボバトルコンテストについてキャチロボバトルコンテストは、京都工芸繊維大学・ロボコン挑戦プロジェクト、 立命館大学・ロボット技術研究会、大阪大学・Robohan...

予選試合1回目は38点

予選試合2回目は15点

敗因は後ほど。

機構

デデーン

自画自賛になりますが、かっこいいです。

どことなく要塞のような、ずっしりとした佇まいです。

このロボットには大きく3つの機構が搭載されています。

1.メインアーム

5種類のワークを1つずつ。計5つを同時に回収して中央にまとめます。

XYZ軸の直動機構で構成されており、横のシューティングエリアには関与しません。

初めからボーナス得点を狙っていたため、中央によせる機構をメインに考えていました。

試作は木材です。

このマジックハンド機構は関節が多く、精度が悪くなりやすいというデメリットがあります。

できる限り実用性を追求したかったので1ヶ所の関節につき4枚のベアリングをいれています。

とは言っても、穴あけをボール盤で行っているため穴間距離はバラつきがありました。

レールにはアルミフレーム(V-slot)を使っています。

メインアームの前後駆動はラック&ピニオン機構です。

ロボマスモーターの減速比が少し大きかったためピニオンギアの直径を大きく設けています。

ラック側はT2のジュラルミン板を切削して作りました。

時間とお金と労力がかかっています。たぶんフレキラックで十分でした。

ちなみに、CNCで歯車を削り出す場合はモジュール2.5を採用しています。∅3のエンドミルで切削が可能であるためです。

Z軸は前後駆動(Y軸)の先端についています。

サーボモーターで動かしており、上から定荷重ばねで吊っています。

後ろ側に基板スペースを設けています。

各アクチュエータに基板が内蔵されているような構成なので省スペース。

2.サブアーム

Y軸と4軸アームで構成されています。

ボーナス得点を獲得するために棚へ陳列する動作を行います。

スローで撮影

3.パスアーム

メインアームで回収されたワークを乗せてサブアームにパスします。

このとき遠心力と慣性を使うことで整頓も行います。

苦肉の策で作った機構でしたが、かなり良いものに仕上がりました。

回路

ArduinoUNOを中心に構成しています。

無線と画面表示をM5stuck(I2C通信)

ロボマス用のCAN通信をMCP2515(SPI通信)

近藤科学のサーボはスレーブモードに(UART)

処理能力的にはあまり余裕が無さそうですね

プログラム

基本的にコントローラーの入力に応じた処理をするだけだったのでシンプルです。

2025_kyati_uno

#include <Wire.h>

#include <IcsBaseClass.h>

#include <IcsHardSerialClass.h>

#include “config.h”

#include “receiveEvent.h”

#include “displayReceivedData.h”

#include “robomas.h”

const int mos = 3;  //真空ポンプ

const byte EN_PIN = 4;  //いらないが消すとエラー

const long BAUDRATE = 115200;

const int TIMEOUT = 3;                                       //魔の数字(送信間隔)

IcsHardSerialClass krs(&Serial, EN_PIN, BAUDRATE, TIMEOUT);  //インスタンス+ENピンおよびUARTの指定

float a = 7500 + 320;  //0 主x

float b = 7500 – 320;  //1 主x

int c = 11250;         //2 主Z

int d = 7500;          //3 サブZ

int e = 7500;          //4 サブx

int g = 7500;          //6 サブ先端

int h = 7800;          //7 サブ先端縦8100

int j = 10500;         //8 パス

int k = 4150;          //9 パス

int haba = 1;  //広く

int taka = 1;  //高さ

int pasu = 0;  //パス

float ha, hb;  //主X 1ステップの値を格納

void setup() {

  Wire.begin(8);  // I2Cアドレスを8に設定

  Wire.onReceive(receiveEvent);

  //Serial.begin(115200);

  krs.begin();  //サーボモータの通信初期設定

  C610::begin();

  pinMode(mos, OUTPUT);

}

void loop() {

  /*構成

  初期化

  メインY

  メインZ

  メインX横

  メインX幅

  サブX

  サブY

  サブZ

  吸着機構

  パス機構

  サブ受取モーション

  サブ設置モーション

  */

  if (dataReceived) {               //データ受け取ってたら

    if (receivedHou == 2) {         //赤コート

      if (receivedPSButton == 1) {  //初期状態に

        a = 7500 + 320;             //0

        b = 7500 – 320;             //1

        c = 11250;                  //2

        j = 10500;                  //8 パス

        pasu = 0;

        krs.setPos(2, c);

        unsigned long start = millis();

        while (millis() – start < 1000) {

          C610::put(1, 2500);

          //C610::put(2, -750);

          C610::send();

        }

      }

      if (receivedRightStickY > 135) {  //主Y

        C610::put(1, map(receivedRightStickY, 135, 225, 0, -10000));

      } else if (receivedRightStickY < 120) {

        C610::put(1, map(receivedRightStickY, 120, 0, 0, 10000));

      } else {

        C610::put(1, 0);

      }

      if (receivedTriangleButton == 1) {  //主Z

        taka = 1;

        c = 11250;  //上方向

      } else if (receivedCrossButton == 1) {

        if (taka == 1) {

          taka = 2;

          c = 4100;  //下方向

          delay(200);

        } else if (taka == 2) {

          c -= 100;

          delay(100);

        }

      }

      if (receivedR2Value > 225) {  //主X横左右選択

        a = 7500 + 20;

        b = 7500 – 620;

        delay(200);

      } else if (receivedL2Value > 225) {

        a = 7500 + 700;

        b = 7500 + 60;

        delay(200);

      } else if (receivedR2Value > 10 && a > 3500 && b > 3500) {  //主X横微調節

        a -= map(receivedR2Value, 10, 255, 1, 40);

        b -= map(receivedR2Value, 10, 255, 1, 40);

      } else if (receivedL2Value > 10 && a < 11500 && b < 11500) {

        a += map(receivedL2Value, 10, 255, 1, 40);

        b += map(receivedL2Value, 10, 255, 1, 40);

      }

      if (receivedR1Button == 1) {  //主X幅

        if (haba == 1) {            //狭める

          haba = 0;

          ha = (a – 6300) / 300;  //6500

          hb = (b – 8300) / 300;  //8500

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

            a -= ha;

            b -= hb;

            krs.setPos(0, a);

            krs.setPos(1, b);

            delay(1);

          }

        } else {  //広げる

          haba = 1;

          ha = (a – (7500 + 320)) / 100;

          hb = (b – (7500 – 320)) / 100;

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

            a -= ha;

            b -= hb;

            krs.setPos(0, a);

            krs.setPos(1, b);

            delay(1);

          }

        }

      }

      if (receivedLeftStickX > 155) {  //サブX

        e -= map(receivedLeftStickX, 155, 225, 0, 200);

      } else if (receivedLeftStickX < 100) {

        e += map(receivedLeftStickX, 100, 0, 0, 200);

      }

      if (receivedLeftStickY > 135) {  //サブY

        C610::put(2, map(receivedLeftStickY, 135, 225, 0, 6384));

      } else if (receivedLeftStickY < 120) {

        C610::put(2, map(receivedLeftStickY, 120, 0, 0, -6384));

      } else {

        C610::put(2, 0);

      }

      if (receivedUpButton == 1) {  //サブZ

        d = 9000;

      } else if (receivedDownButton == 1) {

        d = 4000;

      }

      if (receivedCircleButton == 1) {  //吸着

        digitalWrite(mos, HIGH);

      } else if (receivedSquareButton == 1) {

        digitalWrite(mos, LOW);

        if (haba == 0) {  //自動パス

          delay(250);

          j = 4850;

          krs.setPos(8, j);

          delay(750);

          j = 10205;  //戻る

          krs.setPos(8, j);

        }

      }

      if (receivedL1Button == 1) {  //パス機構

        if (pasu == 0) {            //受取状態へ

          pasu = 1;

          j = 10205;

          delay(200);

        } else if (pasu == 1) {  //パス状態へ

          pasu = 2;

          j = 4850;

          delay(200);

        } else if (pasu == 2) {  //受取状態へ

          pasu = 1;

          j = 10205;

          delay(200);

        }

      }

      if ((receivedLeftButton == 1 && g == 7500) || (receivedRightButton == 1 && g == 7500)) {  //受取角度

        d = 4000;

        krs.setPos(3, d);  //下げる

        e = 7500;

        krs.setPos(4, e);  //引く

        h = 7800;

        krs.setPos(7, h);  //浅く(角度)

        delay(250);

        g = 10200;

        krs.setPos(6, g);

        delay(250);

        e = 11500;

        krs.setPos(4, e);

      } else if ((receivedLeftButton == 1 && g == 10200) || (receivedRightButton == 1 && g == 10200)) {

        h = 8500;

        krs.setPos(7, h);  //深く(角度)

        e = 7500;

        krs.setPos(4, e);

        delay(250);

        g = 7500;

        krs.setPos(6, g);

        delay(250);

      }

      if (receivedTouchPadButton == 1) {  //置く

        krs.setPos(4, e + 1700);          //奥へ

        delay(250);

        krs.setPos(3, d + 2200);  //スライドさせる

        krs.setPos(7, h – 1500);

        delay(700);

        for (int i = 7000; i <= 7800; i += 20) {  //ゆっくり起こす

          krs.setPos(7, i);

          delay(1);

        }

        e = 7500;

        krs.setPos(4, e);

        delay(250);

        d = 4000;

        krs.setPos(3, d);  //下げる

        h = 7800;

        krs.setPos(7, h);  //浅く(角度)

        delay(250);

        g = 10200;

        krs.setPos(6, g);

        delay(250);

        e = 11500;

        krs.setPos(4, e);

      }

      if (receivedShareButton == 1) {

        h = 6500;

      }

      krs.setPos(0, a);

      krs.setPos(1, b);

      krs.setPos(2, c);

      krs.setPos(3, d);

      krs.setPos(4, e);

      krs.setPos(6, g);

      krs.setPos(7, h);

      krs.setPos(8, j);  //iはfor文で使う

      C610::send();

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

      if (receivedPSButton == 1) {  //初期状態に

        a = 7500 + 320;             //0

        b = 7500 – 320;             //1

        c = 11250;                  //2

        k = 4300;                  //8 パス

        pasu = 0;

        krs.setPos(2, c);

        unsigned long start = millis();

        while (millis() – start < 1000) {

          C610::put(1, 2500);

          //C610::put(2, -750);

          C610::send();

        }

      }

      if (receivedRightStickY > 135) {  //主Y

        C610::put(1, map(receivedRightStickY, 135, 225, 0, -10000));

      } else if (receivedRightStickY < 120) {

        C610::put(1, map(receivedRightStickY, 120, 0, 0, 10000));

      } else {

        C610::put(1, 0);

      }

      if (receivedTriangleButton == 1) {  //主Z

        taka = 1;

        c = 11250;  //上方向

      } else if (receivedCrossButton == 1) {

        if (taka == 1) {

          taka = 2;

          c = 4100;  //下方向

          delay(200);

        } else if (taka == 2) {

          c -= 100;

          delay(100);

        }

      }

      if (receivedR2Value > 225) {  //主X横左右選択 間640

        a = 7500 – 106;//-106

        b = 7500 – 734;//-734

        delay(200);

      } else if (receivedL2Value > 225) {

        a = 7500 + 616;//616

        b = 7500 – 36;//-36

        delay(200);

      } else if (receivedR2Value > 10 && a > 3500 && b > 3500) {  //主X横微調節

        a -= map(receivedR2Value, 10, 255, 1, 40);

        b -= map(receivedR2Value, 10, 255, 1, 40);

      } else if (receivedL2Value > 10 && a < 11500 && b < 11500) {

        a += map(receivedL2Value, 10, 255, 1, 40);

        b += map(receivedL2Value, 10, 255, 1, 40);

      }

      if (receivedR1Button == 1) {  //主X幅

        if (haba == 1) {            //狭める

          haba = 0;

          ha = (a – 6300) / 300;  //6500

          hb = (b – 8300) / 300;  //8500

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

            a -= ha;

            b -= hb;

            krs.setPos(0, a);

            krs.setPos(1, b);

            delay(1);

          }

        } else {  //広げる

          haba = 1;

          ha = (a – (7500 + 320)) / 100;

          hb = (b – (7500 – 320)) / 100;

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

            a -= ha;

            b -= hb;

            krs.setPos(0, a);

            krs.setPos(1, b);

            delay(1);

          }

        }

      }

      if (receivedLeftStickX > 155) {  //サブX

        e -= map(receivedLeftStickX, 155, 225, 0, 200);

      } else if (receivedLeftStickX < 100) {

        e += map(receivedLeftStickX, 100, 0, 0, 200);

      }

      if (receivedLeftStickY > 135) {  //サブY

        C610::put(2, map(receivedLeftStickY, 135, 225, 0, 6384));

      } else if (receivedLeftStickY < 120) {

        C610::put(2, map(receivedLeftStickY, 120, 0, 0, -6384));

      } else {

        C610::put(2, 0);

      }

      if (receivedUpButton == 1) {  //サブZ

        d = 9000;

      } else if (receivedDownButton == 1) {

        d = 4000;

      }

      if (receivedCircleButton == 1) {  //吸着

        digitalWrite(mos, HIGH);

      } else if (receivedSquareButton == 1) {

        digitalWrite(mos, LOW);

        if (haba == 0) {  //自動パス

          delay(250);

          k = 9800;

          krs.setPos(9, k);

          delay(750);

          k = 4550;  //戻る

          krs.setPos(9, k);

        }

      }

      if (receivedL1Button == 1) {  //パス機構

        if (pasu == 0) {            //受取状態へ

          pasu = 1;

          k = 4550;

          delay(200);

        } else if (pasu == 1) {  //パス状態へ

          pasu = 2;

          k = 9800;

          delay(200);

        } else if (pasu == 2) {  //受取状態へ

          pasu = 1;

          k = 4550;

          delay(200);

        }

      }

      if ((receivedLeftButton == 1 && g == 7500) || (receivedRightButton == 1 && g == 7500)) {  //受取角度

        d = 4000;

        krs.setPos(3, d);  //下げる

        e = 9500;

        krs.setPos(4, e);  //引く

        h = 7800;

        krs.setPos(7, h);  //浅く(角度)

        delay(250);

        g = 4900;

        krs.setPos(6, g);

        delay(250);

        e = 4600;

        krs.setPos(4, e);

      } else if ((receivedLeftButton == 1 && g == 4900) || (receivedRightButton == 1 && g == 4900)) {

        h = 8500;

        krs.setPos(7, h);  //深く(角度)

        e = 9500;

        krs.setPos(4, e);

        delay(250);

        g = 7500;

        krs.setPos(6, g);

        delay(250);

      }

      if (receivedTouchPadButton == 1) {  //置く

        krs.setPos(4, e – 1700);          //奥へ

        delay(250);

        krs.setPos(3, d + 2200);  //スライドさせる 1500よりもいい

        krs.setPos(7, h – 1500);

        delay(700);

        for (int i = 7000; i <= 7800; i += 20) {  //ゆっくり起こす

          krs.setPos(7, i);

          delay(1);

        }

        e = 9500;

        krs.setPos(4, e);

        delay(250);//ここまで置く動作

        d = 4000;

        krs.setPos(3, d);  //下げる

        e = 9500;

        krs.setPos(4, e);  //引く

        h = 7800;

        krs.setPos(7, h);  //浅く(角度)

        delay(250);

        g = 4900;

        krs.setPos(6, g);

        delay(250);

        e = 4600;

        krs.setPos(4, e);

      }

      if (receivedShareButton == 1) {

        h = 6500;

      }

      krs.setPos(0, a);

      krs.setPos(1, b);

      krs.setPos(2, c);

      krs.setPos(3, d);

      krs.setPos(4, e);

      krs.setPos(6, g);

      krs.setPos(7, h);

      krs.setPos(9, k);

      C610::send();

    }

  }

  displayReceivedData();

}

config.h

int oldreceivedLeftStickY;

int oldreceivedLeftStickX;

bool receivedRightButton;

bool receivedDownButton;

bool receivedUpButton;

bool receivedLeftButton;

bool receivedSquareButton;

bool receivedCrossButton;

bool receivedCircleButton;

bool receivedTriangleButton;

bool receivedL1Button;

bool receivedR1Button;

bool receivedShareButton;

bool receivedOptionsButton;

bool receivedL3Button;

bool receivedR3Button;

bool receivedPSButton;

bool receivedTouchPadButton;

int receivedL2Value;

int receivedR2Value;

int receivedLeftStickX;

int receivedLeftStickY;

int receivedRightStickX;

int receivedRightStickY;

volatile int receivedHou;

volatile int receivedCustom;

volatile bool dataReceived = false;

displayReceivedData.h

void displayReceivedData() {

  // 受信したデータをシリアルモニターに表示

  /*Serial.print(” Buttons: “);

  Serial.print(“→: “);

  Serial.print(receivedRightButton);

  Serial.print(” ↓: “);

  Serial.print(receivedDownButton);

  Serial.print(” ↑: “);

  Serial.print(receivedUpButton);

  Serial.print(” ←: “);

  Serial.print(receivedLeftButton);

  Serial.print(” □: “);

  Serial.print(receivedSquareButton);

  Serial.print(” ✕: “);

  Serial.print(receivedCrossButton);

  Serial.print(” ❍: “);

  Serial.print(receivedCircleButton);

  Serial.print(” △: “);

  Serial.print(receivedTriangleButton);

  Serial.print(” L1: “);

  Serial.print(receivedL1Button);

  Serial.print(” R1: “);

  Serial.print(receivedR1Button);

  Serial.print(” Share: “);

  Serial.print(receivedShareButton);

  Serial.print(” Options: “);

  Serial.print(receivedOptionsButton);

  Serial.print(” L3: “);

  Serial.print(receivedL3Button);

  Serial.print(” R3: “);

  Serial.print(receivedR3Button);

  Serial.print(” PS: “);

  Serial.print(receivedPSButton);

  Serial.print(” TouchPad: “);

  Serial.print(receivedTouchPadButton);*/

  //—————————————————————————————

  if (receivedLeftStickY == 255) {  //変換(?)

    if (oldreceivedLeftStickY <= 125) {

      receivedLeftStickY = 0;

    }

  }

  oldreceivedLeftStickY = receivedLeftStickY;

  if (receivedLeftStickX == 255) {

    if (oldreceivedLeftStickX <= 125) {

      receivedLeftStickX = 0;

    }

  }

  oldreceivedLeftStickX = receivedLeftStickX;

  /*Serial.print(“L2Value: “);

  Serial.print(receivedL2Value);

  Serial.print(” R2Value: “);

  Serial.print(receivedR2Value);

  Serial.print(“左X: “);

  Serial.print(receivedLeftStickX);

  Serial.print(” 左Y: “);

  Serial.print(receivedLeftStickY);

  Serial.print(” 右X: “);

  Serial.print(receivedRightStickX);

  Serial.print(” 右Y: “);

  Serial.print(receivedRightStickY);

  Serial.print(” 方向: “);

  Serial.print(receivedHou);

  Serial.print(” カスタム: “);

  Serial.println(receivedCustom);*/

  /*Serial.print(” ID1:”);

  Serial.print(” 角度”);

  Serial.print(C610::getAngle(1));

  Serial.print(” 速度”);

  Serial.print(C610::getVelocity(1));

  Serial.print(” ID2:”);

  Serial.print(” 角度”);

  Serial.print(C610::getAngle(2));

  Serial.print(” 速度”);

  Serial.print(C610::getVelocity(2));

  Serial.println(” “);*/

}

receiveEvent.h

void receiveEvent() {

  if (Wire.available() >= 24) {  // 16ボタン + 4スティック + 2トリガーボタンのデータ+2自由値

    // ボタンの状態を変数に代入

    receivedRightButton = Wire.read() == 1;

    receivedDownButton = Wire.read() == 1;

    receivedUpButton = Wire.read() == 1;

    receivedLeftButton = Wire.read() == 1;

    receivedSquareButton = Wire.read() == 1;

    receivedCrossButton = Wire.read() == 1;

    receivedCircleButton = Wire.read() == 1;

    receivedTriangleButton = Wire.read() == 1;

    receivedL1Button = Wire.read() == 1;

    receivedR1Button = Wire.read() == 1;

    receivedShareButton = Wire.read() == 1;

    receivedOptionsButton = Wire.read() == 1;

    receivedL3Button = Wire.read() == 1;

    receivedR3Button = Wire.read() == 1;

    receivedPSButton = Wire.read() == 1;

    receivedTouchPadButton = Wire.read() == 1;

    // トリガーボタンのアナログ値を変数に代入

    receivedL2Value = Wire.read();

    receivedR2Value = Wire.read();

    // スティックの値を変数に代入

    receivedLeftStickX = Wire.read();

    receivedLeftStickY = Wire.read();

    receivedRightStickX = Wire.read();

    receivedRightStickY = Wire.read();

    //自由値を変数に代入

    receivedHou = Wire.read() | (Wire.read() << 8);

    receivedCustom = Wire.read() | (Wire.read() << 8);

    dataReceived = true;//受信フラグ

  }

}

robomas.cpp

// robomas.cpp

#include “robomas.h”

#define PI 3.14

MCP_CAN CAN0(SS_Robomas_C610);

uint16_t C610::current_max[8] = {16384, 16384, 16384, 16384, 16384, 16384, 16384, 16384};

uint8_t C610::buf_0x200[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

uint8_t C610::buf_0x1FF[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

int C610::received_angle[8] = {0, 0, 0, 0, 0, 0, 0, 0};

int16_t C610::received_velocity[8] = {0, 0, 0, 0, 0, 0, 0, 0};

int16_t C610::received_torque_current[8] = {0, 0, 0, 0, 0, 0, 0, 0};

// — 追加: 累積位置用変数 —

static long encoder_pos[8] = {0};

static int last_angle[8]   = {0};

void C610::begin()

{

    CAN0.begin(MCP_ANY, CAN_bps, MCP_OSC);

    if (CAN0.begin(MCP_ANY, CAN_bps, MCP_OSC) == CAN_OK)

    {

        //Serial.println(“Success : MCP2515 Initialization”);

    }

    else

    {

        //Serial.println(“Fail : MCP2515 Initialization”);

    }

    delay(100);

    CAN0.setMode(MCP_NORMAL);

}

void C610::setMaxCurrent(unsigned int C610_id, uint16_t max)

{

    if (1 <= C610_id && C610_id <= 8 && 0 <= max && max <= 10000)

    {

        C610::current_max[C610_id – 1] = max;

        return;

    }

    //Serial.println(“Fail : id or max is out of range.”);

}

void C610::put(unsigned int C610_id, double current)

{

    if (abs(current) > current_max[C610_id – 1])

    {

        //Serial.println(“Fail :  current value is out of range.”);

        return;

    }

    int16_t converted = static_cast<int16_t>(current);

    uint8_t high_byte = (converted >> 8) & 0xFF;

    uint8_t low_byte = converted & 0xFF;

    if (1 <= C610_id && C610_id <= 4)

    {

        buf_0x200[(C610_id – 1) * 2] = high_byte;

        buf_0x200[(C610_id – 1) * 2 + 1] = low_byte;

        return;

    }

    if (5 <= C610_id && C610_id <= 8)

    {

        buf_0x1FF[(C610_id – 5) * 2] = high_byte;

        buf_0x1FF[(C610_id – 5) * 2 + 1] = low_byte;

        return;

    }

    //Serial.println(“Fail :  C610_id is out of range.”);

}

void C610::send()

{

    CAN0.sendMsgBuf(0x200, 0, 8, buf_0x200);

    CAN0.sendMsgBuf(0x1FF, 0, 8, buf_0x1FF);

}

void C610::print(bool hex_flag)

{

    for (int i = 1; i < 9; i++)

    {

        int16_t value;

        uint8_t high_byte;

        uint8_t low_byte;

        if (1 <= i && i <= 4)

        {

            high_byte = buf_0x200[(i – 1) * 2];

            low_byte = buf_0x200[(i – 1) * 2 + 1];

        }

        if (5 <= i && i <= 8)

        {

            high_byte = buf_0x1FF[(i – 5) * 2];

            low_byte = buf_0x1FF[(i – 5) * 2 + 1];

        }

        value = (high_byte << 8) + low_byte;

        //if (hex_flag)

            //Serial.print(value, HEX);

        //else

            //Serial.print(value);

        //Serial.print(” “);

    }

    //Serial.println();

}

void C610::receive()

{

    if (CAN_MSGAVAIL == CAN0.checkReceive())

    {

        for (int i = 1; i < 9; i++)

        {

            uint8_t len = 0;

            uint8_t data[8];

            unsigned long int rx_message;

            CAN0.readMsgBuf(&rx_message, &len, data);

            if (rx_message == 0x200 + i)

            {

                received_angle[i – 1] = (data[0] << 8) + data[1];

                received_velocity[i – 1] = (data[2] << 8) + data[3];

                received_torque_current[i – 1] = (data[4] << 8) + data[5];

            }

        }

    }

}

double C610::getAngle(unsigned int C610_id)

{

    if (C610_id < 1 || 8 < C610_id)

    {

        //Serial.print(“Fail : id is out of range.”);

        return -1;

    }

    double angle = (double)received_angle[C610_id – 1] / 8191 * 2 * PI;

    return angle;

}

int16_t C610::getVelocity(unsigned int C610_id)

{

    if (C610_id < 1 || 8 < C610_id)

    {

        //Serial.print(“Fail : id is out of range.”);

        return -1;

    }

    return received_velocity[C610_id – 1];

}

int16_t C610::getTorqueCurrent(unsigned int C610_id)

{

    if (C610_id < 1 || 8 < C610_id)

    {

        //Serial.print(“Fail : id is out of range.”);

        return -1;

    }

    return received_torque_current[C610_id – 1];

}

robomas.h

//robomas.h

//C610_引数のidはC610に割り振られたidであることに注意。(1から始まる)

#ifndef ROBOMAS_H

#define ROBOMAS_H

#include <SPI.h>

#include <mcp_can.h>

#define SS_Robomas_C610 10 // SPI通信に用いるピン

extern MCP_CAN CAN0;

const int CAN_bps = CAN_1000KBPS; // 変更可

const int MCP_OSC = MCP_8MHZ;     // 回路上の発振子と合わせる

class C610

{

public:

    // 動作電流値の最大値のデータ

    // これを超える電流値を流そうとしても反映されない。

    static uint16_t current_max[8]; // = {10000, 10000, 10000, 10000, 10000, 10000, 10000, 10000};

    // C610に送信するデータ

    static uint8_t buf_0x200[8]; // = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

    static uint8_t buf_0x1FF[8]; // = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

    // 受け取ったモーターの角度を格納する変数

    // 格納されているのは0 ~ 8191の範囲の整数でそれぞれ0 ~ 2π radに対応している。

    static int received_angle[8]; // = {0, 0, 0, 0, 0, 0, 0, 0};

    // 受け取ったモーターの速度を格納する変数

    // 単位は rpm

    static int16_t received_velocity[8]; // = {0, 0, 0, 0, 0, 0, 0, 0};

    // 受け取ったモーターの実際のトルク電流を格納する変数

    // 単位は mA

    static int16_t received_torque_current[8]; // = {0, 0, 0, 0, 0, 0, 0, 0};

    // 初期化関数

    static void begin();

    // 電流値の最大値を設定する。

    static void setMaxCurrent(unsigned int C610_id, uint16_t max);

    // idに対して動作電流値を設定する。

    // 電流値の範囲は-10000mA~10000mA

    // PID制御しやすいようにdouble型を引数としているが16bitの整数に変換されるため小数点以下は反映されない。

    static void put(unsigned int C610_id, double current);

    // CAN通信でデータを送信。

    static void send();

    // 指示している電流値を読み取る。

    // 引数をtrueにすると16進数4桁で表示される。

    // falseにすると10進数で表示される。

    static void print(bool hex_flag = true);

    // C610からデータを受け取る

    static void receive();

    // モーターの角度を取得

    // 機械角度範囲:0~2π

    // 単位 rad

    static double getAngle(unsigned int C610_id);

    // モーター回転速度を取得

    // 単位 rpm

    static int16_t getVelocity(unsigned int C610_id);

    // 実際のトルク電流を取得

    // 単位 mA

    static int16_t getTorqueCurrent(unsigned int C610_id);

};

#endif

操縦がとても大変でした。

敗因

いままでに比べて、本当に些細なことで負けた気がします。

青コートの時、サブアームのY軸を駆動するモーターが干渉してしまっていました。
これにより10mm左に機体を設置することとなり、サブアームのX軸可動域が不足してしまいました。

CADをもっと作りこむか、練習環境を再現できていれば…

予選2試合目にサブアームの歯車がメインアームの配線を絡めとり引き抜かれました。
抜けた配線は差すだけで復旧できる状態でしたが、メンテナンス性を考慮していない設計により、その場での修正が困難でなすすべなく3分間を終えました。

まとめ

ロボットの性能を十分に発揮してやれなかったことが、ロボットに対して申し訳ないと思ってます。
悔しすぎる…

とは言いつつ、特別枠で紹介いただけたのでとても嬉しかったです。

来年は本気で優勝を目指します。

それでは、またいつか。

]]>
高級モーター開拓記 /%e9%ab%98%e7%b4%9a%e3%83%a2%e3%83%bc%e3%82%bf%e3%83%bc%e9%96%8b%e6%8b%93%e8%a8%98/ /%e9%ab%98%e7%b4%9a%e3%83%a2%e3%83%bc%e3%82%bf%e3%83%bc%e9%96%8b%e6%8b%93%e8%a8%98/#respond Wed, 07 Oct 2026 00:00:00 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=4475

以前、「買ってよかったもの」というテーマで執筆した記事でロボマスモーターを紹介しました。

しかし、こちらでご紹介したロボマスモーターは導入から数ヵ月後に在庫切れとなり、そのまま補充されず入手困難となっています。

せっかく使いやすくて重宝していたのに…

ということで、新しいモーターを開拓してみます。

(学生広報と称して関係のない内容を記事にしていますが、記事の母数が増えるのは良いことなので許してもらえるはず…)

スペック

今回、導入するモーターはこちら!!!

ゴツくてカッコイイ!

そこそこにずっしりしてます。

分厚い装甲に覆われているようで、多少雑に扱っても壊れなさそうです

モーターは「NEO Brushless Motor V1.1」

スペックはサイト内に記載されている通りなのですが、注目すべき点をピックアップしておきます
・重量 0.425kg
・電圧 12 V
・電流(ピーク) 105 A
・トルク 2.6 Nm
・回転数 5676 RPM
・軸 φ8(キー溝)
・取り付け穴 #10-32 

トルクは少し低めですが、圧倒的な回転数でカバーできそうです。
12V駆動というのが少々トレンドから外れていますね
最大の落とし穴は取り付け穴の規格です。

制御回路は「SPARK MAX Motor Controller」

・対象 ブラシ付き、ブラシレスDCモーター
・電圧 5.5V – 24V
・電流(連続) 60A
・電流(瞬間) 100A(2秒間)
・制御方式 PWM,CAN,USB

ブラシ付き、ブラシレス両方いけるドライバって珍しくないですか!?
定格電力的にもほとんどのモーターは問題なく使えそうです(735モーターは無理か…)
制御方式はPWMだと確実に動きますが、制御回路内でのPID制御を発動できないので基本的にはCAN通信を使うことになりそうです。

使い方(機構)

モーターの取り付けネジは#10-32です。

これはインチ規格ではなくユニファイ規格です。

アメリカンですねぇー

一応、M5タップを無理やり掘って固定できるそうですが、私はこの手法を信用していません。

頑張って、ネジ専門店で購入しました。

(専門店でも在庫が少なく、希望の長さがなかったため旋盤で短くしました)

軸関連は純正のアクセサリーを購入すると楽で確実です。

純正品をケチった場合の固定法を軽く説明します。

軸は∅8で2mmのキー溝が掘ってあります。
手元にキー溝のある歯車がなかったので∅2のエンドミルで追加工しています。
キーには2mm厚のジュラルミン板を突っ込みました。
軸の先端に#10-32規格でタップが掘ってあるのでこの穴を活用して抜け止めします。
更にイモネジで上下の動きを拘束して完成です。

大変でした…

使い方(回路)

まず、モーターの線や制御回路側の線にコネクタをつけます。

純正だと高いので今回は適当なバナナプラグを付けました。

電源線にはXT60コネクタを付けましたが定格を考慮すると微妙な気がします。
バッテリー側の放電能力から我々の環境では問題なさそうです。

モーターから出ている信号線は確実に制御回路側につなげましょう。
押し込みすぎると接触不良になります。
線が抜けるとモーターが焦げ付くらしいので気を付けましょう。

CANで動かす場合は終端抵抗510Ωくらいを探ってみてください

ちなみに、モーターから出ている信号線のうち、黄色と青色がエンコーダの線になっています。

黒色の線(GND)と合わせて分岐することで直接的な回転数を読み取れますが分解能が低い上、電源を投入した後に有効になる仕様だったのでおすすめしません。

使い方(プログラム)

ここが一番厄介です。

まず、制御回路の専用ソフトとプログラムの内容を一致させる必要があります。

専用ソフトのダウンロードページはこちら。
Installation | REV Hardware Client | REV Robotics Documentation

起動するとこのような画面になります。

USBケーブルでPCとSPARK MAX Motor Controllerをつなぐと以下の表示になります。
このときCAN通信の線にマイコンなどを繋いでいると認識されない場合があります。

表示された機器をクリックすると、メイン画面が開きます。
ここでは電流保護の値からリミットスイッチの有無、警告などを設定・確認することができます。

最も注意すべき点は、「ブラシレスモーターをブラシ付きモーターのモードで使わないようにすること」です。
壊れるらしい。

Runのタブでは速度制御や位置制御を実行して動作確認をすることができます。
結線や設定に問題のないことを確認したら、動かしてみましょう。
ここで動けば第一関門突破です。

私の環境ではモーターの信号線を制御回路に接続する部分で押し込みすぎて接触不良が発生しました。

↓専用ソフトから位置制御したときの様子です。

めっちゃ速く1回転を繰り返しています。
感動しちゃいますね

1回転を1、半回転を0.5と表現されていますが0.01単位でも余裕そうです。

スペック上の表記ではエンコーダが42分割と書かれていましたが、位置制御にこのエンコーダは使っていないのでしょうか?
とにかく期待以上でよかった。

↓マイコンからPWM信号で動かしてみました。(デューティー比は1000~ 2000です)

問題なく動きました。

プログラム

#include <M5Stack.h>

#include <ESP32Servo.h>

Servo sparkMax;

void setup() {

  sparkMax.attach(26, 1000, 2000);

  sparkMax.writeMicroseconds(1500);  // 初期停止

  delay(3000);

}

void loop() {

  /*

  sparkMax.writeMicroseconds(1500);//停止

  sparkMax.writeMicroseconds(1000);//正転(最大)

  sparkMax.writeMicroseconds(2000);//逆転(最大)

  */

  sparkMax.writeMicroseconds(1400);

  delay(500);

  sparkMax.writeMicroseconds(1500);

  delay(500);

  sparkMax.writeMicroseconds(1600);

  delay(500);

  sparkMax.writeMicroseconds(1500);

  delay(500);

}

CAN通信で動かす資料が少なく、実例も見当たらなかったのでエンコーダの線を分岐してマイコンから直接PID制御をかけてみることにしました。

正直、実用レベルにはなりませんでした。

原因は
・エンコーダの分解能が足りない
・PWM制御に対する応答性が良くない
・正逆転の性能が異なる
あたりだと思います

↓自作の位置制御が一番うまくいったときの様子です
減速比が大きければ使えなくもない…?レベルでした。

せっかく内部でPID制御を効かせる機能があるのに使わないのはもったいないですよね
頑張って使えるようにする方法を見つけました。

参考:https://www.chiefdelphi.com/t/non-frc-spark-max-can-control/468856

上記のサイト内にあるGoogleドライブにあるコードの一部を編集して使いました。
ジョセフさんありがとうございます。
1週間を費やして調べてみたところ、この情報以外見つかりませんでした。

マイコンのCANからSparkMaxを動かすには、まずファームウェアのバージョンを落とします。
1.5.0くらいだったと思います。
忘れたので動かなければ総当たりしてみてください。

モーターの制御方式とプログラム上の制御方式を一致させます
↓こちらから設定します。今回は単純な位置制御を選択しました。

以上の内容を設定し終えたらマイコンのコードを書いて実行するだけです。

マイコンはESP32-wroom

CANトランシーバーはSN65HVD230を使いました

コード(メイン)

#include “driver/twai.h”        //v1.0.1

void setup() {

  // put your setup code here, to run once:

  init_can();  //SparkMax初期化

}

void loop() {

  // put your main code here, to run repeatedly:

  SparkMax(1.00);

}

SparkMax.ino

#include “driver/twai.h”

unsigned long lastSendTime = 0; // 20ms送信タイマー

void init_can() { // CAN初期化

  twai_general_config_t g_cfg = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_17, GPIO_NUM_16, TWAI_MODE_NORMAL);

  twai_timing_config_t t_cfg = TWAI_TIMING_CONFIG_1MBITS();

  twai_filter_config_t f_cfg = TWAI_FILTER_CONFIG_ACCEPT_ALL();

  twai_driver_install(&g_cfg, &t_cfg, &f_cfg);

  twai_start();

}

void send_can(uint32_t id, const void* data) { // CAN送信

  twai_message_t msg = {.extd = 1, .identifier = id, .data_length_code = 8};

  if (data) memcpy(msg.data, data, 8);

  twai_transmit(&msg, 0);

}

void SparkMax(float setpoint) { // 復帰確認 & SPARK MAX送信 (20ms周期)

  unsigned long now = millis();

  if (now – lastSendTime < 20) return; // 20ms経過していなければ抜ける

  lastSendTime = now;

  twai_status_info_t s;

  twai_get_status_info(&s);

  if (s.state == TWAI_STATE_BUS_OFF) twai_initiate_recovery(), twai_start(); // Bus-Off自動復帰

  uint8_t hb[8] = {255, 255, 255, 255, 255, 255, 255, 255};

  send_can(0x02052C80, hb); // Heartbeat送信

  uint8_t ctrl[8] = {0};

  memcpy(ctrl, &setpoint, sizeof(float));

  send_can(0x02050C80 | (1 & 0x3F), ctrl); // Position_Set送信 (DeviceID: 1)

}

メインのコードは紛失したのでうろ覚えで書きました
関数の方は実際に大会で使っていた内容です。

定期的にCANでデータを送らないと動かない仕様で20msおきに送信するのが正解っぽいです
ハートビートとか言うらしい。かっこよ

↓マイコンから制御している様子です。

すこしクセの強いモーターですが、いざ扱えるようになると、ものすごく便利です。

他の制御方式も同じように実行できると思います。

動かない場合は以下を疑ってみてください
・制御方式の不一致
・終端抵抗(総当たりする覚悟で)
・ファームウェアのバージョン

購入

私はキャチロボの部品支給を活用して入手しました。

モーターは海外の物ですが、日本では代理店があるため簡単に購入できます。

NEO Brushless Motor V1.1 ¥8,550
NEO Brushless Motor V1.1 – Redigiform

SPARK MAX Motor Controller ¥16,200
SPARK MAX Motor Controller – Redigiform

こうみると、めっちゃ高いわけでもないですねー
3万くらいの感覚でいましたが2万ちょいで買えるじゃないですか

ロボマスの代替品としていかがでしょうか。

]]>
/%e9%ab%98%e7%b4%9a%e3%83%a2%e3%83%bc%e3%82%bf%e3%83%bc%e9%96%8b%e6%8b%93%e8%a8%98/feed/ 0
弱小校(?)が学ロボに出場した方法 /%e5%bc%b1%e5%b0%8f%e6%a0%a1%e3%81%8c%e5%ad%a6%e3%83%ad%e3%83%9c%e3%81%ab%e5%87%ba%e5%a0%b4%e3%81%97%e3%81%9f%e6%96%b9%e6%b3%95/ /%e5%bc%b1%e5%b0%8f%e6%a0%a1%e3%81%8c%e5%ad%a6%e3%83%ad%e3%83%9c%e3%81%ab%e5%87%ba%e5%a0%b4%e3%81%97%e3%81%9f%e6%96%b9%e6%b3%95/#respond Fri, 25 Sep 2026 00:00:00 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=4022

20年ぶりの挑戦で出場できた件について、よく聞かれるので、まとめ的な内容を書きます。

※私は20年前を知りません。

ロボット紹介は下記の記事よりご覧ください。

Q.なぜ出場できた?

A.わかりません

審査結果に対して理由が公開されないことから不明です。

言っていいのか分かりませんが一次審査直後から取材がありました。

「注目校」枠か「新参校」枠っぽさはありました。

一次審査時のR1
一次審査時のR2

実際、二次審査では撮影で通しの実施ができず、継ぎ接ぎ状態で提出しました。

Q.マネジメントはどのように?

A.人に期待しない

そもそもエントリー当初、一人で挑む予定でした。

しかし思いのほか手伝ってくれるメンバーが多かったため分担できました。

基本的には「仕事ふる」→「仮完成状態で納品してもらう」→「自分で最終調整」といった流れでした。

もちろん例外もあります。

仕事をふっても、してくれなかったり拒否されたり、逆に組込みまで仕上げてくれたり…

ただ、全てにおいて最終調整の責任を自分にすることで問題が発生してもカバーできました。

Q.ノウハウはどこから?

A.工業高校でのロボコン経験と4つのロボコン

高校ロボコンは、コロナ明けで、かつ先生が入れ替わった時期に始めました。

この時に、「試行錯誤とは何なのか」を学びました。

最終的に、とりあえずロボットを作れる程度の技術を持って高校を卒業できました。

大学(前半)では多くのロボコンに挑戦しました。

九州夏ロボコン、キャチロボ、トマトロボット、学ロボの4つです。

場数を踏むと、何が強いのか、どうやって実現するのか、といったノウハウが得られます。

たくさんの敗北と少しの勝利を経てノウハウを作りました。

Q.メンバー集めはどうやって?

A.入部ハードルを下げた

前提として、まず知ってもらわなければなりません。

サークル勧誘の片隅で勧誘してみたり、ポスター作ったり、各種SNSをしてみたりと、できること全てしました。

入学直後に作ったポスター

次に、興味を持ってくれた人をできる限り引き込みます。

定例会を設けず、部費徴収をせず、簡単に脱退できる旨を伝えます。

入部してくれる人がデメリットに感じそうな物を極限まで排除しました。

そして、とにかく制限を減らしました。

ゲーム禁止、飲食禁止など、他の場所に行く理由になりそうなルールを設けていません。

結果、加工機械を使っている中でサッカーが始まるというカオスも発生しました(さすがに注意しました)が、何とか集団として成り立っています。

アレコレしたら部員が50人を突破しました。

働きアリの法則的に考えると母数は多ければ多い方が良いですよね。

Q.活動時間はどれくらい?

A.使える時間全て使いました

だいたい活動場所には、危険な工作機械があるため教職員がいるときでないと作業できないという体制の団体が多いのではないでしょうか。

ロボット工房では良くも悪くもそのような規則はありません。

よって、土日祝日朝から晩、気が済むまで作業できます。(入試関連の日を除く)

基本的には授業の空きコマと放課後(21時前くらいまで)、さらに土日も活動時間に当てました。

もちろん強制はしなかったので私以外のメンバーが工房にいる時間は、体感で半分以下でした。

長期休暇もほとんど毎日学校に行く日々でしたが、年始年末は暖房が効かなくなるのでアルミを加工する手が震えて大変でした。そんな時期に加工が発生するのは段取りの問題なんですけどね

今振り返ると1日でも欠けたら、あのロボットはできてなかったです。

とまぁ、このような状況だったのでK*RISE(このサイトの運営団体)の定例会に一切参加できませんでした、ごめんなさい。
たぶん絶対これからも同じ状況なので、許されるならリモートワーク的な形でこれからも続けていけたらいいな…と。

Q.資金は?

A.環境に恵まれていました

もともとヒューマノイドをしていた団体なので、良いサーボモーターが沢山あります。

また、過去のヒューマノイドの実績から予算もついていました。

さらに研究室と併設されているため過去に研究で使用した物を譲っていただけます。

譲っていただいたDepthカメラ3つ

とは言いつつ、周辺のロボコン団体と比べると予算額は同等か、それ以下みたいです。

夏ロボの優勝賞品が無ければ今回は間違いなく詰んでました。

10万円分のモーター

自腹で購入した物品もいくつかあります。

まとめ

いかがでしたか?

全体的に士気の向上もみられるので今後に期待したいですね。

質問など、九産大ロボット工房のXかインスタでDMしていただけると回答できます。

文字ばかりの記事になってしまいましたが、ここまで読んでくださりありがとうございます。

それでは、またいつか。

]]>
/%e5%bc%b1%e5%b0%8f%e6%a0%a1%e3%81%8c%e5%ad%a6%e3%83%ad%e3%83%9c%e3%81%ab%e5%87%ba%e5%a0%b4%e3%81%97%e3%81%9f%e6%96%b9%e6%b3%95/feed/ 0
NHK学生ロボコン2026ロボット紹介 /nhk%e5%ad%a6%e7%94%9f%e3%83%ad%e3%83%9c%e3%82%b3%e3%83%b32026%e3%83%ad%e3%83%9c%e3%83%83%e3%83%88%e7%b4%b9%e4%bb%8b/ /nhk%e5%ad%a6%e7%94%9f%e3%83%ad%e3%83%9c%e3%82%b3%e3%83%b32026%e3%83%ad%e3%83%9c%e3%83%83%e3%83%88%e7%b4%b9%e4%bb%8b/#respond Fri, 28 Aug 2026 00:00:00 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=3968

大変長らくお待たせいたしました。

スマホ故障で情報社会から隔離されていました、、、
という言い訳が効かないほど日数が経ってしまいました、ぶちょーです。

今回はNHK学生ロボコン2026の紹介をします。

ルール

テーマは「カンフークエスト」

タスクは大きく3つです。

①武器を組立てる

  ロボット2台で協力して組み立てます
  許容誤差は1mm程度です

  ②ブック(箱)回収

  大きめの箱を回収します
  回収の可否が模様によって定められています

  ③ブック(箱)設置

  棚に箱を置きます。
  縦か斜めに3つ並べた時点で勝利が確定します
  相手の箱を①で組み立てた武器で落とせます

  詳しいルールはコチラからご覧ください。

戦績

予選一試合目:敗北
(九州産業大学vs豊橋技術科学)

予選二試合目:勝利
(九州産業大学vs大阪工業大学)

 →予選敗退

ライブ配信(YouTube)

R1機構

大会出場時のR1
一次審査の補足資料
二次審査の補足資料

ブック(箱)を置く動作と武器で突き落とす動作は同一の面でできた方が良いだろうという話になり、トンボ型の機体になりました。

ブックを1つしか回収できないのも、これの弊害です。

↑インホイール式独立ステアリング機構※私の設計ではありません
R2を持ち上げるときの荷重にメカナムやオムニは耐えられないかも、と考え独ステになりました。
ステアリングが遅すぎました。(サーボ)
タイヤのグリップはダイソーで買える「犬のおもちゃ」ですが、劣化すると割れが発生してしまいました。

↑ポール回収ハンド※私の設計ではありません
蝶番部でポールを押し当てて掴みます。


↑武器組立て機構
空気圧シリンダーで瞬間的に大きな力を与えることで許容誤差が大きくとれました。
後付けなのでサーボモーターで機構ごと避ける動作をしないとポール回収直後に干渉します。

↑ブック回収機構
サーボモーターでワイヤーを引っ張り保持します。
サーボのトルクリミッターが効いてしまったり電源供給が不足したりと、安定性に欠ける仕様でした。
こういう保持には空気圧シリンダーが向いていそうですね。
M2006で電流制御してもよかったのか?
グリップにはダイソーの軍手を使ってます。

↑Z軸昇降機構
下部にあるプーリーを巻き取ると動滑車を通じてワイヤーで全体が持ち上がります。
左右のバーでR2を持ち上げるためハイトルク仕様。
下降はワイヤーを緩めることで自重を活用して落下する仕組みです。このため定荷重バネで吊ることができませんでした。おとなしくタイミングベルトかチェーンを使った方がよさそうです。

↑コントローラー
スマホを搭載できるようにしていましたが使いませんでした。
1試合目で無線トラブルが発生したので来年はルーターを背負いたいところです。

R2機構

大会出場時のR2
一次審査の補足資料
二次審査の補足資料

段差の登り降りが履帯だと速いんじゃね?
という発想から、このような形になりました。

ぜんぜん速くなかったです。

ただ、R1に持ち上げてもらう都合上、軽い方が良いのは確かで、軽量化という第2の目標は達成されています。

R2単体の重量は10.8kg

横移動を捨てたのは致命的でした

↑ヘッド回収ハンド※私の設計ではありません。
ヘッドは奪い合いが発生するため、横移動のできない本機には2本のアームを設けており、すぐに2つ目を再試行することができます。
2試合目で2つのヘッドを回収してしまったのは、アーム根元のサーボが不調で、中途半端に掴んだヘッドがセンサーの判定に入らなかったためです。

↑ヘッド回収アーム
サーボで根本から動かす単純な機構ですが、武器組み立て時の衝撃を受けるため、角パイプの末端が接触するようになっています。
サーボ軸に大きな引っ張り荷重が加わることになりますが壊れませんでした。よかった。

↑ブック回収ハンド
R1とほぼ同じ設計です。
2関節のアームでアプローチします。

↑Depthカメラ
前方と左右に1つずつ、合計3つ搭載しています。
贅沢な構成ですね。

機体中央の謎空間には、お守りが取り付けられています。
ただをこねるR2
段差を登る時の成功率は80%くらいでした。
(本番会場では100%。段の角と地面のグリップが重要)

回路

ローム株式会社様、コアスタッフオンライン様、電子部品を提供いただいたにも関わらず、ロボットに組み込めず、大変申し訳ございません。

全て秋月電子やAmazonでポチッた既製品で構成しています。

(試作では提供いただいたチップLEDやチップ抵抗を使いました)

では、電源付近からご紹介します。

マキタの18vバッテリー
満充電で20vを超えるため24vとみなして運用しています。
マキタの10.8vバッテリー
満充電で12vを超えるため12vとみなして運用しています。
ヒューズ(120a)
12v、24v系統に1つずつ設けています
この前、短絡させてしまいましたが切れませんでした
選定ミスったみたいです
リレー
緊急停止ボタンのスイッチングで動作します。
各バッテリーに対して1つずつ設けています。

次に制御系です。

メインの制御はESP32を1台だけで運用しています。
ラズパイやM5stackはセンサー的な役割です。

近藤科学のサーボはICS変換基板を使わない方式です。
PWMピンが数珠つなぎになったイメージで非常に配線しやすかったです。
R1は配線距離が長くなり、動作不良が頻発してしまいました。
配線距離は規格の範囲内にすべきですねー

画像認識

3台のRealSense(深度カメラ)を使いました。考えられる中で最大級に豪華な構成なんじゃないですかね

Raspberry Pi 5 1台だけで処理しています。
カメラで前と左右をリアルタイム検出、判別をしています。
実はUSBの帯域が不足しており2回に1回ほど映像データの取得に失敗します。
しかたがないので取得に失敗したときには前回の映像を表示するようにしました。
フレームレートは8程度だと思います。

ソフト

基本的にはArduinoIDEで開発しています。
大したことはしていないので、とりあえず張り付けておきます。

R1のプログラム

2026_R1_test77.ino

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

 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;                                                               //青コート1、赤コート2

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);  //干渉回避

  }

}

set.ino

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);

}

PS4doujyou.ino

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軸戻る時間

  }

}

PS4main.ino

//—–メイン操作関数—–
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]);
}

Select.io

int pickupPoint = 0;

void Select() {

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

    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++;  // 4の倍数でなければ右にいける

  }

  // — 左移動 —

  if (nowLeft && !lastLeft) {

    if (pickupPoint % 4 != 1) pickupPoint–;  // 4で割って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)

  }

}

StartPosition.ino

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);

}

getBox.ino

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

箱回収

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

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;

}

moveTo.ino

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

オドメトリ(進行方向で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);

  }

}

turnTo.ino

void turnTo(double targetAngle) {

  // — 1. サーボを旋回用の「ハの字」に変形(指令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;  // オドメトリ用の保持角度を更新して終了

}

weaponCatch.ino

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

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);  //シリンダーセット

  }

}

R2のプログラム

2026_R2_test88

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

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();  //アリーナ

}

set.ino

/*———

初期設定

———–*/

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);

}

StartPosition.ino

/*———————-

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

————————*/

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);  //早すぎるとバッファが飽和してフリーズする

  }

}

moveTo.ino

/*———

移動(台形速度制御)

———–*/

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.ino

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

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);

}

weapon_catch.ino

/——— 道場 ———–/
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);
}

baikarinn.ino

/*———

梅花林エリア

———–*/

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();  //画像認識データ更新(念のため2回)

    readUARTData();

    //【特例対応】

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

      if (analogRead(dis_PIN_L) > 1400) {

        stableYaw = 0;

        getBox_flat();  //ブックある

      } else {          //ブックない

        moveTo(-100);

        i++;

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

        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ブックあり && 保持1以下 && 前奥反応なし && 左レーンじゃない)

    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ブックあり && 保持1以下 && 前奥反応なし && 右レーンじゃない)

    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);

  }

}

up.ino

/*———

段差登る

———–*/

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関数が終了

down.ino

/*———

段差降りる

———–*/

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);

}

getBox.ino

/—————————————— ブック回収 2つ目のブックを落とす : 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; //※平面上の回収は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) { //1ボックス設置済み 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);
}

readUARTData.ino

/*———————–

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

 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;

    }

  }

}

Put_Box.ino

/*———

ブック設置

———–*/

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() {  //3段目

  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–;

}

arina.ino

/*———

アリーナ

———–*/

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();  // 3段目へ

      break;

    }

    if (high_count >= 3) {

      //krs.setPos(1, 4800);

      Put_Box2();  // 2段目へ

      break;

    }

    delay(10);

  }

}

次回はgithub使えるようになっておきます…

おまけ(輸送)

輸送タスクは見落としがちですが、非常に労力を割くイベントです。

輸送箱の外観

B3サイズで印刷した紙をラミネートしています。
ビス止めした上から養生テープで保護してます。

輸送箱内部

ロボットはクッションを挟んで結束バンドでガチガチに固定しました。

被せてあるビニールは防水目的で、建物の塗装時に車に被せてあるアレです。

トラックに積まれる輸送箱と私

トラック大きかったです。

バンドで輸送箱を囲うように固定していただけるので安心です。

会場で開梱された輸送箱

ピットから少し離れた場所に輸送箱が置かれます。

つまり、会場で真っ先に、
・輸送箱を解体
・ロボットと工具類を取り出す
・運ぶ
という工程が立ちはだかります。

ピットクルーは初日の朝から必要です。

また、初日の朝は早いうちに会場入口で並んでおきましょう。

まとめ

練習通りではなかったものの、とりあえずロボットが動いている姿をお見せできて良かったです。

いままでは目標を「出場」としていましたが、想定より1年早く出場できたことから、来年の目標を「優勝」と設定することができました。

これからも頑張って参ります。

またいつか。

]]>
/nhk%e5%ad%a6%e7%94%9f%e3%83%ad%e3%83%9c%e3%82%b3%e3%83%b32026%e3%83%ad%e3%83%9c%e3%83%83%e3%83%88%e7%b4%b9%e4%bb%8b/feed/ 0
書き初めイベント???? /%e6%9b%b8%e3%81%8d%e5%88%9d%e3%82%81%e3%82%a4%e3%83%99%e3%83%b3%e3%83%88%f0%9f%8e%8d/ /%e6%9b%b8%e3%81%8d%e5%88%9d%e3%82%81%e3%82%a4%e3%83%99%e3%83%b3%e3%83%88%f0%9f%8e%8d/#respond Sat, 28 Feb 2026 09:18:27 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=3020

こんにちは˙ ͜ʟ˙

新年あけてだいぶ経ちましたが

あけましておめでとうございます( ◠‿◠ )

今年もよろしくお願いいたします!

新年ということで、K*RISEでは、書き初めイベントを行いました!

今日はその様子を公開しまーす♩

6人で書き初めたので、みんなの抱負を紹介します( ◠‿◠ )

1人目????‍♀️

私の今年の2026年度の文字は「グラタン」でした〜✨

2人目????‍♀️????‍♂️

私の今年の目標はこの学生広報をちゃんと継続的に存続させることです❗️

3人目????‍♀️????‍♂️????

私の今年の漢字は「筋トレ」理由は毎年筋トレ頑張ろうって決意してるのにダラダラしちゃってるから、今年こそは❗️九産大のジムで頑張りま〜す????

4人目????‍♀️????‍♂️????????‍♀️

私の今年の目標は猫を愛でることで〜す????

5人目????‍♀️????‍♂️????????‍♀️????‍♂️

私の目標は全身全霊で物事に取り組むことと、クレープパーティーを開催することです✨

6人目????‍♀️????‍♂️????????‍♀️????‍♂️????

まあ笑っていればなんとかなるということで、今年の漢字は「笑」でーす( ◠‿◠ )

こんな感じでみんなで書き初めを行いましたー♩

クレープパーティは絶対開きたいです❗️

皆さんも書き初めてみてくださ〜い????

]]>
/%e6%9b%b8%e3%81%8d%e5%88%9d%e3%82%81%e3%82%a4%e3%83%99%e3%83%b3%e3%83%88%f0%9f%8e%8d/feed/ 0
立花寮合宿ガイド /%e7%ab%8b%e8%8a%b1%e5%af%ae%e5%ae%bf%e6%b3%8a%e3%82%ac%e3%82%a4%e3%83%89/ /%e7%ab%8b%e8%8a%b1%e5%af%ae%e5%ae%bf%e6%b3%8a%e3%82%ac%e3%82%a4%e3%83%89/#respond Thu, 19 Mar 2026 08:45:36 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=3068

立花寮に泊まれるのは遠方の学生で一人暮らしの学生のみだと思っていませんか・・・?

実家暮らしには縁が無いよな〜????

私もバリバリ実家暮らしなので、寮に泊まれる未来なんて無いと思っていました。

しかしなんと!きちんと申請をすればお泊まり気分で大学の寮に泊まれちゃうのです♪

今回は『RISE合宿』主催者が、寮に泊まる手引きをお伝えします????この方法は部活での合宿利用や、大学から家までわざわざ帰るのが面倒な時におすすめかも!?!?!?

まずは申請をしてみよう!

お泊まりしたいと思いったたらまずは厚生課に電話をしてみましょう!!基本寮での宿泊手続きは厚生課が担っています。

注意:この段階で立花寮へ電話してはいけません。一般学生の宿泊手続きは厚生課管轄らしいのでね。(もちろん私は勘違いして最初は寮に連絡をしていました)

電話では「宿泊利用したい」などの旨を伝えましょう。そしたら厚生課の方が担当して引き続きの動きのことを教えていただけます。

レッツ厚生課

 厚生課に行ったら書類をもらいます。私がもらった必要書類はこちら(間違いがあったらすいません)

・立花寮の施設情報が載ったプリント

・宿泊目的などを記載するプリント

・布団レンタルのプリント

・宿泊者の個人名を記載するプリント

こちらの4枚をもらいます。1枚づつ説明すると

・立花寮の施設情報が載ったプリント

これは寮の概要が載っています。どんな施設なのか、何が備わっているのか、など。一階には卓球施設があるので卓球もできますよ♪

・宿泊目的などを記載するプリント

こちらは、『何のために宿泊するのか』『代表者』『団体名』などを記載するプリントです。

・布団レンタルのプリント

皆さんも気になったであろうこのプリント。寮では素泊まりはできなくて、お布団を外部の業者からレンタルしないといけないのです。(びっくり)これが少し大変で、外部との連絡を自分で行わないといけないので申請が少々大変でした。。。管轄も外部になるので完全に外注になります。(外注ですが、厚生課の指定する会社さんなので安心)

・宿泊者の個人名を記載するプリント

これはお泊まりする人の名前と個人の電話ばんごうを全員分書きます。人数が多いと手が疲れる作業です。

提出しよう

上記の紙が書けたら厚生課にGO!!!

確か2週間前に申請を完了しておく必要がありました。宿泊費は1人1200円くらいだった気がします。(布団代含む
)

ご飯は基本持参ですが、水は飲み水がありました。バスルームはなくて、共同のシャワールームがあります。ちなみにキャンセル料は宿泊日の三日前から発生するのでお気をつけて。。。

さて、これであなたもお泊まりできますね!さようならー????

]]>
/%e7%ab%8b%e8%8a%b1%e5%af%ae%e5%ae%bf%e6%b3%8a%e3%82%ac%e3%82%a4%e3%83%89/feed/ 0
大学生でやってよかったと思うこと /%e5%a4%a7%e5%ad%a6%e7%94%9f%e3%81%a7%e3%82%84%e3%81%a3%e3%81%a6%e3%82%88%e3%81%8b%e3%81%a3%e3%81%9f%e3%81%a8%e6%80%9d%e3%81%86%e3%81%93%e3%81%a8/ /%e5%a4%a7%e5%ad%a6%e7%94%9f%e3%81%a7%e3%82%84%e3%81%a3%e3%81%a6%e3%82%88%e3%81%8b%e3%81%a3%e3%81%9f%e3%81%a8%e6%80%9d%e3%81%86%e3%81%93%e3%81%a8/#respond Tue, 07 Apr 2026 21:16:17 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=3175

こんにちは˙ ͜ʟ˙

なんと4月がきました、????

とうとう四年生になりましたが、一年生になった人もいますね????

中学校よりも、高校よりも長い、四年間ある大学は

有意義な時間を過ごしたいですよねー♩

そんなわくわくな楽しい大学生活を送るみなさんに

私が個人的にやってよかったなと思うことをご紹介します( ◠‿◠ )

ーーーーーーーーーーーーーーーーーーーーーーーーーーーー

????早めに免許を取る

大学生は車の免許を取る人がたくさんいますよね????

学部にもよるかもしせませんが、年々忙しくなってくるので

免許を取りたい方は、なるべく早めにとっておいた方がいいと思います????

私は一年生の後期に車校に通いました????

九産は、送迎バスが出ているすぐ近くの福岡市自動車学校もあるので

とても通いやすいです????

学校前に行って、空きコマにいって、放課後に行ったりしてました( ◠‿◠ )????

長期休みに、免許合宿でとっている人もいるので、合宿も短期間で取れていいかも????

自分に合う取得方法がいいですね〜????

????単位をなるべくとる

単位も、早めに取ることをお勧めします( ◠‿◠ )????

1・2年生で多めにとっておくと

3・4年生の自分がすこし楽になれて、他に時間を使えます????

ただ、履修しすぎて単位を取れなくなってしまうと本末転倒なので

自分で調整しながら、なんかいい感じで

自分にぴったりな時間割をつくりたいですね????

????気になる授業は取る

気になる授業があれば、せっかく学びにきているので

取れる範囲で受けてみると、後悔なく学べますね????

難しそうな授業だなと思って迷ったこともあったけど

取りたくてとった授業は全部とってよかったと思える授業でした????

たのしみながら、いろんなことを身につけたいですね????‍♀️

????アルバイトを充実させる

大学生はアルバイトをする人が多いと思います????

アルバイトはもちろんお金のためでもあるし、経験にもなりますよね????

私は、こんなに色々な職種を経験できる機会は人生で少ないと思うので、

色々なアルバイトをしたいタイプです????

私は一つのアルバイトは高校生くらいから続けていて、

他に掛け持ちで、何個かいろんなバイトをやってみたりしています????

バイトは疲れるけど、いろんな人と出会ったり、面白い体験になったり

意外なことを身につけられたりで面白いし、時には気分転換になるので

いい経験だなと思います????

自分にぴったりなバイトを見つけたいですね????

????色々やってみる

私は今まで、イベントやプロジェクトなど、自分にチャンスが舞い降りたときは

絶対に逃さないように手当たり次第色々やってきました( ◠‿◠ )

なぜなら自分で動けないからです( ◠‿◠ )

特に自身もないしこれといった特技もないし何したらいいかわからないし

みたいな感じなので、躊躇いがちなところを思い切って

とりあえずなんでも挑戦してみることにしました????

このK*RISEもですが、プロジェクトやイベントなどに参加すると

自分の成長だけではなくて、人の関わりがあるので

友達ができたり仲良くなれました♩

参加したプロジェクトきっかけで

海外旅行に行くまでの仲になった子達もいます♩

自分の生活や性格に沿う範囲でいろんなことをやってみると

自分のことを知れたり、いろんな人の考えに触れられたり、

面白いことが起こったりするかも知れません♩

????旅行に行く

私は大学生といえば旅行のイメージでした✈️

でも意外とお金も時間もなかったり….

それでも旅行は行ってよかったと思います✈️

プロジェクトを機に仲良くなった友達とはベトナムに行きました????????

あとは1人で国内や国外に旅行に行ったり????‍♂️

もちろん社会人になってからも行く機会はあると思うし

旅行以外のことにお金を費やすこともとてもいいことですが

大学生で旅行に行くのもおすすめです????‍♀️

普段の生活圏以外に行って

違う文化とか自然とかいろんな人とかと関わったりハプニングもあったり

なんか全部いい経験になります( ◠‿◠ )

旅行は高いので世界旅行の気分で

いろんな国の料理を食べるという国内世界旅行もおすすめです????

自分に使うお金や時間は確保したいですね????

ーーーーーーーーーーーーーーーーーーーーーーーー

こんな感じでとても個人的なやってよかったことでした????‍♂️

まだ私も大学生活は終わっていないので

大学生活をたっぷり堪能し

よかったと思えることをもっと増やしていきたいと思います❇️

]]>
/%e5%a4%a7%e5%ad%a6%e7%94%9f%e3%81%a7%e3%82%84%e3%81%a3%e3%81%a6%e3%82%88%e3%81%8b%e3%81%a3%e3%81%9f%e3%81%a8%e6%80%9d%e3%81%86%e3%81%93%e3%81%a8/feed/ 0
とある黄色い1日???? /%e3%81%a8%e3%81%82%e3%82%8b%e9%bb%84%e8%89%b2%e3%81%841%e6%97%a5%f0%9f%9f%a1/ /%e3%81%a8%e3%81%82%e3%82%8b%e9%bb%84%e8%89%b2%e3%81%841%e6%97%a5%f0%9f%9f%a1/#respond Tue, 14 Apr 2026 15:42:44 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=3110

こんにちは˙ ͜ʟ˙

今日は私の最近黄色かった1日をご紹介します????

この日はK*RISEの活動日でした????‍♀️

K*RISEといえば、きいろですよね

K*RISEらしく、とっても黄色いTシャツを着て活動しました????‍♂️

K*RISEクエスチョンや究極の2択など

楽しく撮影しました♩

きいろ〜い

動画はInstagramにて随時アップ♩????

活動が終わった帰り道、

黄色いレモンを買いました

皮を削ったり、、、、、

、、して黄色い生地の出来上がり

焼いて

おいしそう

アイシング垂らしてー

こんなイメージで飾ってー

できたー!

レモンケーキ!????

おいしかった????

みなさんの忙しない日々にも黄色い1日をぜひ

]]>
/%e3%81%a8%e3%81%82%e3%82%8b%e9%bb%84%e8%89%b2%e3%81%841%e6%97%a5%f0%9f%9f%a1/feed/ 0
売店にボザエが来たよ /%e5%a3%b2%e5%ba%97%e3%81%ab%e3%83%9c%e3%82%b6%e3%82%a8%e3%81%8c%e6%9d%a5%e3%81%9f%e3%82%88/ /%e5%a3%b2%e5%ba%97%e3%81%ab%e3%83%9c%e3%82%b6%e3%82%a8%e3%81%8c%e6%9d%a5%e3%81%9f%e3%82%88/#respond Wed, 20 May 2026 10:05:44 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=3521

こんにちは???? ͗ ͗ ͗ 

15号館前にある売店「カフェ・デ・ボザール」に
口のでかいキャラクターが登場したのを
見かけた方いらっしゃいますか????

今回はそのプロジェクトについて書きまーす__✍????

彼女はボザエです。ボザールだからボザエ
2秒で命名した

学生を笑顔にできると嬉しくて口がデカくなります

誕生したのは2年次のプロモーションの授業です
課題で提案した企画でしたが
なんだかんだで実現できることに(ᐢ ̫ᐢ)
ありがとうございます????‍????

KRISEメンバーのミカエル・ミカエラと2人
(そして先生)でやりました。


印刷を外部に頼むものもあるので
そっちを先に作っていきます????????‍????< カタカタ

のぼりでーす 見え方も確認したよ ミカエルありがと
他にもいろいろ作りました

入稿が済んだら次は
学内印刷できるものたちを作ります。

納期が遅れるぶん自分たちの首を
しめることになります。しめました

店内に貼るパネルとか作りました。右はアイスボザエ
店内に居ます。見つけてね

いろんなボザエを作ることができて
結構楽しくやりました。

カットは数名に手伝ってもらったりもしました????
ありがとう

お気に入りはこれです 指さしボザエ
なんでもアピールしてくれる 便利ですね
気づいたら9時だった

そんなこんなでバタバタしながら実装

最後思いつきで貼りました。

売店の方はとても優しくて
店内に貼ってたらボザエを可愛いと言ってくれました
生みの親として心に沁みました。


ポスターとかもいろんなとこにあります
インスタも、実はありますよ

ボザエ期間がいつまでかは確定してませんので
よかったら早めに会いに来てね

???? ͗ ͗ ͗

]]>
/%e5%a3%b2%e5%ba%97%e3%81%ab%e3%83%9c%e3%82%b6%e3%82%a8%e3%81%8c%e6%9d%a5%e3%81%9f%e3%82%88/feed/ 0
九州産業大学 芸術学部 〜プロジェクションマッピング Lantopia????〜 /lantopia%e3%80%9c%e8%8a%b8%e8%a1%93%e5%ad%a6%e9%83%a8-%e3%83%97%e3%83%ad%e3%82%b8%e3%82%a7%e3%82%af%e3%82%b7%e3%83%a7%e3%83%b3%e3%83%9e%e3%83%83%e3%83%94%e3%83%b3%e3%82%b0%f0%9f%8e%84%e3%80%9c/ /lantopia%e3%80%9c%e8%8a%b8%e8%a1%93%e5%ad%a6%e9%83%a8-%e3%83%97%e3%83%ad%e3%82%b8%e3%82%a7%e3%82%af%e3%82%b7%e3%83%a7%e3%83%b3%e3%83%9e%e3%83%83%e3%83%94%e3%83%b3%e3%82%b0%f0%9f%8e%84%e3%80%9c/#respond Thu, 22 Jan 2026 07:00:00 +0000 https://k-rise.cs.kyusan-u.ac.jp/?p=2811

みなさんこんにちは????

今日は
12月22日23日に行われていたソーシャルデザイン学科 情報デザイン専攻の学生さんが行っていたプロジェクションマッピング

「Lantopia」をご紹介!

授業終わりに北門へ向かう途中大きくて綺麗な映像が校舎に投影されていました✳︎

落ち着きのある音楽ととともに学生さんが作ったクリスマスの映像が建物一面に投影されており、
いつも通る道が、初めての場所に来たかのような空間に????♪
実際の時刻と連動している時計台や雪が降る街でクリスマスを楽しむ人々などカラフルで暖かな映像が流れていました

北門のすぐそばで行われていて門の外からも映像が見えるからか、学生だけでなく、小さなお子さんや大人の方まで大学に訪れていました

映像だけでなくココアやマシュマロの無料提供も!
映像の前では焚き火があるから焚き火でマシュマロを炙って食べました????????
寒い中食べる炙りマシュマロはフワトロ甘くてとっても美味しかったです


作成者の学生さんにお気に入り箇所を聞いたところ守衛室に投影されているパッチワークをイメージした映像だと教えてもらいました^^(写真↓)

作成者のソーシャルデザイン学科の生徒さん

<作成者コメント>
落ち着いた空間をコンセプトに作成しました。
暖かくファンタジーな空間を子どもから大人までたくさんに人に楽しんでもらいたいです。

落ち着く空間で寒くてもあたたかな気持ちになれる素敵なプロジェクションマッピングでした????☃️
プロジェクションマッピングはソーシャルデザインの学生さんによって不定期で行われているので見かけた際はぜひ訪れてみてください

See you〜 ^^/~~~

]]>
/lantopia%e3%80%9c%e8%8a%b8%e8%a1%93%e5%ad%a6%e9%83%a8-%e3%83%97%e3%83%ad%e3%82%b8%e3%82%a7%e3%82%af%e3%82%b7%e3%83%a7%e3%83%b3%e3%83%9e%e3%83%83%e3%83%94%e3%83%b3%e3%82%b0%f0%9f%8e%84%e3%80%9c/feed/ 0