質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

ただいまの
回答率

88.62%

Arduinoを使ってプログラムを組んだ二足歩行ロボットの横歩行モーションがうまく動作しないので至急助けてください。

受付中

回答 1

投稿

  • 評価
  • クリップ 1
  • VIEW 1,404

chihiro_0701

score 10

 前提・実現したいこと

ROBO-ONEという大会に参加すべく、作成したロボットをArduinoで動かすようにしました。
コントローラはps3のコントローラをbuletooth接続を行って実際に動かしています。
ここからが本題です。
右に移動する歩行プログラムを作成したのですが、ps3のボタンを押している間は動作し、話した瞬間動作をやめるようにプログラムを作りたいのですが、何回かいろいろと試しても誤動作ばかりしてしまいます。
どうにか、押している間は動作し、離した瞬間に止まれるプログラムになるようにしたいので、よろしくおねがいします。

 発生している問題・エラーメッセージ

発生している問題としては、ボタンを押している間は、きちんと右歩行をする動作をするのですが、離した瞬間に止まらずに、速く動いて何秒かたってから急に止まるという誤動作が発生しています。

 該当のソースコード

include <MsTimer2.h>

include <math.h>

define pi 3.1415926535

int  L1 = 120, L2 = 120; //腕の長さ
float  L3,               //長さ
t, tL, tR,        //時間[s]
T,                //歩行周期[s]
eps_X,            //水平方向デューティー比
eps_Z,            //鉛直方向デューティー比
dt,               //サンプリングタイム[s]
v_xL, v_xR,       //水平方向速度[mm/s]
v_zL, v_zR,       //鉛直方向速度[mm/s]
xL, xR,           //水平方向位置[mm]
yL, yR,           //左右振り[mm]
zL, zR,           //鉛直方向位置[mm]
h,                //蹴上高さ[mm]
H,                //本体高さ[mm]
K,                //座標(X,Y)を指定して角度を求める
A,                //角度1
AA,               //サーボ角度1
B,                //サーボ角度2
C,                //足首角度
E,                //ロールの動き
F;                //ロールの動き
void servoset(unsigned char id, uint16_t angle) {
static byte msg[3] = {
0x80, 0x00, 0x00
};
msg[0] = 0x80 | id;    //10000010 ID
msg[1] = (angle >> 7) & 0x7f;//0~255
msg[2] = angle & 0x7f;
Serial.write(msg, 3); //msgは3byteです
delayMicroseconds(1);
}
void RF(float  x, float y, float z) {
uint16_t val1, val2, val3;
L3 = sqrt((x * x) + (z * z)); //原点からの目標座標
K = atan2(z, x); //座標(x,z)を指定して角度を求める
A = degrees( K - acos(((L1 * L1) + (L3 * L3) - (L2 * L2)) / (2 * L1 * L3)) ); //角度1
AA = (90.0 - A);
B = degrees( PI - acos(((L1 * L1) + (L2 * L2) - (L3 * L3)) / (2 * L1 * L2)) ); //角度2
C = -(A + B) + 90.0;
val1 = map(AA, -135, 135, 3500, 11500);
val2 = map(B, -135, 135, 3500, 11500);
val3 = map(C, -135, 135, 3500, 11500);
/*ピッチ軸*/
servoset(0, val3 + 180); //ID_0番をval3の角度に
servoset(2, val2 - 300); //ID_2番をval2の角度に
servoset(4, val1 - 160); //ID_4番をval1の角度に
}
void RA(float  x, float y, float z) {
L3 = sqrt((x * x) + (z * z)); //原点からの目標座標
K = atan2(z, x); //座標(x,z)を指定して角度を求める
A = degrees( K - acos(((L1 * L1) + (L3 * L3) - (L2 * L2)) / (2 * L1 * L3)) ); //角度1
AA = (90.0 - A);
val9 = map(AA*1.5, -135, 135, 3500, 11500);
/*ピッチ軸*/
servoset(15, val9  -2630); //ID_15番をval3の角度に
}
void TRLD(float tR, float v_b) {
//前後方向
if (tR < eps_X * T / 2.0) {
v_xR = -v_b;
xR   = -v_b * tR;
}
else if (tR < (1.0 - eps_X / 2.0)*T) {
v_xR = (v_b / (1.0 - eps_X)) * (1.0 - cos(2 * pi / ((1.0 - eps_X) * T) * (tR - T * (eps_X / 2.0)))) - v_b;
xR   = (v_b / (1.0 - eps_X)) * (tR - (eps_X * T / 2.0) - (T * (1.0 - eps_X) / (2.0 * pi)) * sin(2 * pi / ((1.0 - eps_X) * T) * (tR - T * (eps_X / 2.0)))) - (v_b * tR);
}
else {
v_xR = -v_b;
xR   = -v_b * (tR - T);
}
}
void TRH(float tR, float h) {
//足上げ高さ
if (tR < eps_Z * T / 2.0) {
v_zR = 0.0;
zR   = -H;
}
else if (tR < (1.0 - eps_Z / 2.0)*T) {
v_zR = pi * h / ((1.0 - eps_Z) * T) * sin(2.0 * pi / ((1.0 - eps_Z) * T) * (tR - eps_Z / 2.0 * T));
zR   = -h / 2.0 * cos(2.0 * pi / ((1.0 - eps_Z) * T) * (tR - eps_Z / 2.0 * T)) + h / 2.0 - H;
}
else {
v_zR = 0.0;
zR   = -H;
}
}
void LF(float x, float y, float z) {
uint16_t val4, val5, val6;
L3 = sqrt((x * x) + (z * z)); //原点からの目標座標
K = atan2(z, x); //座標(x,z)を指定して角度を求める
A = degrees( K - acos(((L1 * L1) + (L3 * L3) - (L2 * L2)) / (2 * L1 * L3)) ); //角度1
AA = -(90.0 - A);
B = -degrees( PI - acos(((L1 * L1) + (L2 * L2) - (L3 * L3)) / (2 * L1 * L2)) ); //角度2
C = -(A - B) + 90.0;
val4 = map(AA, -135, 135, 3500, 11500);
val5 = map(B, -135, 135, 3500, 11500);
val6 = map(C, -135, 135, 3500, 11500);
/*ピッチ軸*/
servoset(12, val6 + 400);     //ID_12番をval3の角度に +で↓ 左足首(手動で治す)
servoset(10, val5 - 80); //ID_10番をval2の角度に     -で後ろ   (手動で治す)
servoset(8, val4 - 120);  //ID_8番をval1の角度に      +で後ろ    (手動で治す)
}
void LA(float x, float y, float z) {
uint16_t val10
L3 = sqrt((x * x) + (z * z)); //原点からの目標座標
K = atan2(z, x); //座標(x,z)を指定して角度を求める
A = degrees( K - acos(((L1 * L1) + (L3 * L3) - (L2 * L2)) / (2 * L1 * L3)) ); //角度1
AA = -(90.0 - A);
val10 = map(AA*1.5, -135, 135, 3500, 11500);
/*ピッチ軸*/
servoset(13, val10 + 2080);     //ID_12番をval3の角度に  左足首(手動で治す)
}
void TLLD(float tL, float v_b) {
//前後方向
if (tL < eps_X * T / 2.0) {
v_xL = -v_b;
xL   = -v_b * tL;
}
else if (tL < (1.0 - eps_X / 2.0)*T) {
v_xL = (v_b / (1.0 - eps_X)) * (1.0 - cos(2 * pi / ((1.0 - eps_X) * T) * (tL - T * (eps_X / 2.0)))) - v_b;
xL   = (v_b / (1.0 - eps_X)) * (tL - (eps_X * T / 2.0) - (T * (1.0 - eps_X) / (2.0 * pi)) * sin(2 * pi / ((1.0 - eps_X) * T) * (tL - T * (eps_X / 2.0)))) - (v_b * tL);
}
else {
v_xL = -v_b;
xL   = -v_b * (tL - T);
}
}
void TLH(float tL, float h) {
//足上げ高さ
if (tL < eps_Z * T / 2.0) {
v_zL = 0.0;
zL   = -H;
}
else if (tL < (1.0 - eps_Z / 2.0)*T) {
v_zL = pi * h / ((1.0 - eps_Z) * T) * sin(2.0 * pi / ((1.0 - eps_Z) * T) * (tL - eps_Z / 2.0 * T));
zL   = -h / 2.0 * cos(2.0 * pi / ((1.0 - eps_Z) * T) * (tL - eps_Z / 2.0 * T)) + h / 2.0 - H;
}
else {
v_zL = 0.0;
zL   = -H;
}
}
void TRM() {
/*R右ロール*/
servoset(1, 7450);
servoset(3, 7500);//-で右、+で左
/*L左ロール*/
servoset(9, 7400);//-で右、+で左
servoset(11, 7550);
/*腰*/
servoset(5, 7300);//右 +で反時計回り -で時計回り
servoset(6, 7650);//    -で反時計回り
servoset(7, 7650);//左 +で反時計回り -で時計回り
/*右腕*/
servoset(13, 6920);        //3500~11500の間で動くが「0」にすることで脱力状態にする
servoset(14, 7900);
/*左腕*/
servoset(15, 7330);        //3500~11500の間で動くが「0」にすることで脱力状態にする
servoset(16, 8000);
}

