京都で開催されたキャチロボバトルコンテストに、ロボット工房から｢ぶちょー｣と｢FTFT｣の2チームで出場しました。
私はチーム｢ぶちょー｣としてロボットを製作しました。
このロボットについてご紹介します。
大会概要
一言でいうと、
｢お菓子を陳列する競技｣
今年のワークはプチアソートでした。
もちろん競技はチョコが入っている状態で行われるので、会場は甘い匂いが漂っていました。
試合は対戦形式で、赤コートと青コートがあります。
コートの色によって、お菓子の陳列先が自身の右側なのか左側なのかと変化します。
シューティングエリアにお菓子を置くと得点が入るわけですが、このようにパッケージが揃うように5つ陳列することで追加のボーナス得点を獲得できます。
このようにワークを破損させると減点されます。
共通エリアなんて要素もありますが今回はスルーしました。
詳しいルールは公式ホームページをご覧ください。
結果報告
予選敗退でしたが、特別枠で紹介いただきました。
予選試合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 １ステップの値を格納
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トリガーボタンのデータ＋２自由値
// ボタンの状態を変数に代入
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をもっと作りこむか、練習環境を再現できていれば…
予選２試合目にサブアームの歯車がメインアームの配線を絡めとり引き抜かれました。
抜けた配線は差すだけで復旧できる状態でしたが、メンテナンス性を考慮していない設計により、その場での修正が困難でなすすべなく３分間を終えました。
まとめ
ロボットの性能を十分に発揮してやれなかったことが、ロボットに対して申し訳ないと思ってます。
悔しすぎる…
とは言いつつ、特別枠で紹介いただけたのでとても嬉しかったです。
来年は本気で優勝を目指します。
それでは、またいつか。