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

前提・実現したいこと

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(AA1.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からのシリアル信号の１個目は固定。
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) { //１byte読み込んで0x80のスタートビットかチェック
for (chksum = c[0], i = 1; i < 8; i++) { //スタートビットは読み込み済みなので、次の７個のデータを読み込む。
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) { //１byte読み込んで0x80のスタートビットかチェック
for (chksum = c[0], i = 1; i < 8; i++) { //スタートビットは読み込み済みなので、次の７個のデータを読み込む。
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) { //１byte読み込んで0x80のスタートビットかチェック
for (chksum = c[0], i = 1; i < 8; i++) { //スタートビットは読み込み済みなので、次の７個のデータを読み込む。
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();
}
}
}
}