include <SoftwareSerial.h>

// Switch on LED on pin 13 each second

define MYRX 12 //デジタル12番ピンはソフトウェアシリアルRX

define MYTX 11 //デジタル11番ピンはソフトウェアシリアルTX

SoftwareSerial mySerial(MYRX, MYTX);
unsigned char c[8];
unsigned long chksum;
void flash() {
t += dt;
if (T > 2.0);
t = 0.0;
}
void setup() {
c[0] = 0x80; //SBDBTからのシリアル信号の1個目は固定。
mySerial.begin(2400);//SBDBTとArduinoは2400bps
/*設定*/
T     =  2.0;             //歩行周期[s]
eps_X =  0.7;             //水平方向デューティ比
eps_Z =  0.6;            //鉛直方向デューティ比
//dt    =  0.04;            //サンプリングタイム
//v_b   =  70.0;           //歩行速度[mm/s]
//h    =  -30.0;           //蹴上高さ[mm]
H     =  -185.0;          //本体高さ[mm]
t     =  0.0;
tL    =  3.0 * T / 4.0;
tR    =  T / 4.0;
MsTimer2::set(500, flash);
MsTimer2::start();
Serial.begin(115200, SERIAL_8E1);//通信速度115200に
RF(0, 0, 185);
LF(0, 0, 185);
TRLD(0, 0);
TRH(0, 0);
TLLD(0, 0);
TLH(0, 0);
TRM();
}

void loop() {
int i, k, o,j;
if (mySerial.available() >= 8) { //8byte以上あるかチェック
if (mySerial.read() == 0x80) { //1byte読み込んで0x80のスタートビットかチェック
for (chksum = c[0], i = 1; i < 8; i++) { //スタートビットは読み込み済みなので、次の7個のデータを読み込む。
c[i] = mySerial.read();
if (i < 7) {
chksum += c[i];
}
}

if ((c[2] & 0x04 ) == 0x04 ) {//右歩行 →
k = 0;
while ((c[2] & 0x04) == 0x04) {
if (mySerial.available() >= 8) { //8byte以上あるかチェック
if (mySerial.read() == 0x80) { //1byte読み込んで0x80のスタートビットかチェック
for (chksum = c[0], i = 1; i < 8; i++) { //スタートビットは読み込み済みなので、次の7個のデータを読み込む。
c[i] = mySerial.read();
if (i < 7) chksum += c[i];
}
}
}//信号が来た時にwhileの条件に当てはめる
if((c[2] & 0x04 ) != 0x04 ) {
if (mySerial.available() >= 8) { //8byte以上あるかチェック
if (mySerial.read() == 0x80) { //1byte読み込んで0x80のスタートビットかチェック
for (chksum = c[0], i = 1; i < 8; i++) { //スタートビットは読み込み済みなので、次の7個のデータを読み込む。
c[i] = mySerial.read();
if (i < 7) chksum += c[i];
}
}
}
goto owari;
//break;
} else {
for (int k = 0; k <= 10; k++) {
RF(0 - k, 0, 180 - 3 * k);
LF(0, 0, 180);
servoset(1, 7450 - 30 * k);
servoset(3, 7500 - 30 * k);
servoset(9, 7400 + 30 * k);
servoset(11, 7550 + 30 * k);
delay(5);
}
for (int k = 0; k <= 10; k++) {
RF(-10 + k, 0, 150 + 3 * k);
LF(0, 0, 180 - 3 * k);
servoset(1, 7150 + 30 * k);
servoset(3, 7200 + 30 * k);
servoset(9, 7700 - 30 * k);
servoset(11, 7850 - 30 * k);
delay(5);
}
for (int k = 0; k <= 10; k++) {
LF(0, 0, 150 + 3 * k);
delay(4);
}
}
}
owari();
}
}
}
}

 試したこと

 補足情報(FW/ツールのバージョンなど)

  • 気になる質問をクリップする

    クリップした質問は、後からいつでもマイページで確認できます。

    またクリップした質問に回答があった際、通知やメールを受け取ることができます。

    クリップを取り消します

  • 良い質問の評価を上げる

    以下のような質問は評価を上げましょう

    • 質問内容が明確
    • 自分も答えを知りたい
    • 質問者以外のユーザにも役立つ

    評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

    質問の評価を上げたことを取り消します

  • 評価を下げられる数の上限に達しました

    評価を下げることができません

    • 1日5回まで評価を下げられます
    • 1日に1ユーザに対して2回まで評価を下げられます

    質問の評価を下げる

    teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。

    • プログラミングに関係のない質問
    • やってほしいことだけを記載した丸投げの質問
    • 問題・課題が含まれていない質問
    • 意図的に内容が抹消された質問
    • 過去に投稿した質問と同じ内容の質問
    • 広告と受け取られるような投稿

    評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。

    質問の評価を下げたことを取り消します

    この機能は開放されていません

    評価を下げる条件を満たしてません

    評価を下げる理由を選択してください

    詳細な説明はこちら

    上記に当てはまらず、質問内容が明確になっていない質問には「情報の追加・修正依頼」機能からコメントをしてください。

    質問の評価を下げる機能の利用条件

    この機能を利用するためには、以下の事項を行う必要があります。

質問への追記・修正、ベストアンサー選択の依頼

  • y_waiwai

    2018/10/19 06:56

    このままではコードが見づらいので、編集して、<code>ボタン、’’’の枠の中にコードを貼り付けてください

    キャンセル

  • coco_bauer

    2018/10/19 13:28

    「ps3のボタンを押している間」、bluetooth接続を介して送られるデータは、どのようなものなのでしょうか。「ps3のボタンを話した時」bluetooth接続を介して送られているデータは中断するのでしょうか?中断するとしたら、どこで中断するのでしょうか?

    キャンセル

  • OtoMura

    2018/10/20 13:04

    僕も大きなプログラムを作ったことがあるのですが機能が増えすぎたり複雑になると1つの動作が違うところのプログラムにもかんしょうしてしまうということがありました。参考になったら嬉しいです。

    キャンセル

  • 退会済みユーザー

    2018/11/26 00:32

    複数のユーザーから「やってほしいことだけを記載した丸投げの質問」という意見がありました
    「質問を編集する」ボタンから編集を行い、調査したこと・試したことを記入していただくと、回答が得られやすくなります。

回答 1

0

https://github.com/felis/USB_Host_Shield_2.0
の中にあるPS3BT.hを利用したほうが宜しいかと思います。

このままでいくのであれば、一度受信したデータをシリアルモニタにも同時に送り
ボタンを離した後の通信内容を確認したほうがよろしいかと。

投稿

  • 回答の評価を上げる

    以下のような回答は評価を上げましょう

    • 正しい回答
    • わかりやすい回答
    • ためになる回答

    評価が高い回答ほどページの上位に表示されます。

  • 回答の評価を下げる

    下記のような回答は推奨されていません。

    • 間違っている回答
    • 質問の回答になっていない投稿
    • スパムや攻撃的な表現を用いた投稿

    評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

15分調べてもわからないことは、teratailで質問しよう!

  • ただいまの回答率 88.62%
  • 質問をまとめることで、思考を整理して素早く解決
  • テンプレート機能で、簡単に質問をまとめられる

同じタグがついた質問を見る

  • トップ
  • Cに関する質問
  • Arduinoを使ってプログラムを組んだ二足歩行ロボットの横歩行モーションがうまく動作しないので至急助けてください。