#include <Ps3Controller.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);
#define SERVOMIN 115 //-144degree // 560us/20ms *4096(12bit)=114.7 //FutabaのRS304MDデータシート参照
#define SERVOMAX 508 //144degree // 2480us/20ms *4096(12bit)=507.9
#define SERVOMID 311 //0degree // 1520us/20ms *4096(12bit)=311.3
int16_t angle[24];
int16_t anglePulse[24];
uint8_t i;
uint8_t GyroOn;
// 端子設定
uint8_t svPin[24];
//アクションモード
#define Stop 0
#define Forward_walk 1
#define Backward_walk 2
#define Forward_dash 3
#define Guard 4
#define Left_step 5
#define Right_step 6
#define Left_turn 7
#define Right_turn 8
#define Right_shift 9
#define Left_turn 10
#define Right_turn 11
#define Left_harai 12
#define Right_harai 13
#define Left_hook_punch 14
#define Right_hook_punch 15
#define Left_upper_punch 16
#define Right_upper_punch 17
#define Straight_punch 18
#define One_two_punch 19
#define Ms 20
#define Stand_up_back 21
#define Stand_up_front 22
#define Right_high_kick 23
#define Greeting 24
#define Karate_chop 25
#define Kawara_wari 26
//モーションデータ配列
int16_t offset[24] = {-850,100,20,40,-130,410, 660,-400,-440,410,-460,470, -400,520,-370,500,480,-670, 60,-90,110,40,-50,900 };
int16_t std_pose[24] = { 0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0 };
int16_t tempAngles[24] = { 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 };
int16_t tempAngles1[24] = { 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 };
int16_t action[32][25] = {
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20},
{ 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0 , 20}
};
int16_t forward_walk[][25] = { //前進
{ 0,0,700,0,0, 0, -30,403,301,-300, -30,-301, 301, 30,300,-301,-403, 30, 0, 0,0,-700,0, 0, 20},
{ 21,0,700,0,0, 0, -119,332,280,-279,-119,-280, 297,-60,295,-297,-349,-60, 0, 0,0,-700,0, -4, 3},
{ -61,0,700,0,0, 0, -130,414,362,-360,-130,-362, 297,-60,295,-297,-349, 20, 0, 0,0,-700,0, -4, 3},
//
{ -90,0,700,0,0, 0, -135,443,391,-389,-135,-391, 296,-60,326,-296,-316, 60, 0, 0,0,-700,0, -37, 3},
{-109,0,700,0,0, 0, -159,462,345,-278,-159,-345, 285,-88,344,-285,-276, -8, -75, 0,0,-700,0, -77, 3},
{ -59,0,700,0,0, 0, -101,412,274,-185,-101,-274, 287,-43,377,-287,-248,-43, -130, 0,0,-700,0,-105, 3},
{ -54,0,700,0,0, 0, -14,407,297,-237, -14,-297, 277, 45,396,-277,-209, 45, -150, 0,0,-700,0,-144, 3},
{ -22,0,700,0,0, 0, 65,375,295,-265, 65,-295, 241,120,385,-241,-147,120, -130, 0,0,-700,0,-206, 3},
{ 11,0,700,0,0, 0, 10,342,290,-288, 90,-290, 335,158,446,-335,-274,158, -75, 0,0,-700,0, -79, 3},
{ 44,0,700,0,0, 0, -30,309,289,-319, 90,-289, 380,168,378,-380,-432,168, 0, 0,0,-700,0, 79, 3},
{ 77,0,700,0,0, 0, 8,276,285,-344, 88,-285, 345,159,278,-345,-462,159, 75, 0,0,-700,0, 109, 3},
{ 105,0,700,0,0, 0, 43,248,287,-377, 43,-287, 274,101,185,-274,-412,101, 130, 0,0,-700,0, 59, 3},
{ 144,0,700,0,0, 0, -45,209,277,-396, -45,-277, 297, 14,237,-297,-407, 14, 150, 0,0,-700,0, 54, 3},
{ 206,0,700,0,0, 0, -120,147,241,-385,-120,-241, 295,-65,265,-295,-375,-65, 130, 0,0,-700,0, 22, 3},
{ 70,0,700,0,0, 0, -128,283,346,-459,-128,-346, 297,-60,295,-297,-349, 20, 75, 0,0,-700,0, -4, 3},
//
{ -90,0,700,0,0, 0, -135,443,391,-389,-135,-391, 296,-60,326,-296,-316, 60, 0, 0,0,-700,0, -37, 3},
{-109,0,700,0,0, 0, -159,462,345,-278,-159,-345, 285,-88,344,-285,-276, -8, -75, 0,0,-700,0, -77, 3},
{ -59,0,700,0,0, 0, -101,412,274,-185,-101,-274, 287,-43,377,-287,-248,-43, -130, 0,0,-700,0,-105, 3},
{ -54,0,700,0,0, 0, -14,407,297,-237, -14,-297, 277, 45,396,-277,-209, 45, -150, 0,0,-700,0,-144, 3},
{ -22,0,700,0,0, 0, 65,375,295,-265, 65,-295, 241,120,385,-241,-147,120, -130, 0,0,-700,0,-206, 3},
{ 11,0,700,0,0, 0, 10,342,290,-288, 90,-290, 335,158,446,-335,-274,158, -75, 0,0,-700,0, -79, 3},
{ 44,0,700,0,0, 0, -30,309,289,-319, 90,-289, 380,168,378,-380,-432,168, 0, 0,0,-700,0, 79, 3},
{ 77,0,700,0,0, 0, 8,276,285,-344, 88,-285, 345,159,278,-345,-462,159, 75, 0,0,-700,0, 109, 3},
{ 105,0,700,0,0, 0, 43,248,287,-377, 43,-287, 274,101,185,-274,-412,101, 130, 0,0,-700,0, 59, 3},
{ 144,0,700,0,0, 0, -45,209,277,-396, -45,-277, 297, 14,237,-297,-407, 14, 150, 0,0,-700,0, 54, 3},
{ 206,0,700,0,0, 0, -120,147,241,-385,-120,-241, 295,-65,265,-295,-375,-65, 130, 0,0,-700,0, 22, 3},
{ 70,0,700,0,0, 0, -128,283,346,-459,-128,-346, 297,-60,295,-297,-349, 20, 75, 0,0,-700,0, -4, 3},
//
{ -61,0,700,0,0, 0, -130,414,362,-360,-130,-362, 297,-60,295,-297,-349, 20, 0, 0,0,-700,0, -4, 3},
{ 0,0,700,0,0 ,0, -30,353,301,-300, -30,-301, 301, 30,300,-301,-353, 30, 0 ,0,0,-700,0, 0, 20}
};
int16_t backward_walk[][25] = { //後進
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 20},
{ 17,0,700,0,0, 0, -114,366,314,-313,-114,-314, 329,-53,327,-329,-381,-53, 0, 0,0,-700,0, -2, 3},
{ -47,0,700,0,0, 0, -123,430,378,-376,-123,-378, 329,-53,327,-329,-381, 7, 0, 0,0,-700,0, -2, 3},
//
{ -70,0,700,0,0, 0, -127,453,402,-400,-127,-402, 328,-53,307,-328,-399, 27, 0, 0,0,-700,0, 16, 3},
{ 12,0,700,0,0, 0, -150,371,367,-413,-150,-367, 322,-79,280,-322,-413,-19, 75, 0,0,-700,0, 30, 3},
{ 80,0,700,0,0, 0, -98,303,314,-375, -98,-314, 325,-37,263,-325,-437,-37, 130, 0,0,-700,0, 54, 3},
{ 45,0,700,0,0, 0, -16,338,330,-371, -16,-330, 320, 45,238,-320,-453, 45, 150, 0,0,-700,0, 70, 3},
{ 26,0,700,0,0, 0, 58,357,327,-348, 58,-327, 298,117,197,-298,-448,117, 130, 0,0,-700,0, 65, 3},
{ 8,0,700,0,0, 0, 20,375,324,-322, 80,-324, 362,150,286,-362,-488,150, 75, 0,0,-700,0,105, 3},
{ -11,0,700,0,0, 0, 0,394,323,-303, 80,-323, 393,157,391,-393,-445,157, 0, 0,0,-700,0, 62, 3},
{ -30,0,700,0,0, 0, 19,413,322,-280, 79,-322, 367,150,413,-367,-371,150, -75, 0,0,-700,0,-12, 3},
{ -54,0,700,0,0, 0, 37,437,325,-263, 37,-325, 314, 98,375,-314,-303, 98, -130, 0,0,-700,0,-80, 3},
{ -70,0,700,0,0, 0, -45,453,320,-238, -45,-320, 330, 16,371,-330,-338, 16, -150, 0,0,-700,0,-45, 3},
{ -65,0,700,0,0, 0, -117,448,298,-197,-117,-298, 327,-58,348,-327,-357,-58, -130, 0,0,-700,0,-26, 3},
{-115,0,700,0,0, 0, -122,498,371,-294,-122,-371, 329,-53,327,-329,-381, 7, -75, 0,0,-700,0, -2, 3},
//
{ -70,0,700,0,0, 0, -127,453,402,-400,-127,-402, 328,-53,307,-328,-399, 27, 0, 0,0,-700,0, 16, 3},
{ 12,0,700,0,0, 0, -150,371,367,-413,-150,-367, 322,-79,280,-322,-413,-19, 75, 0,0,-700,0, 30, 3},
{ 80,0,700,0,0, 0, -98,303,314,-375, -98,-314, 325,-37,263,-325,-437,-37, 130, 0,0,-700,0, 54, 3},
{ 45,0,700,0,0, 0, -16,338,330,-371, -16,-330, 320, 45,238,-320,-453, 45, 150, 0,0,-700,0, 70, 3},
{ 26,0,700,0,0, 0, 58,357,327,-348, 58,-327, 298,117,197,-298,-448,117, 130, 0,0,-700,0, 65, 3},
{ 8,0,700,0,0, 0, 20,375,324,-322, 80,-324, 362,150,286,-362,-488,150, 75, 0,0,-700,0,105, 3},
{ -11,0,700,0,0, 0, 0,394,323,-303, 80,-323, 393,157,391,-393,-445,157, 0, 0,0,-700,0, 62, 3},
{ -30,0,700,0,0, 0, 19,413,322,-280, 79,-322, 367,150,413,-367,-371,150, -75, 0,0,-700,0,-12, 3},
{ -54,0,700,0,0, 0, 37,437,325,-263, 37,-325, 314, 98,375,-314,-303, 98, -130, 0,0,-700,0,-80, 3},
{ -70,0,700,0,0, 0, -45,453,320,-238, -45,-320, 330, 16,371,-330,-338, 16, -150, 0,0,-700,0,-45, 3},
{ -65,0,700,0,0, 0, -117,448,298,-197,-117,-298, 327,-58,348,-327,-357,-58, -130, 0,0,-700,0,-26, 3},
{-115,0,700,0,0, 0, -122,498,371,-294,-122,-371, 329,-53,327,-329,-381, 7, -75, 0,0,-700,0, -2, 3},
//
{ -47,0,700,0,0, 0, -123,430,378,-376,-123,-378, 329,-53,327,-329,-381, 7, 0, 0,0,-700,0, -2, 3},
{ 0,0,700,0,0, 0,- 31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 20}
};
int16_t forward_dash[][25] = { //前ダッシュ
{12,0,700,0,0,0,-116,472,420,-419,-116,-420,432,-32,431,-432,-484,-32,0,0,0,-700,0,0, 1},
{-87,0,700,0,0,0,-140,571,519,-517,-140,-519,432,-32,431,-432,-484,48,0,0,0,-700,0,0, 1},
//
{-126,0,700,0,0,0,-139,610,558,-556,-139,-558,432,-21,463,-432,-451,99,0,0,0,-700,0,-33, 1},
{-165,0,700,0,0,0,-105,649,520,-443,-105,-520,430,-2,492,-430,-418,78,-25,0,0,-700,0,-66, 1},
{-82,0,700,0,0,0,-42,566,423,-332,-42,-423,423,42,516,-423,-382,42,-43,0,0,-700,0,-102, 1},
{-58,0,700,0,0,0,-7,542,430,-368,-7,-430,413,77,534,-413,-342,77,-50,0,0,-700,0,-142, 1},
{-29,0,700,0,0,0,8,513,432,-401,8,-432,403,90,552,-403,-303,90,-43,0,0,-700,0,-181, 1},
{-1,0,700,0,0,0,-72,485,433,-432,8,-433,512,110,641,-512,-434,110,-25,0,0,-700,0,-50, 1},
{33,0,700,0,0,0,-112,451,432,-463,8,-432,560,123,559,-560,-612,123,0,0,0,-700,0,128, 1},
{66,0,700,0,0,0,-87,418,430,-492,-7,-430,522,94,444,-522,-650,94,25,0,0,-700,0,166, 1},
{102,0,700,0,0,0,-42,382,423,-516,-42,-423,423,42,332,-423,-566,42,43,0,0,-700,0,82, 1},
{143,0,700,0,0,0,-85,341,412,-533,-85,-412,430,-2,368,-430,-542,-2,50,0,0,-700,0,58, 1},
{185,0,700,0,0,0,-112,299,398,-547,-112,-398,431,-32,401,-431,-513,-32,43,0,0,-700,0,29, 1},
{54,0,700,0,0,0,-137,430,507,-635,-137,-507,432,-32,431,-432,-484,48,25,0,0,-700,0,0, 1},
//
{-126,0,700,0,0,0,-139,610,558,-556,-139,-558,432,-21,463,-432,-451,99,0,0,0,-700,0,-33, 1},
{-165,0,700,0,0,0,-105,649,520,-443,-105,-520,430,-2,492,-430,-418,78,-25,0,0,-700,0,-66, 1},
{-82,0,700,0,0,0,-42,566,423,-332,-42,-423,423,42,516,-423,-382,42,-43,0,0,-700,0,-102, 1},
{-58,0,700,0,0,0,-7,542,430,-368,-7,-430,413,77,534,-413,-342,77,-50,0,0,-700,0,-142, 1},
{-29,0,700,0,0,0,8,513,432,-401,8,-432,403,90,552,-403,-303,90,-43,0,0,-700,0,-181, 1},
{-1,0,700,0,0,0,-72,485,433,-432,8,-433,512,110,641,-512,-434,110,-25,0,0,-700,0,-50, 1},
{33,0,700,0,0,0,-112,451,432,-463,8,-432,560,123,559,-560,-612,123,0,0,0,-700,0,128, 1},
{66,0,700,0,0,0,-87,418,430,-492,-7,-430,522,94,444,-522,-650,94,25,0,0,-700,0,166, 1},
{102,0,700,0,0,0,-42,382,423,-516,-42,-423,423,42,332,-423,-566,42,43,0,0,-700,0,82, 1},
{143,0,700,0,0,0,-85,341,412,-533,-85,-412,430,-2,368,-430,-542,-2,50,0,0,-700,0,58, 1},
{185,0,700,0,0,0,-112,299,398,-547,-112,-398,431,-32,401,-431,-513,-32,43,0,0,-700,0,29, 1},
{54,0,700,0,0,0,-137,430,507,-635,-137,-507,432,-32,431,-432,-484,48,25,0,0,-700,0,0, 1},
//
{-87,0,700,0,0,0,-140,571,519,-517,-140,-519,432,-32,431,-432,-484,48,0,0,0,-700,0,0, 1},
{0,0,700,0,0,0,-43,484,431,-430,-43,-431,431,43,430,-431,-484,43,0,0,0,-700,0,0, 1},
{0,0,700,0,0,0,-43,484,431,-430,-43,-431,431,43,430,-431,-484,43,0,0,0,-700,0,0, 20}
};
int16_t guard[][25] = { //ガード低姿勢
{55,-67,838,125,-1,-250,-467,703,727,-677,-447,-727,727,437,662,-727,-688,482,41,-2,-287,-696,-25,-266, 3},
{55,-67,838,125,-1,-250,-467,703,727,-677,-447,-727,727,437,662,-727,-688,482,41,-2,-287,-696,-25,-266, 30},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0, 15}
};

int16_t left_step[][25] = { //左ステップ
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 5},
//
{ 13,0,700,0,0, 0, -100,370,319,-317,-100,-319, 330,-39,329,-330,-382,-39, 0, 0,0,-700,0, -1, 2},
{-79,0,700,0,0, 0, -113,462,410,-408,-113,-410, 330,-39,329,-330,-382, 51, 0, 0,0,-700,0, -1, 2},
{ 14,0,700,0,0, 0, -296,369,317,-315,-296,-317, 330,-39,329,-330,-382, 51, 0, 0,0,-700,0, -1, 2},
{125,0,700,0,0, 0, -267,258,206,-204,-267,-206, 330,-39,329,-330,-382,-39, 0, 0,0,-700,0, -1, 2},
{ 35,0,700,0,0, 0, -159,348,296,-294,-159,-296, 323, 85,321,-323,-374, 85, 0, 0,0,-700,0, -9, 2},
{ 1,0,700,0,0, 0, -39,382,330,-329, -39,-330, 270,201,268,-270,-322,201, 0, 0,0,-700,0, -61, 2},
{ 9,0,700,0,0, 0, 85,374,323,-321, 85,-323, 144,303,142,-144,-196,303, 0, 0,0,-700,0,-187, 2},
{ 4,0,700,0,0, 0, -28,379,327,-325, 62,-327, 297,316,295,-297,-349,316, 0, 0,0,-700,0, -34, 2},
{ 4,0,700,0,0, 0, -28,379,327,-325, 62,-327, 404,138,402,-404,-455,138, 0, 0,0,-700,0, 72, 2},
{ -1,0,700,0,0, 0, 16,384,332,-330, 16,-332, 324, 77,322,-324,-376, 77, 0, 0,0,-700,0, -7, 2},
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 5}
};
int16_t right_step[][25] = { //右ステップ
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 5},
//
{ 1,0,700,0,0, 0, 39,382,330,-329, 39,-330, 319,100,317,-319,-370,100, 0, 0,0,-700,0, -13, 2},
{ 1,0,700,0,0, 0, -51,382,330,-329, 39,-330, 410,113,408,-410,-462,113, 0, 0,0,-700,0, 79, 2},
{ 1,0,700,0,0, 0, -51,382,330,-329, 39,-330, 317,296,315,-317,-369,296, 0, 0,0,-700,0, -14, 2},
{ 1,0,700,0,0, 0, 39,382,330,-329, 39,-330, 206,267,204,-206,-258,267, 0, 0,0,-700,0,-125, 2},
{ 9,0,700,0,0, 0, -85,374,323,-321, -85,-323, 296,159,294,-296,-348,159, 0, 0,0,-700,0, -35, 2},
{ 61,0,700,0,0, 0,-201,322,270,-268,-201,-270, 330, 39,329,-330,-382, 39, 0, 0,0,-700,0, -1, 2},
{ 187,0,700,0,0, 0,-303,196,144,-142,-303,-144, 323,-85,321,-323,-374,-85, 0, 0,0,-700,0, -9, 2},
{ 34,0,700,0,0, 0,-316,349,297,-295,-316,-297, 327,-62,325,-327,-379, 28, 0, 0,0,-700,0, -4, 2},
{ -72,0,700,0,0, 0,-138,455,404,-402,-138,-404, 327,-62,325,-327,-379, 28, 0, 0,0,-700,0, -4, 2},
{ 7,0,700,0,0, 0, -77,376,324,-322, -77,-324, 332,-16,330,-332,-384,-16, 0, 0,0,-700,0, 1, 2},
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 5}
};
int16_t left_turn[][25] = { //左ターン
{ -35,0,700,0,0, 0, -239,418,440,-513,-109,-440, 440,109,368,-440,-563,239, 0, 0,0,-700,0,180, 1},
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 6},
//
{ -35,0,700,0,0, 0, -239,418,440,-513,-109,-440, 440,109,368,-440,-563,239, 0, 0,0,-700,0,180, 1},
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0, 0, 6}
};
int16_t right_turn[][25] = { //右ターン
{ -180,0,700,0,0, 0, -239,563,440,-368,-109,-440, 440,109,513,-440,-418,239, 0, 0,0,-700,0,35, 1},
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0 ,0, 6},
//
{ -180,0,700,0,0, 0, -239,563,440,-368,-109,-440, 440,109,513,-440,-418,239, 0, 0,0,-700,0,35, 1},
{ 0,0,700,0,0, 0, -31,383,331,-329, -31,-331, 331, 31,329,-331,-383, 31, 0, 0,0,-700,0 ,0, 6}
};
int16_t left_harai[][25] = { //左払い
{953,797,-64,602,2,-21,-240,489,442,-435,-242,-442,443,240,435,-443,-486,249,-343,-3,853,-819,-437,-1, 8 },
{1027,-3,-56,-67,2, 29,-309, 69,167,-278,-369,-167,525,127,555,-525,-490,197,776,-1,589,-693,-590,-147, 2},
{1027,-3,-56,-67,2, 29,-309, 69,167,-278,-369,-167,525,127,555,-525,-490,197,776,-1,589,-693,-590,-147, 15},
{953,797,-64,602,2,-21,-240,489,442,-435,-242,-442,443,240,435,-443,-486,249,-343,-3,853,-819,-437,-1, 1},
{953,797,-64,602,2,-21,-240,489,442,-435,-242,-442,443,240,435,-443,-486,249,-343,-3,853,-819,-437,-1, 15},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0, 15}
};
int16_t right_harai[][25] = { //右払い
{1, 437, 819, -853, 3, -21, -249, 486, 443, -435, -240, -443, 442, 242, 435, -442, -489, 240, 343, -2, -602, 64, -797, -953, 8 },
{147, 590, 693, -589, 1, 29, -197, 490, 525, -555, -127, -525, 167, 369, 278, -167, -69, 309, -776, -2, 67, 56, 3, -1027, 2},
{147, 590, 693, -589, 1, 29, -197, 490, 525, -555, -127, -525, 167, 369, 278, -167, -69, 309, -776, -2, 67, 56, 3, -1027, 15},
{1, 437, 819, -853, 3, -21, -249, 486, 443, -435, -240, -443, 442, 242, 435, -442, -489, 240, 343, -2, -602, 64, -797, -953, 1},
{1, 437, 819, -853, 3, -21, -249, 486, 443, -435, -240, -443, 442, 242, 435, -442, -489, 240, 343, -2, -602, 64, -797, -953, 15},
{0, 0, 700, 0, 0, 0, -21, 485, 433, -431, -21, -433, 433, 21, 431, -433, -485, 21, 0, 0, 0, -700, 0, 0, 15}
};
int16_t left_hook_punch[][25] = { //左フック
{953,797,-64,602,2,-21,-240,489,442,-435,-242,-442,444,240,435,-444,-486,249,-343,-3,853,-819,-437,-1,7},
{1027,-3,-56,-67,2,29,-309,69,167,-278,-369,-167,526,127,555,-526,-490,197,776,-1,589,-693,-590,-147,3},
{1027,-3,-56,-67,2,29,-309,69,167,-278,-369,-167,526,127,555,-526,-490,197,776,-1,589,-693,-590,-147,12},
{953,797,-64,602,2,-21,-240,489,442,-435,-242,-442,444,240,435,-444,-486,249,-343,-3,853,-819,-437,-1,8},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,15}
};
int16_t right_hook_punch[][25] = { //右フック
{1,437,819,-853,3,-21,-249,486,444,-435,-240,-444,442,242,435,-442,-489,240,343,-2,-602,64,-797,-953,7},
{147,590,693,-589,1,29,-197,490,526,-555,-127,-526,167,369,278,-167,-69,309,-776,-2,67,56,3,-1027,3},
{147,590,693,-589,1,29,-197,490,526,-555,-127,-526,167,369,278,-167,-69,309,-776,-2,67,56,3,-1027,12},
{1,437,819,-853,3,-21,-249,486,444,-435,-240,-444,442,242,435,-442,-489,240,343,-2,-602,64,-797,-953,8},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,15}
};
int16_t left_upper_punch[][25] = { //左アッパー
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,6},
{-387,-326,986,-965,0,-39,-24,486,441,-432,-26,-441,440,32,438,-440,-482,30,35,0,957,-916,166,441,6},
{-387,-326,986,-966,0,-45,-149,798,759,-766,-134,-759,772,57,865,-772,-726,39,30,0,956,-916,166,439,6},
{146,-317,1292,-225,0,-90,-155,799,760,-773,-138,-760,773,58,872,-773,-732,41,441,0,956,-916,166,438,6},
{1010,-67,729,-124,0,138,-215,151,241,-293,-218,-241,254,167,372,-254,-106,167,359,0,955,-918,168,435,3},
{1010,-67,729,-124,0,138,-215,151,241,-293,-218,-241,254,167,372,-254,-106,167,359,0,955,-918,168,435,10},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,10}
};
int16_t right_upper_punch[][25] = { //右アッパー
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,6},
{-441,-166,916,-957,0,-39,-30,482,440,-438,-32,-440,441,26,432,-441,-486,24,-35,0,965,-986,326,387,6},
{-439,-166,916,-956,0,-45,-39,726,772,-865,-57,-772,759,134,766,-759,-798,149,-30,0,966,-986,326,387,6},
{-438,-166,916,-956,0,-90,-41,732,773,-872,-58,-773,760,138,773,-760,-799,155,-441,0,225,-1292,317,-146,6},
{-435,-168,918,-955,0,138,-167,106,254,-372,-167,-254,241,218,293,-241,-151,215,-359,0,124,-729,67,-1010,3},
{-435,-168,918,-955,0,138,-167,106,254,-372,-167,-254,241,218,293,-241,-151,215,-359,0,124,-729,67,-1010,10},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,10}
};
int16_t straight_punch[][25] = { //ストレートパンチ
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,8},
{-4, -2, 835, -884, 0, 3, -177, 482, 438, -426, -172, -438, 440, 177, 427, -440, -481, 170, 381, 0, 854, -817, 85, 212, 8},
{-3, -6, 833, -883, 0, 48, -186, 609, 445, -302, -186, -445, 445, 183, 559, -445, -352, 174, -845, 0, -271, -1018, -354, -542, 3},
{-3, -6, 833, -883, 0, 48, -186, 609, 445, -302, -186, -445, 445, 183, 559, -445, -352, 174, -845, 0, -271, -1018, -354, -542, 8},
{594, 353, 808, 379, 0, 15, -186, 354, 447, -546, -186, -447, 447, 185, 298, -447, -602, 176, 742, 0, 849, -818, 86, 214, 3},
{594, 353, 808, 379, 0, 15, -186, 354, 447, -546, -186, -447, 447, 185, 298, -447, -602, 176, 742, 0, 849, -818, 86, 214, 8},
{13, -21, 907, -956, 0, 22, -192, 473, 454, -429, -198, -454, 450, 188, 420, -450, -481, 182, -36, 0, 881, -941, 82, 215, 10},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,15}
};
int16_t one_two_punch[][25] = { //ワンツーパンチ
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,6},
{136,62,638,-815,0,-3,-171,401,404,-408,-168,-404,404,165,408,-404,-403,172,0,0,830,-742,34,-18,6},
{134,64,665,-944,0,-1,-172,401,409,-409,-170,-409,407,164,410,-407,-403,174,-471,0,834,-750,-6,56,6},
{560,398,449,83,0,-53,-246,298,409,-438,-295,-409,578,63,611,-578,-473,147,831,0,825,-735,-2,151,3},
{560,398,449,83,0,-53,-246,298,409,-438,-295,-409,578,63,611,-578,-473,147,831,0,825,-735,-2,151,8},
{726,499,226,-958,0,-141,-68,273,446,-566,-53,-446,249,292,313,-249,-111,288,-497,0,-363,-22,113,-1042,4},
{726,499,226,-958,0,-141,-68,273,446,-566,-53,-446,249,292,313,-249,-111,288,-497,0,-363,-22,113,-1042,8},
{136,62,638,-815,0,-3,-171,401,404,-408,-168,-404,404,165,408,-404,-403,172,0,0,830,-742,34,-18,6},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,15}
};
int16_t ms[][25] = { //投げ技
{0,0,700,0,0,0,-321,485,433,-431,-321,-433,433,321,431,-433,-485,321,0,0,0,-700,0,0,7},
{1630,217,-28,563,1,-622,-355,490,438,-440,-346,-438,440,344,450,-440,-493,352,26,-1,-318,-2,-482,-1693,7},
{1568,-409,-57,323,2,-591,-366,492,442,-444,-358,-442,442,354,454,-442,-496,360,-1,-2,-334,102,467,-1595,7},
//
{1768,-469,609,70,0,-539,-336,348,305,-85,-448,-305,716,281,738,-716,-578,472,-649,-1,-65,227,-6,-1257,5},
{1768,-469,609,70,0,-539,-336,348,305,-85,-448,-305,716,281,738,-716,-578,472,-649,-1,-65,227,-6,-1257,15},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,16}
};
int16_t stand_up_back[][25] = { //仰向け起き上がり
{96,190,292,106,-1,22,-3,4,-5,-1,-3,-1,-1,-4,-2,5,-4,0,-8,-2,26,-344,-393,58,30},
{564,-495,919,49,-1,24,-3,4,-5,0,-2,-1,0,-3,-2,3,-3,0,-7,-1,1,-912,401,-538,30},
{1349,-482,940,-831,1427,-20,-14,-241,5,78,30,-756,512,-21,-142,-265,173,1,-7,-1453,793,-914,508,-1360,30},
{1955,-485,937,-183,1427,150,-7,-242,416,-583,33,-746,634,17,521,-498,223,29,-1,-1454,55,-917,516,-2018,30},
{2176,-471,935,-182,1427,145,23,-243,632,-967,35,-737,633,-6,936,-694,257,-4,-7,-1454,1,-921,455,-2255,30},
{2154,-474,935,-183,1427,143,19,-243,629,-923,34,-740,633,-7,884,-692,256,-7,19,-1454,-23,-917,219,133,30},
{-146,-186,808,20,1427,141,18,-243,631,-919,34,-739,633,-6,886,-693,257,-7,7,-1454,-26,-920,249,110,30},
{254,-358,807,19,1427,-367,26,-8,648,-1009,27,-742,719,-18,1002,-666,2,13,-15,-1454,-372,-803,351,-470,30},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,60},
{ 0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,20 }
};
int16_t stand_up_front[][25] = { //うつ伏せ起き上がり
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,30},
{4,526,900,442,1,-232,-77,-105,43,8,1,-663,301,3,-217,-217,121,36,-12,-3,-484,-860,-404,-52,30},
{875,440,899,441,0,-101,-59,-35,78,-929,6,-55,56,-1,914,-71,37,62,-6,-1,-473,-855,-501,-891,30},
{870,178,898,-471,-1,-115,-58,-34,77,-929,7,-55,56,-1,914,-70,37,63,-9,-3,441,-854,-196,-890,30},
{823,-346,914,-416,-1,-911,18,798,1052,-1000,6,-609,-95,255,892,-736,121,262,13,-2,440,-877,364,-856,30},
{348,-218,914,191,0,-780,15,871,516,-517,20,-1070,-71,254,891,-1074,230,259,2,-3,-340,-941,158,-514,30},
{286,-42,784,-748,0,-435,-148,719,477,-264,-314,-544,300,115,876,-836,-214,263,-346,-3,-297,-728,-175,-322,30},
{286,-43,785,-748,-1,-706,-170,455,486,-382,-318,-548,477,119,566,-873,-596,263,-264,-4,-296,-730,-172,-327,30},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,30},
{ 0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0,20 }
};
int16_t right_high_kick[][25] = { //右ハイキック
{ -9,-140, 784, -802, 0, 6, 2, 2, 1, -1, -6, -1, -1, -1, 8, 1, -5, 0, -4, 0,955,-967, 114,191, 13},
{ -9,-140, 784, -802, 0, 6, 102, 2, 1, -1, 94+50, -1, -1, 99, 8, 1, -5, 100, -4, 0,955,-967, 114,191, 13},
{-12,-143, 781, -801, 0, 11, 110, -7, 5, 10, 119+50, -5,757, 31,633,-757,-815, 183, -8, 0,955,-964, 117,192, 13},
{-12,-143, 781, -801, 0, 11, 110, -7, 5, 10, 109+20, -5,757, 31,633,-757,-815, 83,445, 0,955,-964, 117,192, 13},
{-14,-141, 782, -801, 0,-33,-403, 1,-16, -4+70, -12, 16,758, 27,632,-758,-824, 586,526, 0,955,-963, 120,192, 13},
{ -5,-142, 782, -802, 0,-10,-554,-117, -5, -26+60, -24, 5,128, 26, 33,-128,-302, 728,550, 0,955,-963, 121,193, 9},
{ -5,-142, 782, -802, 0,-10,-554,-117, -5, -26+50, -24, 5,128, 26, 33,-128,-302, 728,550, 0,955,-963, 121,193, 13},
{ -9,-150, 779, -804, 0, 6,-453, 106,365,-460-50, -77, -365,213, 96,287,-213, 39,-214,308, 0,956,-965, 122,237, 9},
{ -9,-150, 779, -804, 0,-74,-453, 106,365, -460, -157, -365,213, 96,287,-213, 39,-214,308, 0,956,-965, 122,237, 13},
{ -9,-140, 784, -802, 0, 6, 2, 2, 1, -1, -6, -1, -1, -1, 8, 1, -5, 0, -4, 0,955,-967, 114,191, 13},
// 左足首ピッチ, ロール,
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0, 10},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0, 20}
};
int16_t greeting[][25] = {
{0,0,0,0,0,0,-100,0,0,0,-100,0,0,100,0,0,0,100,0,0,0,0,0,0, 15},
{0,0,0,0,0,0,-100,0,0,0,-100,0,0,100,0,0,0,100,0,-3,117,-86,9,-510, 15},
{-2,-5,0,-3,0,13,-114,-7,-2,-1,-101,-2,4,100,-9,-5,-2,100,-1,-3,547,-290,275,-606, 15},
{-2,-5,0,-3,0,13,-114,-7,-2,-1,-101,-2,4,100,-9,-5,-2,100,-1,-1289,547,-290,275,-606, 15},
{-2,-5,0,-3,0,13,-114,343,-2,99,-101,-2,4,100,-109,-5,-352,100,-1,-1289,547,-290,275,-606, 15},
{-2,-5,0,-3,0,-387,-114,343,-2,99,-101,-2,4,100,-109,-5,-352,100,-1,-1289,547,-290,275,-606, 15},
{-2,-5,0,-3,0,-387,-114,343,-2,99,-101,-2,4,100,-109,-5,-352,100,-1,-1289,547,-290,275,-606, 15},
{-2,-5,0,-3,0,-387,-114,343,-2,99,-101,-2,4,100,-109,-5,-352,100,-1,-1289,547,-290,275,-606, 15},
{-2,-5,0,-3,0,13,-114,343,-2,99,-101,-2,4,100,-109,-5,-352,100,-1,-1289,547,-290,275,-606, 15},
{-2,-5,0,-3,0,13,-114,-7,-2,-1,-101,-2,4,100,-9,-5,-2,100,-1,-1289,547,-290,275,-606, 15},
{-2,-5,0,-3,0,13,-114,-7,-2,-1,-101,-2,4,100,-9,-5,-2,100,-1,-3,547,-290,275,-606, 15},
{0,0,0,0,0,0,-100,0,0,0,-100,0,0,100,0,0,0,100,0,-3,117,-86,9,-510, 15},
{0,0,0,0,0,0,-100,0,0,0,-100,0,0,100,0,0,0,100,0,0,0,0,0,0, 15},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 15},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0, 15}
};
int16_t karate_chop[][25] = {
{-55,136,997,-343,-3,0,-100,0,0,0,-100,0,0,100,0,0,0,100,-3,1,411,-1045,-166,193, 8},
{-408,-210,1042,-1003,-3,1,-111,3,2,4,-101,2,4,103,-4,-4,-1,104,-2,0,979,-1059,166,425, 8},
{-408,-210,1042,-1003,1359,1,-111,3,2,4,-101,2,4,103,-4,-4,-1,104,-2,0,979,-1059,166,425, 8},
{-408,-210,1042,-1003,1359,1,-111,3,2,4,-101,2,4,103,-4,-4,-1,104,-2,1,984,-854,393,-821, 8},
{-410,-215,1038,-1001,1356,8,-118,-3,9,12,-105,-1,11,110,-8,-8,2,113,-6,1,-369,-12,361,-1780, 8},
{-410,-215,1038,-1001,1356,8,-118,-3,9,12,-105,-1,11,110,-8,-8,2,113,-6,1,-369,-12,361,-1780, 8},
{-410,-215,1038,-1001,1356,-145,-118,447,459,-438,-105,-451,461,110,442,-458,-448,113,-422,1,-374,-37,38,-863, 2},
{-410,-215,1038,-1001,1356,-145,-118,447,459,-438,-105,-451,461,110,442,-458,-448,113,-422,1,-374,-37,38,-863, 8},
{-412,-218,1037,-1000,1360,-145,-118,447,459,-438,-105,-451,461,110,442,-458,-448,113,16,1,946,-1060,144,255, 8},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 8},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0, 15}
};
int16_t kawara_wari[][25] = {
{33,-56,834,-681,-2,-4,-100,1,1,1,-107,0,2,101,2,-3,-2,97,0,1,693,-863,77,1, 16},
{33,-56,834,-681,1453,-4,-100,1,1,1,-107,0,2,101,2,-3,-2,97,0,-1453,693,-863,77,1,16},
{126,-58,816,-92,1452,-416,-103,346,359,-352,-112,-359,359,96,354,-361,-355,100,403,-1452,718,2,-285,-879,16},
{126,-58,816,-92,1452,-416,-103,346,359,-352,-112,-359,359,96,354,-361,-355,100,403,-1452,718,2,-285,-879,16},
{925,513,-56,-1016,1453,-813,-482,358,312,-148,-568,-275,682,96,819,-745,-531,145,-145,-1453,-260,994,178,-1261, 4},
{925,513,-56,-1016,1453,-813,-482,358,312,-148,-568,-275,682,96,819,-745,-531,145,-145,-1453,-260,994,178,-1261,16},
{925,512,-375,-562,1453,-315,-224,625,877,-614,-293,-522,696,89,842,-768,-544,153,68,-1453,212,835,181,-985,16},
{0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 16},
{0,0,700,0,0,0,-21,485,433,-431,-21,-433,433,21,431,-433,-485,21,0,0,0,-700,0,0, 15}
};
//フレーム処理、アクションモード、ボタンデータ関連 変数
uint8_t maxRows;
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;
uint8_t nextKeyFrame1;
uint8_t flag;
uint8_t actionMode;
uint8_t prevbtnData1; //フラグとして使用　0 or 1
uint16_t btnData = 32; //初期設定
uint16_t prevbtnData = 32; //初期設定

void setup() {
//Wire.begin(21,22);
// PS3 related
Serial.begin(115200);
Ps3.begin("84:0d:8e:c1:6a:ae"); // My ESP32 Mac address,need to be changed
Serial.println("Ready");
// Serial monitor setup
//Serial.begin(115200);
// Initialize PCA9685
pwm1.begin();
pwm2.begin();
// Set PWM Frequency to 50Hz
pwm1.setPWMFreq(50);
pwm2.setPWMFreq(50);
// I2Cサーボ用端子設定
svPin[0] = 0; //肩ピッチ　　左 pca9685-1
svPin[1] = 1; //肩ロール 　 "
svPin[2] = 2; //肘ピッチ 　 "
svPin[3] = 3; //手首ロール "
svPin[4] = 4; //手グリップ "
svPin[5] = 5; //腰ピッチ 　 "
svPin[6] = 6; //股ロール 　 "
svPin[7] = 7; //股ピッチ 　 "
svPin[8] = 8; //膝上ピッチ "
svPin[9] = 9; //足首ピッチ "
svPin[10] = 10; //足首ロール "
svPin[11] = 11; //膝下ピッチ "
svPin[12] = 0; //膝下ピッチ　右 pca9685-2
svPin[13] = 1; //足首ロール "
svPin[14] = 2; //足首ピッチ "
svPin[15] = 3; //膝上ピッチ "
svPin[16] = 4; //股ピッチ 　"
svPin[17] = 5; //股ロール 　 "
svPin[18] = 6; //腰ピッチ 　 "
svPin[19] = 7; //手グリップ "
svPin[20] = 8; //手首ロール "
svPin[21] = 9; //肘ピッチ　　　　　"
svPin[22] = 10; //肩ロール 　　 "
svPin[23] = 11; //肩ピッチ 　　 "
// フレーム処理関連変数初期化
actionMode = Stop;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
maxRows = 31; //action配列の初期値に対するmaxRowsを仮置き
flag = 0;
}

void loop() {
// PS3 controller にコネクト状態でloopごとにジョイスティック値を取り込む。
if (Ps3.isConnected()){
uint8_t LX = (Ps3.data.analog.stick.lx)+128;
uint8_t LY = 255-((Ps3.data.analog.stick.ly)+128);
uint8_t RX = (Ps3.data.analog.stick.rx)+128;
uint8_t RY = 255-((Ps3.data.analog.stick.ry)+128);

Serial.print(" LX=,");
Serial.print(LX);
Serial.print(",LY=,");
Serial.print(LY);
Serial.print(",RX=,");
Serial.print(RX);
Serial.print(",RY=,");
Serial.println(RY);

//右Joystickが中立の時に左Joystickの範囲(位置)とbtnDataを紐付けしてアクションモードを決める
if (LY>=192 && LX>=96 && LX<=160 && RY<160 && RY>96 && RX<160 && RX>96) { //右Joystickが中立(RY<160 && RY>96 && RX<160 && RX>96)
btnData = 21; //前進
}else if (LY<=64 && LX>=96 && LX<=160 && RY<160 && RY>96 && RX<160 && RX>96) {
btnData = 22; //後退
}else if (LX<=64 && LY>=96 && LY<=160 && RY<160 && RY>96 && RX<160 && RX>96) {
btnData = 23; //左横移動
}else if (LX>=192 && LY>=96 && LY<=160 && RY<160 && RY>96 && RX<160 && RX>96) {
btnData = 24; //右横移動
}else if (LY>=192 && LX<=64 && RY<160 && RY>96 && RX<160 && RX>96) {
btnData = 19; //左旋回
}else if (LY>=192 && LX>=192 && RY<160 && RY>96 && RX<160 && RX>96) {
btnData = 20; //右旋回
}else if (LY<=64 && LX<=64 && RY<160 && RY>96 && RX<160 && RX>96) {
btnData = 17; //ダッシュ
}else if (LY<=64 && LX>=192 && RY<160 && RY>96 && RX<160 && RX>96) {
btnData = 12; //右ハイキック
//
//左Joystickが中立の時に右Joystickの範囲(位置)とbtnDataを紐付けしてアクションモードを決める
}else if (RY>=192 && RX>=96 && RX<=160 && LY<160 && LY>96 && LX<160 && LX>96) { //左Joystickが中立(LY<160 && LY>96 && LX<160 && LX>96)
btnData = 1; //ストレートパンチ
}else if (RY<=64 && RX>=96 && RX<=160 && LY<160 && LY>96 && LX<160 && LX>96) {
btnData = 16; //投げ技MS
}else if (RX<=64 && RY>=96 && RY<=160 && LY<160 && LY>96 && LX<160 && LX>96) {
btnData = 8; //左フック
}else if (RX>=192 && RY>=96 && RY<=160 && LY<160 && LY>96 && LX<160 && LX>96) {
btnData = 4; //右フック
}else if (RY>=192 && RX<=64 && LY<160 && LY>96 && LX<160 && LX>96) {
btnData = 512; //左アッパー
}else if (RY>=192 && RX>=192 && LY<160 && LY>96 && LX<160 && LX>96) {
btnData = 2048; //右アッパー
}else if (RY<=64 && RX<=64 && LY<160 && LY>96 && LX<160 && LX>96) {
btnData = 256; //左払い
}else if (RY<=64 && RX>=192 && LY<160 && LY>96 && LX<160 && LX>96) {
btnData = 64; //右払い
}else {
btnData = 32;
}
// if (btnData == 0) {btnData = 32;} //nullのときは32をいれてstop状態にする
//ボタン値が変わった時に 次は何のアクションか選定する
if (btnData != prevbtnData) { //今の読取り値が前回の値と違う時だけ 以下switch文を実行
//Serial.println(btnData);
switch(btnData){
case 1: //ストレートパンチ
Serial.println("up ボタン圧下"); // 0x0001 1
actionMode = Straight_punch;
memcpy(action, straight_punch, sizeof(straight_punch));
maxRows = sizeof(straight_punch) / sizeof(*straight_punch) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 4: //右フックパンチ
Serial.println("right ボタン"); // 0x0004 4
actionMode = Right_hook_punch;
memcpy(action, right_hook_punch, sizeof(right_hook_punch));
maxRows = sizeof(right_hook_punch) / sizeof(*right_hook_punch) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
/*case 2: //ワンツーパンチ
Serial.println("down ボタン圧下"); // 0x0002 2
actionMode = One_two_punch;
memcpy(action, one_two_punch, sizeof(one_two_punch));
maxRows = sizeof(one_two_punch) / sizeof(*one_two_punch) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;*/
case 8: //左フックパンチ
Serial.println("left ボタン"); // 0x0008 8
actionMode = Left_hook_punch;
memcpy(action, left_hook_punch, sizeof(left_hook_punch));
maxRows = sizeof(left_hook_punch) / sizeof(*left_hook_punch) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 32: //ストップ
Serial.println("cross ボタン"); // 0x0020 32
actionMode = Stop;
keyFrame = 0;
nextKeyFrame = 0;
divCounter = 0;
GyroOn = 0;
break;
case 256: //左払い
Serial.println("square ボタン"); // 0x0100 256
actionMode = Left_harai;
memcpy(action, left_harai, sizeof(left_harai));
maxRows = sizeof(left_harai) / sizeof(*left_harai) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 16: //ms投げ技
Serial.println("triangle ボタン"); // 0x0010 16
actionMode = Ms;
memcpy(action, ms, sizeof(ms));
maxRows = sizeof(ms) / sizeof(*ms) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 64: //右払い
Serial.println("circle ボタン"); // 0x0040 64
actionMode = Right_harai;
memcpy(action, right_harai, sizeof(right_harai));
maxRows = sizeof(right_harai) / sizeof(*right_harai) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
/*case 3: //あいさつ
Serial.println("start ボタン"); // 0x0003 3
actionMode = Greeting;
memcpy(action, greeting, sizeof(greeting));
maxRows = sizeof(greeting) / sizeof(*greeting) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;*/
case 12: //右ハイキック
Serial.println("select ボタン"); // 0x000c 12
actionMode = Right_high_kick;
memcpy(action, right_high_kick, sizeof(right_high_kick));
maxRows = sizeof(right_high_kick) / sizeof(*right_high_kick) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 512: //左アッパーパンチ
Serial.println("l1 ボタン"); // 0x0200 512
actionMode = Left_upper_punch;
memcpy(action, left_upper_punch, sizeof(left_upper_punch));
maxRows = sizeof(left_upper_punch) / sizeof(*left_upper_punch) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 2048: //右アッパーパンチ
Serial.println("r1 ボタン"); // 0x0800 2048
actionMode = Right_upper_punch;
memcpy(action, right_upper_punch, sizeof(right_upper_punch));
maxRows = sizeof(right_upper_punch) / sizeof(*right_upper_punch) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
/*
case 1024:
Serial.println("l2 ボタン"); // 0x0400 1024
actionMode = Karate_chop;
memcpy(action, karate_chop, sizeof(karate_chop));
maxRows = sizeof(karate_chop) / sizeof(*karate_chop) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 4096:
Serial.println("r2 ボタン"); // 0x1000 4096
actionMode = Kawara_wari;
memcpy(action, kawara_wari, sizeof(kawara_wari));
maxRows = sizeof(kawara_wari) / sizeof(*kawara_wari) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
*/
/*case 768: //うつ伏せ起き上がり
Serial.println("l3 ボタン"); // 0x0300 768
actionMode = Stand_up_front;
memcpy(action, stand_up_front, sizeof(stand_up_front));
maxRows = sizeof(stand_up_front) / sizeof(*stand_up_front) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 1280: //仰向け起き上がり
Serial.println("r3 ボタン"); // 0x0500 1280
actionMode = Stand_up_back;
memcpy(action, stand_up_back, sizeof(stand_up_back));
maxRows = sizeof(stand_up_back) / sizeof(*stand_up_back) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;*/
case 17: //前方ダッシュ
Serial.println("右ジョイスティック 上"); //
actionMode = Forward_dash;
memcpy(action, forward_dash, sizeof(forward_dash));
maxRows = sizeof(forward_dash) / sizeof(*forward_dash) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
/*case 18: //空手チョップ
Serial.println("右ジョイスティック 下"); //
actionMode = Karate_chop;
memcpy(action, karate_chop, sizeof(karate_chop));
maxRows = sizeof(karate_chop) / sizeof(*karate_chop) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;*/
case 19: //左旋回
Serial.println("右ジョイスティック 左"); //
actionMode = Left_turn;
memcpy(action, left_turn, sizeof(left_turn));
maxRows = sizeof(left_turn) / sizeof(*left_turn) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 20: //右旋回
Serial.println("右ジョイスティック 右"); //
actionMode = Right_turn;
memcpy(action, right_turn, sizeof(right_turn));
maxRows = sizeof(right_turn) / sizeof(*right_turn) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 21: //前歩き
Serial.println("左ジョイスティック 上"); //
actionMode = Forward_walk;
memcpy(action, forward_walk, sizeof(forward_walk));
maxRows = sizeof(forward_walk) / sizeof(*forward_walk) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 22: //後ろ歩き
Serial.println("左ジョイスティック 下"); //
actionMode = Backward_walk;
memcpy(action, backward_walk, sizeof(backward_walk));
maxRows = sizeof(backward_walk) / sizeof(*backward_walk) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 23: //左移動
Serial.println("左ジョイスティック 左"); //
actionMode = Left_step;
memcpy(action, left_step, sizeof(left_step));
maxRows = sizeof(left_step) / sizeof(*left_step) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 24: //右移動
Serial.println("左ジョイスティック 右"); //
actionMode = Right_step;
memcpy(action, right_step, sizeof(right_step));
maxRows = sizeof(right_step) / sizeof(*right_step) - 1;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
default:
break;
} //switch
} //if (btnData != prevbtnData)
prevbtnData = btnData ; //次の読み取りにそなえて 今の値をprevbtnDataに入れる
Serial.print("prevbtnData = ");
Serial.print(prevbtnData);
//アクションモードにのっとった動作出力処理
if(actionMode != Stop){ // Stopモード以外
// フレーム処理
divCounter++;
if(divCounter >= action[nextKeyFrame][24]) {
divCounter = 0;
keyFrame = nextKeyFrame;
nextKeyFrame++;
if(nextKeyFrame > maxRows) {
nextKeyFrame = 0;
nextKeyFrame1 = maxRows+1;
}
}// フレーム処理end
// サーボ角度計算
// STOPからaction[][24]に他のアクション配列が読み込まれた時、 std_pose[i]姿勢からアクションモードの始まりにゆっくり移行する。
// それはアクション開始時の(divCounter >= action[nextKeyFrame][24])を満たさない間だけ。
if(keyFrame == 0 && nextKeyFrame == 0){ //アクションモードの始まり 最初のdivCounter >= action[nextKeyFrame][24]を満たさない時
divCounter++;
if(divCounter <= 20) {
// サーボ角度計算@Stopモード
if (prevbtnData1 == 1) { //前がStopだった時
for(int i=0; i<24; i++) {
tempAngles[i] = std_pose[i] +
int16_t((action[nextKeyFrame][i] - std_pose[i])
* divCounter / action[nextKeyFrame][24]);
}
for(i = 0; i < 12; i++) { // サーボ信号出力
angle[i] = (tempAngles[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm1.setPWM(svPin[i], 0, anglePulse[i]);
}
for(i = 12; i < 24; i++) { // サーボ信号出力
angle[i] = (tempAngles[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm2.setPWM(svPin[i], 0, anglePulse[i]);
}
delay(30);
}else{ //前がStopではなくアクションモードのいずれかだった時
for(int i = 0; i < 24; i++) { //tempAngles[i]はactionmode変更時のservo角度
tempAngles1[i] = tempAngles[i] +
int16_t((action[keyFrame][i] - tempAngles[i])
* divCounter / 20);
}
// サーボ信号出力
for(i = 0; i < 12; i++) {
angle[i] = (tempAngles1[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm1.setPWM(svPin[i], 0, anglePulse[i]);
}
for(i = 12; i < 24; i++) {
angle[i] = (tempAngles1[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm2.setPWM(svPin[i], 0, anglePulse[i]);
}
delay(30);
}
}
if ((divCounter > 20) && (prevbtnData != 1)) { //同じactionが連続する時のために前のtempAngles1[i]をtempAngles[i]に入れておく
for(int i = 0; i < 24; i++) {
tempAngles[i] = tempAngles1[i];
}
}
}else{ //通常のアクションモード。 アクション開始時のdivCounter >= action[nextKeyFrame][24]を満たした後
prevbtnData1 = 0; //1から0に戻す
for(int i = 0; i < 24; i++) { //通常のアクションモード中の角度計算(keyFrame = 0 nextKeyFrame = 1 からは)
tempAngles[i] = action[keyFrame][i] +
int16_t((action[nextKeyFrame][i] - action[keyFrame][i])
* divCounter / action[nextKeyFrame][24]);
}
// サーボ信号出力
for(i = 0; i < 12; i++) {
angle[i] = (tempAngles[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm1.setPWM(svPin[i], 0, anglePulse[i]);
}
for(i = 12; i < 24; i++) {
angle[i] = (tempAngles[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm2.setPWM(svPin[i], 0, anglePulse[i]);
}
delay(30);
}
} //if(actionMode != Stop)
if(actionMode == Stop){ //Stopを受け取った時 std_pose姿勢にゆっくり移行する(差分を20分割したステップで)
prevbtnData1 = 1;
divCounter++;
if(divCounter <= 20) {
// サーボ角度計算@Stopモード
for(int i = 0; i < 24; i++) { //tempAngles[i]はaction動作停止した時の角度または連続Stop時のstd_pose[i]
tempAngles1[i] = tempAngles[i] +
int16_t((std_pose[i] - tempAngles[i])
* divCounter / 20);
}
// サーボ信号出力
for(i = 0; i < 12; i++) {
angle[i] = (tempAngles1[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm1.setPWM(svPin[i], 0, anglePulse[i]);
}
for(i = 12; i < 24; i++) {
angle[i] = (tempAngles1[i]+offset[i])/10;
anglePulse[i] = map(angle[i], 144, -144, SERVOMIN, SERVOMAX);
pwm2.setPWM(svPin[i], 0, anglePulse[i]);
}
}
delay(30);
if (divCounter > 20) { //Stopが連続する時のため前のStopのstd_pose[i]をtempAngles[i]に入れておく
for(int i = 0; i < 24; i++) {
tempAngles[i] = std_pose[i];
}
}
}//if(actionMode == Stop){ // Stopモード
} //if (Ps3.isConnected()){
}//loop

#include <Ps3Controller.h>

/*PS3 Controllerからのデータを格納する変数*/
int v120 = 0; //1バイト目 リアルタイムでの取り込み値(BLE通信)
int vv120 = 0; //1バイト目 ボタンが押された時にこの変数に値を格納する(ボタン離したとき0値にならないように)
int ex120 = 0; //1バイト目 前回押されたボタン値を格納しておいて、次に押されたボタン値と比較する
// 変動してなかったら(=ボタンから指を離したままか押したままの時) vv120は0値でリセット

int v121 = 0; //2バイト目 リアルタイムでの取り込み値(BLE通信)
int vv121 = 0; //2バイト目 ボタンが押された時にこの変数に値を格納する
int ex121 = 0; //2バイト目 前回押されたボタン値を格納しておいて、次に押されたボタン値と比較する
// 変動してなかったら(=ボタンから指を離したままか押したままの時) vv121は0値でリセット


/* 使うピンの定義 */
#define AIN1 13 //左IN1 pwm@mode=0 DRV8835 IN/INモードで使う、phase/enableモードは空転がなく両方0の時にブレーキでモーター回らない
#define AIN2 12 //左IN2 pwm@mode=0 DRV8835
//#define BIN1 0 //右IN1 pwm@mode=0 DRV8835
//#define BIN2 4 //右IN2 pwm@mode=0 DRV8835
#define SERVO1 32 //steering servo

/* PWMチャンネルの定義 */
#define CHA 0 //AIN1 for 左motor
#define CHB 1 //AIN2 for 左motor
//#define CHC 2 //BIN1 for 右motor
//#define CHD 3 //BIN2 for 右motor
#define CHE 4 //for servo 1

/* モーター用PWMチャンネルの設定 */
const int LEDC_TIMER_BIT = 8; // PWMの範囲(8bitなら0〜255、10bitなら0〜1023)
const int LEDC_BASE_FREQ = 490; // 周波数(Hz)
const int VALUE_MAX = 255; // PWMの最大値

/* サーボ用PWMチャンネルの設定 */
const int LEDC_SERVO_TIMER_BIT = 10; // PWMの範囲(8bitなら0〜255、10bitなら0〜1023)
const int LEDC_SERVO_BASE_FREQ = 50; // 周波数(Hz)
const int VALUE_SERVO_MAX = 1023; // PWMの最大値

//モータースピード変数
int speedFL;
int speedBL;
//int speedFR;
//int speedBR;

//サーボ関連変数
// (26/1024)*20ms ≒ 0.5 ms (-90°)
// (74/1024)*20ms ≒ 1.45 ms (0°)
// (123/1024)*20ms ≒ 2.4 ms (+90°)
// 以下の値は 適宜 調整必要)
int posX = 70; // servo1 position (可変値)
int posX_ex = 70; // servo1 neutral position (中立角 固定値)
int servo_angle_min_X = 56; // servo1 minimum position(右最大振れ角の値)
int servo_angle_max_X = 84; // servo1 maximum position(左最大振れ角の値)

//セットアップ
void setup() {
Serial.begin(115200);
Ps3.begin("84:0D:8E:C1:6A:AE"); // My ESP32 Mac address
Serial.println("Ready");

//使うピンのpinModeセットアップ
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
//pinMode(BIN1, OUTPUT);
//pinMode(BIN2, OUTPUT);
pinMode(SERVO1, OUTPUT);

// PWMピンのセットアップ
ledcSetup(CHA, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch0 490 Hz 8bit resolution
ledcSetup(CHB, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch1 490 Hz 8bit resolution
//ledcSetup(CHC, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch0 490 Hz 8bit resolution
//ledcSetup(CHD, LEDC_BASE_FREQ, LEDC_TIMER_BIT); // ch1 490 Hz 8bit resolution
ledcSetup(CHE, LEDC_SERVO_BASE_FREQ, LEDC_SERVO_TIMER_BIT); // ch2 50 Hz 10bit resolution

// PWMピンのチャンネルをセット
ledcAttachPin(AIN1, CHA); //13 pin, ch0
ledcAttachPin(AIN2, CHB); //12 pin, ch1
//ledcAttachPin(BIN1, CHC); //0 pin, ch2
//ledcAttachPin(BIN2, CHD); //4 pin, ch3
ledcAttachPin(SERVO1, CHE); //32pin, ch4

//サーボの初期位置をセット
ledcWrite(CHE, posX_ex); // (中立)
//モーターの初期値をセット
ledcWrite(CHA, 0);
ledcWrite(CHB, 0);
//ledcWrite(CHC, 0);
//ledcWrite(CHD, 0);

} //end of setup


//Motor Drive の関数(DRV8833 Fast decay)
void forwardLeftMotor(int speedFL)//左正転
{
ledcWrite(CHA, 0);
ledcWrite(CHB, speedFL);
}
void backLeftMotor(int speedBL)//左逆転
{
ledcWrite(CHA, speedBL);
ledcWrite(CHB, 0);
}
/*void forwardRightMotor(int speedFR)//右正転
{
ledcWrite(CHC, speedFR);
ledcWrite(CHD, 0);
}
void backRightMotor(int speedBR)//右逆転
{
ledcWrite(CHC, 0);
ledcWrite(CHD, speedBR);
}*/
void stopLeftMotor() //停止(Brake)
{
ledcWrite(CHA, 255); //255
ledcWrite(CHB, 255); //255
}
/*void stopLeftMotor() //停止(空転)
{
ledcWrite(CHA, 0);
ledcWrite(CHB, 0);
}*/


void loop() {
if (Ps3.isConnected()){

if (Ps3.data.button.select){
Serial.println("select ボタン圧下");
v120 = 0;
v121 = 12;
if (v120 > 0) { vv120 = v120; } //if (v120 > 0)とif (v121 > 0)の意味は
if (v121 > 0) { vv121 = v121; } //ボタンを離した時(=0値)は　ボタン状態変化とみなさないため
}

if (Ps3.data.button.start){
Serial.println("start ボタン圧下");
v120 = 0;
v121 = 3;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.up ){
Serial.println("up ボタン圧下");
v120 = 0;
v121 = 1;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.right){
Serial.println("right ボタン圧下");
v120 = 0;
v121 = 4;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.down ){
Serial.println("down ボタン圧下");
v120 = 0;
v121 = 2;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.left){
Serial.println("left ボタン圧下");
v120 = 0;
v121 = 8;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.l2 ){
Serial.println("l2 ボタン圧下");
v120 = 4;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.r2){
Serial.println("r2 ボタン圧下");
v120 = 16;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.l1 ){
Serial.println("l1 ボタン圧下");
v120 = 2;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.r1){
Serial.println("r1 ボタン圧下");
v120 = 8;
v121 = 0;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.cross){
Serial.println("cross ボタン圧下");
v120 = 0;
v121 = 32;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.square){
Serial.println("square ボタン圧下");
v120 = 0;
v121 = 1;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.triangle){
Serial.println("triangle ボタン圧下");
v120 = 0;
v121 = 16;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.circle){
Serial.println("circle ボタン圧下");
v120 = 0;
v121 = 64;
if (v120 > 0) { vv120 = v120; }
if (v121 > 0) { vv121 = v121; }
}

if (Ps3.data.button.ps){
Serial.println("ps ボタン圧下");
}

if (Ps3.data.button.l3 ){
Serial.println("l3 ボタン圧下");
}

if (Ps3.data.button.r3){
Serial.println("r3 ボタン圧下");
}

//チャタリングなのか、新しいボタン入力なのか はんだんするための処理
if ((vv120!=ex120)&&(vv121!=ex121)) {
//ボタンが押された時のvv120とvv121値が　前回押されたボタンの時と違う場合　vv120とvv121を次回のためにex120とex121に格納しておく
ex120 = vv120;
ex121 = vv121;
}
if ((vv120==ex120)&&(vv121!=ex121)) {
//ボタンが押された時のvv121値だけが　前回押されたボタンの時と違う場合　vv120をリセットしvv120、vv121を次回のためにex120とex121に格納しておく
vv120 = 0;
ex120 = vv120;
ex121 = vv121;
}
if ((vv120!=ex120)&&(vv121==ex121)) {
//ボタンが押された時のvv120値だけが　前回押されたボタンの時と違う場合　vv121をリセットしvv120、vv121を次回のためにex120とex121に格納しておく
vv121 = 0;
ex120 = vv120;
ex121 = vv121;
}
//PS3 Controller Buttonの値をbtnData変数に入れる
int btnData = vv120*256 + vv121;
Serial.print("btnData =,");
Serial.println(btnData);

//ジョイスティックの値で動かすプログラムをここに書く
int LX = (Ps3.data.analog.stick.lx)+128;
int LY = 255-((Ps3.data.analog.stick.ly)+128);
int RX = (Ps3.data.analog.stick.rx)+128; //servo1 右Joystick左右方向 (left-right turn)
int RY = 255-((Ps3.data.analog.stick.ry)+128); //right motor 右Joystick上下方向 (motor power & forward-backward)

Serial.print(" LX=,");
Serial.print(LX);
Serial.print(",LY=,");
Serial.print(LY);
Serial.print(",RX=,");
Serial.print(RX);
Serial.print(",RY=,");
Serial.println(RY);


//PS3 Controller 右joystick上下の値でモーターを動かす

//R2ボタン 0x1000押されたとき // モータースピード 低速 操舵角85%
if (btnData == 4096){
if (RY>160) //Forward前進
//if (RY>128) //Forward前進
{
speedFL = 6*(RY-128)/5; //0.6x3.7v Vm
forwardLeftMotor(speedFL);
}

if (RY<96) //Backward後進
{
speedBL =6*(128-RY)/5 ;
backLeftMotor(speedBL);
}

if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}


if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/150; //操舵角85%
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/150; //操舵角85%
ledcWrite(CHE, posX);
}

if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}

//R1ボタン 0x0800押されたとき // モータースピード 高速 3.7v,操舵角85%
if (btnData == 2048){
if (RY>160) //Forward前進
{
speedFL = 2*(RY-128); //3.7v
forwardLeftMotor(speedFL);
}

if (RY<96) //Backward後進
{
speedBL = 2*(128-RY);
backLeftMotor(speedBL);
}

if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}

if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/150; //操舵角85%
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/150; //操舵角85%
ledcWrite(CHE, posX);
}

if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}

//L2ボタン 0x0400押されたとき // 低速、操舵角100%
if (btnData == 1024){
if (RY>160) //Forward前進
{
speedFL = 6*(RY-128)/5;
forwardLeftMotor(speedFL);
}

if (RY<96) //Backward後進
{
speedBL = 6*(128-RY)/5;
backLeftMotor(speedBL);
}

if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}

if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/128; //操舵角100%
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/128; //操舵角100%
ledcWrite(CHE, posX);
}

if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}

//L1ボタン 0x0200押されたとき // 高速、操舵角100%
if (btnData == 512){
if (RY>160) //Forward前進
{
speedFL = 2*(RY-128); //3.7v
forwardLeftMotor(speedFL);
}

if (RY<96) //Backward後進
{
speedBL = 2*(128-RY);
backLeftMotor(speedBL);
}

if (RY>=96 && RY<=160) //Stop motors
{
stopLeftMotor();
}

if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/128;
ledcWrite(CHE, posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/128;
ledcWrite(CHE, posX);
}

if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
ledcWrite(CHE, posX);
}
}

}
delay(50);
} //loop関数の終わり、loop関数のはじめに戻ってloop関数をエンドレスで繰り返す

//PS2 ワイヤレスコントローラーで動かす
//PS2ライブラリー
#include <PS2X_lib.h>

/******************************************************************
* set pins connected to PS2 controller:
* - 1e column: original
* - 2e colmun: Stef?
* replace pin numbers by the ones you use
******************************************************************/
#define PS2_DAT 13 //14
#define PS2_CMD 11 //15
#define PS2_SEL 10 //16
#define PS2_CLK 12 //17

/******************************************************************
* select modes of PS2 controller:
* - pressures = analog reading of push-butttons
* - rumble = motor rumbling
* uncomment 1 of the lines for each mode selection
******************************************************************/
//#define pressures true
#define pressures false
//#define rumble true
#define rumble false

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

// Reset func
void (* resetFunc) (void) = 0;


//モーターシールドとArduinoのピンコネクション
#define DIRA1 7
#define DIRA2 8
#define PWMA 5

/***************************************************
Servo run forward(512), and backward(102) depending on LX,LY position....
****************************************************/
//サーボ関連ライブラリー
#include <Servo.h>
Servo myservo1; // create servo object to control a servo
// twelve servo objects can be created on most boards
int posX = 113; // variable to store the servo position
int posX_ex =113;
int servo_angle_min_X = 94; //right
int servo_angle_max_X = 127; //left

//モータースピード変数
int speedFL;
int speedBL;

//ここからsetup関数

void setup(){
//PS2 コントロール
Serial.begin(57600);
// Serial.begin(115200);
delay(500); //added delay to give wireless ps2 module some time to startup, before configuring it

//CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************

//setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

if(error == 0){
Serial.print("Found Controller, configured successful ");
Serial.print("pressures = ");
if (pressures)
Serial.println("true ");
else
Serial.println("false");
Serial.print("rumble = ");
if (rumble)
Serial.println("true)");
else
Serial.println("false");
Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
Serial.println("holding L1 or R1 will print out the analog stick values.");
Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
}
else if(error == 1)
Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

else if(error == 2)
Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

else if(error == 3)
Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

type = ps2x.readType();
switch(type) {
case 0:
Serial.println("Unknown Controller type found ");
break;
case 1:
Serial.println("DualShock Controller found ");
break;
case 2:
Serial.println("GuitarHero Controller found ");
break;
case 3:
Serial.println("Wireless Sony DualShock Controller found ");
break;
}

//モーター駆動用ピン出力
pinMode(DIRA1, OUTPUT);
pinMode(DIRA2, OUTPUT);
pinMode(PWMA, OUTPUT);


//サーボのセットアップ
myservo1.attach(9); // attaches the servo on pin 9 to the servo object
myservo1.write(posX_ex);

} //setup関数の最後


//Motor Drive の関数

void forwardLeftMotor(int speedFL)//正転
{
digitalWrite(DIRA1,HIGH);
digitalWrite(DIRA2,LOW);
analogWrite(PWMA, speedFL);
}

void backLeftMotor(int speedBL)//逆転
{
digitalWrite(DIRA1,LOW);
digitalWrite(DIRA2,HIGH);
analogWrite(PWMA, speedBL);
}

void stopLeftMotor() //停止
{
digitalWrite(DIRA1,HIGH);
digitalWrite(DIRA2,HIGH);
analogWrite(PWMA, 0);
}


// loop関数の始まり
void loop(){
/* You must Read Gamepad to get new values and set vibration values
ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
if you don't enable the rumble, use ps2x.read_gamepad(); with no values
You should call this at least once a second
*/
if(error == 1){ //skip loop if no controller found
resetFunc();
}

if(type == 2){ //Guitar Hero Controller
ps2x.read_gamepad(); //read controller
}
else { //DualShock Controller
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);

//Joystickの位置を読んで変数に代入する
int LY=ps2x.Analog(PSS_LY);
int LX=ps2x.Analog(PSS_LX);
int RY=ps2x.Analog(PSS_RY);
int RX=ps2x.Analog(PSS_RX);

if (RY<118) //Forward前進
{
speedFL = 2*(127-RY);
forwardLeftMotor(speedFL); //過電圧(5v)が130モーターにかかり続けないよう擬似的に50%maxでPWMさせる
}

if (RY>138) //右Backward後進 Fast dekay (smaller torque)DRV8833
{
speedBL =2*(RY-128) ;
backLeftMotor(speedBL); //過電圧(5v)が130モーターにかかり続けないよう擬似的に55%maxでPWMさせる
}

if (RY>=118 && RY<=138) //Stop motor
{
stopLeftMotor();
}



/* driving servoes*/
//サーボの 駆動

if (LX>138) { //right turn
posX= posX_ex-(LX-128)*(posX_ex - servo_angle_min_X)/128;
myservo1.write(posX);
}
if (LX<118) { //left turn
posX= posX_ex+(128-LX)*(servo_angle_max_X - posX_ex)/128;
myservo1.write(posX);
}
if (LX>=118 && LX<=138) { //neutral
posX= posX_ex;
myservo1.write(posX);
}


} //else Dualshock controllerのブレース
delay(50);

} //loop関数の終わり、loop関数のはじめに戻ってloop関数をエンドレスで繰り返す

/******************************************************************/
/* Futabaのサンプルプログラム(for Arduino)を若干変更して使用。*/
/* https://www.futaba.co.jp/img/uploads/files/robot/Futaba_Sample_Arduino_RS30x_Program_rev2.pdf
/* ボード ：ESP32 Dev Module, UART2(Tx:GPIO17) */
/* 電源投入順序 */
/* 1. サーボ, (ID=1 工場出荷状態)*/
/* 2. ESP32 */
/******************************************************************/
//int REDE = 2; // デジタルPin2 をRS485出力イネーブルピンに設定
/*-------------------------------------------------*/
/* 機能 : Arduino 初期化 */
/* 名前 : setup */
/* 引数 : 無し */
/* 戻り値 : 無し */
/*-------------------------------------------------*/

void setup (){
//pinMode(REDE, OUTPUT); // デジタルPin2(REDE)を出力に設定
Serial.begin(115200);
Serial2.begin(115200); // ボーレート 115,200bps
delay(500); // 500msディレイ
}
/*-------------------------------------------------*/
/* 機能 : サーボトルク設定 */
/* 名前 : torque */
/* 引数 : ID (Servo ID) */
/* : data (Torque enable) */
/* 戻り値 : 無し */
/*-------------------------------------------------*/
void torque (unsigned char ID, unsigned char data){
unsigned char TxData[9]; //送信データバッファ [9byte]
unsigned char CheckSum = 0; // チェックサム計算用変数
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x24; // Address
TxData[5] = 0x01; // Length
TxData[6] = 0x01; // Count
TxData[7] = data; // Data
// チェックサム計算
for(int i=2; i<=7; i++){
CheckSum = CheckSum ^ TxData[i]; // ID～DATAまでのXOR
}
TxData[8] = CheckSum; //Sum
// パケットデータ送信
//digitalWrite(REDE, HIGH); // RS485 送信許可 (TTLの場合は必要無し)
for(int i=0; i<=8; i++){
Serial2.write(TxData[i]);
}
delayMicroseconds(250); // データ送信完了待ち
//digitalWrite(REDE, LOW); // RS485 送信禁止 (TTLの場合は必要無し)
}
/*-------------------------------------------------*/
/* 機能 : サーボ角度・速度指定 */
/* 名前 : Move_SV */
/* 引数 : ID (Servo ID) */
/* : Angle (Present Posion L&H) */
/* : Speed (Present Time L&H) */
/* 戻り値 : --- */
/*-------------------------------------------------*/
void Move_SV (unsigned char ID, int Angle, int Speed){
unsigned char TxData[12]; //送信データバッファ [12byte]
unsigned char CheckSum = 0; // チェックサム計算用変数
TxData[0] = 0xFA; // Header
TxData[1] = 0xAF; // Header
TxData[2] = ID; // ID
TxData[3] = 0x00; // Flags
TxData[4] = 0x1E; // Address
TxData[5] = 0x04; // Length
TxData[6] = 0x01; // Count
// Angle
TxData[7] = (unsigned char)0x00FF & Angle; // Low byte
TxData[8] = (unsigned char)0x00FF & (Angle >> 8); //Hi byte
// Speed
TxData[9] = (unsigned char)0x00FF & Speed; // Low byte
TxData[10] = (unsigned char)0x00FF & (Speed >> 8); //Hi byte
// チェックサム計算
for(int i=2; i<=10; i++){
CheckSum = CheckSum ^ TxData[i]; // ID～DATAまでのXOR
}
TxData[11] = CheckSum; //Sum
// パケットデータ送信
//digitalWrite(REDE, HIGH); // RS485 送信許可 (TTLの場合は必要無し)
for(int i=0; i<=11; i++){
Serial2.write(TxData[i]);
}
delayMicroseconds(250); // データ送信完了待ち
//digitalWrite(REDE, LOW); // RS485 送信禁止 (TTLの場合は必要無し)
}
/*-------------------------------------------------*/
/* 機能 : メインプログラム */
/* 名前 : loop */
/* 引数 : 無し */
/* 戻り値 : 無し */
/*-------------------------------------------------*/
void loop (){
Serial.println("Demo test");
unsigned char i = 0; // for文用
//setup (); // Arduino 初期化
torque(0x01, 0x01); // ID = 1(0x01) torque = OFF(0x00), ON(0x01), BRAKE(0x02)
delay(1000); // wait (1sec)
//while(1){
//for(i=0; i<=20; i++){
Move_SV(0x01,300,100); //ID = 1～20 , GoalPosition = 30.0deg(300) , Time = 1.0sec(100)
delay(500); // wait (0.5sec)
//}
delay(500); // wait (0.5sec)
Move_SV(0x01,-300,100); //ID = 1(0x01) , GoalPosition = -30.0deg(-300) , Time = 1.0sec(100)
delay(2000); // wait (2.0sec)
//}
}

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pca9685 = Adafruit_PWMServoDriver(0x40);
#define SERVOMIN 114 // this is the 'minimum' pulse length count (out of 4096) for 0°最小パルス幅
#define SERVOMAX 488 // this is the 'maximum' pulse length count (out of 4096) for 180°最大パルス幅
#define SERVOMID 301
int16_t angle[16];
int16_t anglePulse[16];
uint8_t i;
uint8_t GyroOn;
// 端子設定
uint8_t svPin[16];
uint8_t interruptPin = 5;
uint8_t ledPin = 18;
// 動作関連
#define Stop 0
#define Standard_pose 1
#define Hans_up 2
#define Bow_pose 3
#define Sit_down_pose 4
#define Sit_hans_up 5
#define Forward_walk 6
#define Backward_walk 7
#define Left_shift 8
#define Right_shift 9
#define Left_turn 10
#define Right_turn 11
#define Left_right_shoot 12
#define Double_arms_pushing_up 13
#define Left_attack 14
#define Right_attack 15
#define Left_forward_attack 16
#define Right_forward_attack 17
#define Fast_walk 18
#define Wing_move 19
#define Handstanding 20
#define Single_left_arm_push_up 21
#define Single_right_arm_push_up 22
#define Forward_stand_up 23
#define Backward_stand_up 24
#define Forward_tumbling 25
#define Left_tumbling 26
#define Right_tumbling 27
#define Backward_tumbling 28
//#define Left_foot_back_attack 27
//#define Right_foot_back_attack 28

// 動作関連
int16_t offset[] = { -12, 9,-56,-17,-12, -12,-77,-32, -13, 57, 12, -11,-32, 33, -3,-12}; //my robonova offset
int16_t tempAngles[16] = { 100,100,100,100,100, 100,100,100, 100,100,100, 100,100,100,100,100}; //standard_pose
int16_t tempAngles1[16] = { 100,100,100,100,100, 100,100,100, 100,100,100, 100,100,100,100,100};
int16_t action[32][17] = {
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60},
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60}
};
//回転方向　1:正 0:逆 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
//パターン補正量 0 -24 +45 +7 0 0 +70 +20 0 -70 -20 0 +24 -45 -7 0
//G6A________________ G6B________ G6C________ G6D________________
int16_t standard_pose[][17]= {// 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60 },
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60 }
};
int16_t hans_up[][17] = {
{ 100, 100, 100, 100, 100, 100, 238, 170, 100, -38, 30, 100, 100, 100, 100, 100, 60 },
{ 100, 100, 100, 100, 100, 100, 238, 170, 100, -38, 30, 100, 100, 100, 100, 100, 60 }
};
int16_t bow_pose[][17] = {
{ 100, 118, 110, 167, 100, 100, 100, 100, 100, 100, 100, 100, 82, 90, 33, 100, 60 },
{ 100, 118, 110, 167, 100, 100, 100, 100, 100, 100, 100, 100, 82, 90, 33, 100, 60 }
};
int16_t sit_down_pose[][17] = {
{ 100, 25, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 175, -22, 53, 101, 60 },
{ 100, 25, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 175, -22, 53, 101, 60 }
};
int16_t sit_hans_up[][17] = {
{ 100, 25, 222, 147, 99, 100, 238, 170, 100, -38, 30, 100, 175, -22, 53, 101, 60 },
{ 100, 25, 222, 147, 99, 100, 238, 170, 100, -38, 30, 100, 175, -22, 53, 101, 60 }
};
int16_t forward_walk[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 90, 69, 140, 112, 86, 90, 110, 100, 100, 90, 100, 90, 100, 100, 100, 90, 5}, //SPEED 14 left up
{ 90, 120, 102, 129, 86, 80, 110, 100, 95, 90, 100, 90, 104, 100, 103, 90, 5}, //SPEED 14 left down
{ 90, 130, 82, 119, 86, 80, 110, 100, 95, 90, 100, 91, 104, 100, 103, 90, 5}, //SPEED 14
{ 100, 110, 104, 120, 100, 90, 110, 100, 100, 90, 100, 100, 107, 111, 113, 100, 9}, //SPEED 10 left center
{ 110, 98, 103, 112, 110, 100, 110, 100, 100, 90, 100, 110, 126, 91, 108, 114, 9}, //SPEED 10
{ 110, 100, 100, 100, 110, 100, 110, 100, 110, 90, 100, 110, 131, 60, 88, 114, 5}, //SPEED 14 right up
{ 110, 96, 100, 97, 110, 105, 110, 100, 120, 90, 100, 110, 80, 98, 71, 114, 5}, //SPEED 14 right down
{ 109, 96, 100, 97, 110, 105, 110, 100, 120, 90, 100, 110, 70, 118, 81, 114, 5}, //SPEED 14
{ 100, 93, 89, 87, 100, 100, 110, 100, 110, 90, 100, 100, 90, 96, 80, 100, 9}, //SPEED 10 right center
{ 90, 74, 109, 92, 86, 100, 110, 100, 100, 90, 100, 90, 102, 97, 88, 90, 9} //SPEED 10
};
int16_t backward_walk[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 90, 69, 140, 112, 86, 90, 110, 100, 100, 90, 100, 90, 102, 100, 100, 90, 7}, //SPEED 13 left up
{ 90, 80, 109, 92, 86, 100, 110, 100, 100, 90, 100, 90, 100, 97, 88, 90, 12}, //SPEED 7
{ 100, 88, 105, 93, 100, 100, 110, 100, 110, 90, 100, 100, 86, 101, 85, 100, 12}, //SPEED 7 right center
{ 110, 96, 100, 97, 110, 105, 110, 100, 120, 90, 100, 110, 80, 98, 71, 114, 12}, //SPEED 7 right down
{ 110, 98, 100, 100, 110, 100, 110, 100, 110, 90, 100, 110, 131, 60, 88, 114, 6}, //SPEED 13 right up
{ 110, 100, 103, 112, 110, 100, 110, 100, 100, 90, 100, 110, 120, 91, 108, 114, 12}, //SPEED 7
{ 100, 114, 99, 115, 100, 90, 110, 100, 100, 90, 100, 100, 112, 95, 107, 100, 12}, //SPEED 7 left center
{ 90, 120, 102, 129, 86, 80, 110, 100, 95, 90, 100, 90, 104, 100, 103, 90, 12}, //SPEED 7 left down
}; //
int16_t left_shift[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 85, 105, 93, 98, 88, 100, 110, 100, 100, 90, 100, 88, 100, 100, 100, 92, 14}, //SPEED 5
{ 76, 104, 85, 89, 72, 100, 105, 110, 100, 95, 90, 90, 116, 79, 96, 93, 10}, //SPEED 9
{ 93, 100, 100, 101, 91, 100, 105, 110, 100, 95, 90, 107, 100, 100, 99, 109, 10}, //SPEED 9
{ 110, 84, 121, 104, 107, 100, 105, 110, 100, 95, 90, 124, 96, 115, 111, 128, 10}, //SPEED 9
{ 113, 98, 100, 100, 107, 100, 110, 100, 110, 90, 100, 114, 107, 90, 96, 114, 10}, //SPEED 9
{ 112, 100, 100, 100, 108, 100, 110, 100, 100, 90, 100, 115, 95, 107, 102, 112, 10} //SPEED 9
}; //
int16_t right_shift[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 112, 100, 100, 100, 108, 100, 110, 100, 100, 90, 100, 115, 95, 107, 102, 112, 14}, //SPEED 5
{ 110, 84, 121, 104, 107, 100, 105, 110, 100, 95, 90, 124, 96, 115, 111, 128, 10}, //SPEED 9
{ 93, 100, 100, 101, 91, 100, 105, 110, 100, 95, 90, 107, 100, 100, 99, 109, 10}, //SPEED 9
{ 76, 104, 85, 89, 72, 100, 105, 110, 100, 95, 90, 90, 116, 79, 96, 93, 10}, //SPEED 9
{ 86, 93, 110, 104, 86, 90, 110, 100, 100, 90, 100, 87, 102, 100, 100, 93, 10}, //SPEED 9
{ 85, 105, 93, 98, 88, 100, 110, 100, 100, 90, 100, 88, 100, 100, 100, 92, 10} //SPEED 9
}; //
int16_t left_turn[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 112, 100, 100, 100, 108, 100, 110, 100, 100, 90, 100, 115, 90, 107, 102, 112, 13}, //SPEED 6
{ 112, 101, 100, 104, 107, 105, 110, 90, 110, 90, 110, 110, 69, 112, 78, 112, 10}, //SPEED 9
{ 108, 98, 100, 105, 107, 105, 110, 90, 120, 90, 110, 105, 62, 124, 83, 110, 10}, //SPEED 9
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 80, 100, 100, 100, 100, 100, 10}, //SPEED 9
{ 85, 105, 93, 98, 88, 100, 110, 100, 100, 90, 100, 88, 100, 100, 100, 92, 12}, //SPEED 7
{ 95, 96, 76, 74, 90, 105, 110, 90, 110, 90, 110, 87, 99, 100, 101, 93, 10}, //SPEED 9
{ 95, 96, 76, 74, 90, 105, 110, 90, 120, 90, 110, 92, 102, 100, 100, 93, 10}, //SPEED 9
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 80, 100, 100, 100, 100, 100, 9} //SPEED 10
}; //
int16_t left_back_turn[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 85, 105, 93, 98, 88, 100, 110, 100, 100, 90, 100, 88, 100, 100, 100, 92, 15}, //SPEED 4
{ 95, 96, 76, 74, 90, 105, 110, 90, 110, 90, 110, 87, 99, 100, 101, 93, 13}, //SPEED 6
{ 95, 96, 76, 74, 90, 105, 110, 90, 120, 90, 110, 92, 102, 100, 100, 93, 13}, //SPEED 6
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 80, 100, 100, 100, 100, 100, 9} //SPEED 10
}; //
int16_t right_turn[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 85, 110, 93, 98, 88, 100, 110, 100, 100, 90, 100, 88, 100, 100, 100, 92, 13}, //SPEED 6
{ 90, 131, 88, 122, 88, 90, 110, 90, 95, 90, 110, 87, 99, 100, 96, 93, 10}, //SPEED 9
{ 95, 138, 76, 117, 90, 80, 110, 90, 95, 90, 110, 92, 102, 100, 95, 93, 10}, //SPEED 9
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 80, 100, 100, 100, 100, 100, 10}, //SPEED 9
{ 112, 100, 100, 100, 108, 100, 110, 100, 100, 90, 100, 115, 95, 107, 102, 112, 12}, //SPEED 7
{ 113, 101, 100, 99, 107, 90, 110, 90, 95, 90, 110, 105, 104, 124, 126, 110, 10}, //SPEED 9
{ 108, 98, 100, 100, 107, 80, 110, 90, 95, 90, 110, 105, 104, 124, 126, 110, 10}, //SPEED 9
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 80, 100, 100, 100, 100, 100, 9} //SPEED 10
}; //
int16_t right_back_turn[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 112, 100, 100, 100, 108, 100, 110, 100, 100, 90, 100, 115, 95, 107, 102, 112, 15}, //SPEED 4
{ 113, 101, 100, 99, 107, 90, 110, 90, 95, 90, 110, 105, 104, 124, 126, 110, 13}, //SPEED 6
{ 108, 98, 100, 100, 107, 80, 110, 90, 95, 90, 110, 105, 104, 124, 126, 110, 13}, //SPEED 6
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 80, 100, 100, 100, 100, 100, 9} //SPEED 10
}; //
int16_t left_right_shoot[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 70, 123, 65, 86, 98, 90, 115, 90, 90, 85, 110, 89, 77, 135, 114, 104, 15}, //SPEED 4
{ 90, 89, 118, 72, 84, 140, 115, 90, 140, 85, 110, 86, 81, 135, 114, 95, 13}, //SPEED 6
{ 90, 89, 118, 154, 84, 60, 115, 90, 60, 85, 110, 86, 73, 135, 114, 95, 5}, //SPEED 14
{ 90, 89, 118, 154, 84, 60, 115, 90, 60, 85, 110, 86, 73, 135, 114, 95, 30}, //wait500ms
{ 70, 100, 100, 100, 96, 90, 115, 90, 90, 85, 110, 86, 100, 100, 100, 102, 14}, //SPEED 5
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 80, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 40}, //wait
{ 111, 123, 65, 86, 96, 110, 115, 90, 110, 85, 110, 130, 77, 135, 114, 102, 15}, //SPEED 4
{ 114, 119, 65, 86, 105, 80, 115, 90, 80, 85, 110, 110, 111, 82, 128, 116, 13}, //SPEED 6
{ 114, 127, 65, 86, 105, 140, 115, 90, 140, 85, 110, 110, 111, 82, 46, 116, 5}, //SPEED 14
{ 114, 127, 65, 86, 105, 140, 115, 90, 140, 85, 110, 110, 111, 82, 46, 116, 30}, //wait500ms
{ 114, 100, 100, 100, 98, 110, 115, 90, 110, 85, 110, 130, 100, 100, 100, 104, 14}, //SPEED 5
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait
}; //
int16_t left_attack[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 85, 108, 93, 98, 93, 130, 110, 100, 70, 90, 100, 92, 97, 100, 100, 100, 12}, //SPEED 7
{ 107, 15, 224, 132, 107, 189, 110, 97, 150, 58, 94, 134, 184, 40, 128, 130, 7}, //SPEED 12
{ 107, 15, 224, 132, 107, 189, 110, 97, 150, 58, 94, 134, 184, 40, 128, 130, 40}, //wait
{ 100, 28, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 172, -22, 53, 101, 9}, //SPEED 10
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 40}, //wait
{ 85, 106, 93, 98, 93, 100, 110, 100, 100, 90, 100, 92, 99, 100, 100, 100, 12}, //SPEED 7
{ 98, 20, 225, 141, 90, 107, 205, 128, 88, 38, 81, 143, 138, 32, 68, 134, 7}, //SPEED 12
{ 98, 20, 225, 141, 90, 107, 205, 128, 88, 38, 81, 143, 138, 32, 68, 134, 40}, //wait
{ 100, 26, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 174, -22, 53, 101, 4}, //SPEED 15
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait
}; //
int16_t right_attack[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 108, 103, 100, 100, 100, 70, 110, 100, 70, 90, 100, 115, 92, 107, 102, 107, 12}, //SPEED 7
{ 66, 16, 160, 72, 70, 50, 142, 106, 11, 90, 103, 93, 185, -24, 68, 93, 7}, //SPEED 12
{ 66, 16, 160, 72, 70, 50, 142, 106, 11, 90, 103, 93, 185, -24, 68, 93, 40}, //wait
{ 100, 28, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 172, -22, 53, 101, 9}, //SPEED 10
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 40}, //wait
{ 108, 101, 100, 100, 100, 100, 110, 100, 100, 90, 100, 115, 94, 107, 102, 107, 12}, //SPEED 7
{ 57, 62, 168, 132, 66, 112, 162, 119, 93, -5, 72, 102, 180, -25, 59, 110, 7}, //SPEED 12
{ 57, 62, 168, 132, 66, 112, 162, 119, 93, -5, 72, 102, 180, -25, 59, 110, 40}, //wait
{ 100, 26, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 174, -22, 53, 101, 4}, //SPEED 15
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait
}; //
int16_t left_forward_attack[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 85, 108, 93, 98, 93, 130, 110, 100, 70, 90, 100, 92, 97, 100, 100, 100, 12}, //SPEED 7
{ 107, 15, 224, 132, 107, 189, 110, 97, 150, 58, 94, 134, 184, 40, 128, 130, 7}, //SPEED 12
{ 107, 15, 224, 132, 107, 189, 110, 97, 150, 58, 94, 134, 184, 40, 128, 130, 40}, //wait
{ 100, 28, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 172, -22, 53, 101, 9}, //SPEED 10
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait
}; //
int16_t right_forward_attack[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 108, 103, 100, 100, 100, 70, 110, 100, 70, 90, 100, 115, 92, 107, 102, 107, 12}, //SPEED 7
{ 66, 16, 160, 72, 70, 50, 142, 106, 11, 90, 103, 93, 185, -24, 68, 93, 7}, //SPEED 12
{ 66, 16, 160, 72, 70, 50, 142, 106, 11, 90, 103, 93, 185, -24, 68, 93, 40}, //wait
{ 100, 28, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 172, -22, 53, 101, 9}, //SPEED 10
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 3
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait
}; //
int16_t fast_walk[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 100, 100, 100, 100, 100, 100, 110, 100, 100, 90, 100, 100, 100, 100, 100, 9}, //SPEED 10
{ 90, 104, 97, 100, 90, 100, 100, 110, 100, 100, 90, 92, 99, 100, 100, 95, 12}, //SPEED 7rightG
{ 90, 81, 140, 122, 90, 90, 100, 110, 90, 100, 90, 88, 99, 100, 100, 95, 4}, //SPEED 15rightG left up
//
{ 100, 96, 126, 125, 94, 80, 100, 110, 80, 100, 90, 95, 99, 100, 100, 100, 4}, //SPEED 15left leg up
{ 105, 102, 105, 113, 100, 100, 100, 110, 100, 100, 90, 105, 129, 79, 100, 106, 4}, //SPEED 15right up0
{ 105, 101, 100, 100, 100, 120, 100, 110, 120, 100, 90, 100, 104, 74, 75, 106, 4}, //SPEED 15right leg up
{ 95, 71, 121, 100, 94, 100, 100, 110, 100, 100, 90, 95, 98, 95, 87, 100, 4}, //SPEED 15left up0
//
{ 100, 96, 126, 125, 94, 80, 100, 110, 80, 100, 90, 95, 99, 100, 100, 100, 3}, //SPEED 15
{ 105, 102, 105, 113, 100, 100, 100, 110, 100, 100, 90, 105, 129, 79, 100, 106, 3}, //SPEED 15
{ 105, 101, 100, 100, 100, 120, 100, 110, 120, 100, 90, 100, 104, 74, 75, 106, 3}, //SPEED 15
{ 95, 71, 121, 100, 94, 100, 100, 110, 100, 100, 90, 95, 98, 95, 87, 100, 3}, //SPEED 15
{ 100, 96, 126, 125, 94, 80, 100, 110, 80, 100, 90, 95, 99, 100, 100, 100, 3}, //SPEED 15
{ 105, 102, 105, 113, 100, 100, 100, 110, 100, 100, 90, 105, 129, 79, 100, 106, 3}, //SPEED 15
{ 105, 101, 100, 100, 100, 120, 100, 110, 120, 100, 90, 100, 104, 74, 75, 106, 3}, //SPEED 15
{ 95, 71, 121, 100, 94, 100, 100, 110, 100, 100, 90, 95, 98, 95, 87, 100, 3}, //SPEED 15
{ 100, 96, 126, 125, 94, 80, 100, 110, 80, 100, 90, 95, 99, 100, 100, 100, 3}, //SPEED 15
{ 105, 102, 105, 113, 100, 100, 100, 110, 100, 100, 90, 105, 129, 79, 100, 106, 3}, //SPEED 15
{ 105, 101, 100, 100, 100, 120, 100, 110, 120, 100, 90, 100, 104, 74, 75, 106, 3}, //SPEED 15
{ 95, 71, 121, 100, 94, 100, 100, 110, 100, 100, 90, 95, 98, 95, 87, 100, 3}, //SPEED 15
{ 85, 96, 115, 102, 94, 80, 100, 110, 80, 100, 90, 92, 97, 100, 100, 100, 4}, //SPEED 8rightGleft leg up0
{ 90, 104, 97, 100, 90, 80, 100, 110, 80, 100, 90, 92, 99, 100, 100, 93, 12}, //SPEED 8rightG
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14} //SPEED 5
}; //
int16_t wing_move[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 85, 105, 93, 98, 88, 100, 110, 100, 100, 90, 100, 88+3, 100, 100, 100, 92, 30}, //slow
{ 90, 78, 140, 122, 85, 100, 220, 170, 100, -20, 30, 84+3, 98, 100, 95, 93, 30}, //slow
{ 90, 55, 209, 112, 85, 100, 220, 170, 100, -20, 30, 84+3, 84-2, 101, 55, 93, 30}, //slow
{ 90, 78, 140, 71, 85, 145, 180, 130, 55, 20, 70, 84+3, 74-2, 115, 40, 93, 30}, //slow
{ 90, 78, 140, 71, 85, 145, 150, 100, 55, 50, 100, 84+3, 74-2, 115, 33, 93, 9}, //SPEED 10
{ 90, 78, 140, 71, 85, 145, 190, 140, 55, 10, 60, 84+3, 74-2, 115, 33, 93, 9}, //SPEED 10
{ 90, 78, 140, 71, 85, 145, 150, 100, 55, 50, 100, 84+3, 74-2, 115, 33, 93, 8}, //SPEED 11
{ 90, 78, 140, 71, 85, 145, 190, 140, 55, 10, 60, 84+3, 74-2, 115, 33, 93, 8}, //SPEED 11
{ 90, 78, 140, 71, 85, 145, 150, 100, 55, 50, 100, 84+3, 74-2, 115, 33, 93, 7}, //SPEED 12
{ 90, 78, 140, 71, 85, 145, 190, 140, 55, 10, 60, 84+3, 74-2, 115, 33, 93, 7}, //SPEED 12
{ 90, 78, 140, 71, 85, 145, 150, 100, 55, 50, 100, 84+3, 74-2, 115, 33, 93, 6}, //SPEED 13
{ 90, 78, 140, 71, 85, 145, 190, 140, 55, 10, 60, 84+3, 74-2, 115, 33, 93, 6}, //SPEED 13
{ 90, 78, 140, 71, 85, 145, 150, 100, 55, 50, 100, 84+3, 74-2, 115, 33, 93, 5}, //SPEED 14
{ 90, 78, 140, 71, 85, 145, 190, 140, 55, 10, 60, 84+3, 74-2, 115, 33, 93, 5}, //SPEED 14
{ 90, 78, 140, 71, 85, 145, 150, 100, 55, 50, 100, 84+3, 74-2, 115, 33, 93, 4}, //SPEED 15
{ 90, 78, 140, 71, 85, 145, 190, 140, 55, 10, 60, 84+3, 74-2, 115, 33, 93, 4}, //SPEED 15
{ 90, 78, 140, 71, 85, 145, 190, 140, 55, 10, 60, 84+3, 74-2, 115, 33, 93, 20}, //wait
{ 90, 78, 140, 71, 85, 145, 180, 130, 55, 20, 70, 84+3, 74-2, 115, 36, 93, 14}, //SPEED 5
{ 90, 55, 209, 112, 85, 100, 220, 170, 100, -20, 30, 84+3, 84-2, 101, 55, 93, 14}, //SPEED 5
{ 90, 78, 140, 122, 85, 100, 220, 170, 100, -20, 30, 84+3, 98, 100, 95, 93, 14}, //SPEED 5
{ 85, 105, 93, 98, 88, 100, 110, 100, 100, 90, 100, 88+3, 100, 100, 100, 92, 14}, //SPEED 5
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait
}; //
int16_t handstanding[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 30}, //Slow
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 140, 30}, //Slow
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 30}, //Slow
{ 75, 11, 190, 172, 45, 185, 80, 120, 15, 120, 80, 125, 189, 10, 28, 155, 30}, //Slow
{ 80, 21, 160, 157, 50, 185, 110, 80, 15, 90, 120, 120, 179, 40, 43, 150, 30}, //Slow
{ 100, 46, 125, 87, 90, 125, 230, 32, 75, -30, 168, 100, 154, 75, 113, 110, 30}, //Slow
// fall forwaed
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 51, 180, 17, 100, 110, 100, 100, 90, 100, 100, 100, 149, 20, 183, 100, 13}, //SPEED 6
{ 100, 51, 180, 17, 100, 170, 100, 100, 30, 100, 100, 100, 149, 20, 183, 100, 16}, //SPEED 3
{ 100, 51, 180, 17, 100, 170, 100, 100, 30, 100, 100, 100, 149, 20, 183, 100, 20}, //wait 200
{ 100, 87, 116, 64, 100, 180, 100, 100, 13, 100, 100, 100, 113, 84, 136, 100, 13}, //SPEED 6
{ 100, 112, 66, 64, 100, 190, 120, 100, 13, 80, 100, 100, 88, 134, 136, 100, 13}, //SPEED 6
{ 100, 112, 66, 64, 100, 190, 120, 100, 13, 80, 100, 100, 88, 134, 136, 100, 40}, //wait 2000
{ 100, 112, 66, 64, 100, 190, 120, 100, 13, 80, 100, 100, 88, 134, 136, 100, 13}, //SPEED 6
{ 100, 87, 116, 64, 100, 180, 100, 100, 13, 100, 100, 100, 113, 84, 136, 100, 13}, //SPEED 6
{ 100, 51, 180, 17, 100, 170, 100, 100, 30, 100, 100, 100, 149, 20, 183, 100, 16}, //SPEED 3
{ 100, 51, 180, 17, 100, 110, 100, 100, 90, 100, 100, 100, 149, 20, 183, 100, 13}, //SPEED 6
// foot up2
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 46, 125, 87, 90, 150, 230, 32, 50, -30, 168, 100, 154, 75, 113, 110, 12}, //SPEED 7
{ 80, 21, 160, 157, 50, 185, 110, 80, 13, 90, 120, 120, 179, 40, 43, 150, 12}, //SPEED 7
{ 75, 11, 190, 172, 45, 185, 80, 120, 13, 120, 80, 125, 189, 10, 28, 155, 12}, //SPEED 7
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 12}, //SPEED 7
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 145, 12}, //SPEED 7
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; // forward stand up
int16_t single_left_arm_push_up[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 30}, //Slow
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 140, 30}, //Slow
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 30}, //Slow
{ 75, 11, 190, 172, 45, 185, 80, 120, 15, 120, 80, 125, 189, 10, 28, 155, 30}, //Slow
{ 80, 21, 160, 157, 50, 185, 110, 80, 15, 90, 120, 120, 179, 40, 43, 150, 30}, //Slow
{ 100, 46, 125, 87, 90, 125, 230, 32, 75, -30, 168, 100, 154, 75, 113, 110, 30}, //Slow
// fall forwaed
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 100, 100, 100, 100, 100, 220, 40, 100, -20, 160, 100, 100, 100, 100, 100, 11}, //SPEED 8
{ 100, 100, 100, 100, 100, 180, 220, 40, 20, -20, 160, 100, 100, 100, 100, 100, 11}, //SPEED 8
{ 100, 100, 100, 100, 100, 180, 120, 40, 20, 80, 160, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 120, 40, 20, 80, 160, 100, 99, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 120, 40, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 130, 32, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 95, 90, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 130, 32, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 95, 90, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 130, 32, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 95, 90, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 130, 32, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 95, 90, 20, 10, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 130, 32, 20, 70, 168, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 180, 120, 40, 20, 80, 160, 100, 100, 100, 80, 145, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 103, 100, 100, 97, 100, 100, 100, 100, 100, 16}, //SPEED 3
// single_left_arm_push_up
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 46, 125, 87, 90, 150, 230, 32, 50, -30, 168, 100, 154, 75, 113, 110, 12}, //SPEED 7
{ 80, 21, 160, 157, 50, 185, 110, 80, 13, 90, 120, 120, 179, 40, 43, 150, 12}, //SPEED 7
{ 75, 11, 190, 172, 45, 185, 80, 120, 13, 120, 80, 125, 189, 10, 28, 155, 12}, //SPEED 7
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 12}, //SPEED 7
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 145, 12}, //SPEED 7
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; // forward stand up
int16_t single_right_arm_push_up[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 30}, //Slow
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 140, 30}, //Slow
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 30}, //Slow
{ 75, 11, 190, 172, 45, 185, 80, 120, 15, 120, 80, 125, 189, 10, 28, 155, 30}, //Slow
{ 80, 21, 160, 157, 50, 185, 110, 80, 15, 90, 120, 120, 179, 40, 43, 150, 30}, //Slow
{ 100, 46, 125, 87, 90, 125, 230, 32, 75, -30, 168, 100, 154, 75, 113, 110, 30}, //Slow
// fall forwaed
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 100, 100, 100, 100, 100, 220, 40, 100, -20, 160, 100, 100, 100, 100, 100, 11}, //SPEED 8
{ 100, 100, 100, 100, 100, 180, 220, 40, 20, -20, 160, 100, 100, 100, 100, 100, 11}, //SPEED 8
{ 100, 100, 100, 100, 100, 180, 120, 40, 20, 80, 160, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 120, 55, 180, 120, 40, 20, 80, 160, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 80, 160, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 70, 168, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 105, 110, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 70, 168, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 105, 110, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 70, 168, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 105, 110, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 70, 168, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 190, 32, 20, 105, 110, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 130, 32, 20, 70, 168, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 120, 55, 180, 120, 40, 20, 80, 160, 100, 100, 100, 100, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 103, 100, 100, 97, 100, 100, 100, 100, 100, 16}, //SPEED 3
// single_right_arm_push_up
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 46, 125, 87, 90, 150, 230, 32, 50, -30, 168, 100, 154, 75, 113, 110, 12}, //SPEED 7
{ 80, 21, 160, 157, 50, 185, 110, 80, 13, 90, 120, 120, 179, 40, 43, 150, 12}, //SPEED 7
{ 75, 11, 190, 172, 45, 185, 80, 120, 13, 120, 80, 125, 189, 10, 28, 155, 12}, //SPEED 7
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 12}, //SPEED 7
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 145, 12}, //SPEED 7
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; // forward stand up
int16_t double_arms_pushing_up[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 30}, //Slow
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 140, 30}, //Slow
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 30}, //Slow
{ 75, 11, 190, 172, 45, 185, 80, 120, 15, 120, 80, 125, 189, 10, 28, 155, 30}, //Slow
{ 80, 21, 160, 157, 50, 185, 110, 80, 15, 90, 120, 120, 179, 40, 43, 150, 30}, //Slow
{ 100, 46, 125, 87, 90, 125, 230, 32, 75, -30, 168, 100, 154, 75, 113, 110, 30}, //Slow
// fall forwaed
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 101, 100, 100, 100, 100, 220, 40, 100, -20, 160, 100, 99, 100, 100, 100, 11}, //SPEED 8
{ 100, 101, 100, 100, 100, 180, 220, 40, 20, -20, 160, 100, 99, 100, 100, 100, 11}, //SPEED 8
{ 100, 101, 100, 100, 100, 180, 120, 40, 20, 80, 160, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 130, 32, 20, 70, 168, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 95, 90, 20, 105, 110, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 130, 32, 20, 70, 168, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 95, 90, 20, 105, 110, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 130, 32, 20, 70, 168, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 95, 90, 20, 105, 110, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 130, 32, 20, 70, 168, 100, 99, 100, 100, 100, 12}, //SPEED 7
{ 100, 101, 100, 100, 100, 180, 120, 40, 20, 80, 160, 100, 99, 100, 100, 100, 12}, //SPEED 7
// double_arms_pushing_up
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 46, 125, 87, 90, 150, 230, 32, 50, -30, 168, 100, 154, 75, 113, 110, 12}, //SPEED 7
{ 80, 21, 160, 157, 50, 185, 110, 80, 13, 90, 120, 120, 179, 40, 43, 150, 12}, //SPEED 7
{ 75, 11, 190, 172, 45, 185, 80, 120, 13, 120, 80, 125, 189, 10, 28, 155, 12}, //SPEED 7
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 12}, //SPEED 7
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 145, 12}, //SPEED 7
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; // forward stand up
int16_t forward_stand_up[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 46, 125, 87, 90, 150, 230, 32, 50, -30, 168, 100, 154, 75, 113, 110, 12}, //SPEED 7
{ 80, 21, 160, 157, 50, 185, 110, 80, 13, 90, 120, 120, 179, 40, 43, 150, 12}, //SPEED 7
{ 75, 11, 190, 172, 45, 185, 80, 120, 13, 120, 80, 125, 189, 10, 28, 155, 12}, //SPEED 7
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 12}, //SPEED 7
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 145, 12}, //SPEED 7
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; // forward stand up
int16_t backward_stand_up[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 14}, //SPEED 5
// std pose
{ 100, 166, 145, 122, 100, 100, 200, 32, 100, 0, 168, 100, 34, 55, 78, 100, 12}, //SPEED 7
{ 100, 166, 162, 147, 100, 20, 200, 32, 180, 0, 168, 100, 34, 38, 53, 100, 12}, //SPEED 7
{ 100, 50, 185, 57, 100, 20, 100, 110, 180, 100, 90, 100, 150, 15, 143, 100, 12}, //SPEED 7
{ 100, 11, 175, 22, 100, 30, 90, 115, 170, 110, 85, 100, 189, 25, 178, 100, 12}, //SPEED 7
{ 100, 11, 205, 107, 100, 110, 140, 70, 90, 60, 130, 100, 189, -5, 93, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; // backward stand up
int16_t forward_tumbling[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 21, 225, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -25, 53, 100, 20}, //Slow
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 20}, //Slow
{ 75, 11, 190, 172, 45, 185, 80, 120, 15, 120, 80, 125, 189, 10, 28, 155, 20}, //Slow
{ 80, 21, 160, 157, 50, 185, 110, 80, 15, 90, 120, 120, 179, 40, 43, 150, 20}, //Slow
{ 100, 46, 125, 87, 90, 130, 230, 32, 70, -30, 168, 100, 154, 75, 113, 110, 20}, //Slow
{ 100, 16, 135, 147, 100, 140, 140, 40, 60, 60, 160, 100, 184, 65, 53, 100, 20}, //Slow
{ 100, 120, 135, 33, 100, 170, 110, 70, 30, 90, 130, 100, 95, 132, 31, 100, 4}, //SPEED 15
{ 100, 114, 135, 22, 100, 190, 110, 70, 13, 90, 130, 100, 95, 83, 80, 100, 4}, //SPEED 15
{ 100, 121, 135, 22, 100, 190, 110, 70, 13, 90, 130, 100, 79, 65, 178, 100, 4}, //SPEED 15
{ 100, 66, 145, 22, 100, 170, 230, 135, 30, -30, 65, 100, 134, 55, 178, 100, 9}, //SPEED 10
{ 100, 6, 175, 22, 100, 190, 240, 140, 13, -40, 60, 100, 194, 25, 178, 100, 9}, //SPEED 10
{ 100, 6, 215, 117, 100, 190, 110, 80, 13, 90, 120, 100, 194, -15, 83, 100, 11}, //SPEED 8
{ 100, 25, 222, 147, 99, 100, 100, 100, 100, 100, 100, 100, 175, -22, 53, 101, 14}, //SPEED 5
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; //
int16_t backward_tumbling[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 6, 174, 30, 100, 80, 120, 90, 120, 80, 110, 100, 194, 26, 170, 100, 18}, //SPEED 4
{ 100, 43, 174, 30, 100, 10, 166, 35, 190, 34, 166, 100, 157, 26, 170, 100, 18}, //SPEED 4
{ 100, 43, 196, 30, 100, 45, 186, 35, 155, 14, 166, 100, 157, 4, 170, 100, 18}, //SPEED 4
{ 100, 43, 196, 30, 100, 45, 120-35, 90, 155, 80+35, 110, 100, 94, 135, 33, 100, 18}, //SPEED 4
{ 100, 43, 65, 167, 100, 10, 120-35, 90, 190, 80+35, 110, 100, 157, 135, 33, 100, 4}, //SPEED 15
{ 100, 43, 65, 167, 100, 10, 120-35, 90, 190, 80+35, 110, 100, 157, 135, 33, 100, 5}, //wait
{ 100, 81, 65, 167, 100, 160, 120-35, 90, 40, 80+35, 110, 100, 119, 135, 33, 100, 4}, //SPEED 15
{ 100, 46, 125, 87, 90, 130, 230, 32, 70, -30, 168, 100, 154, 75, 113, 110, 4}, //SPEED 15
{ 100, 46, 125, 87, 90, 150, 230, 32, 50, -30, 168, 100, 154, 75, 113, 110, 12}, //SPEED 7
{ 80, 21, 160, 157, 50, 185, 110, 80, 13, 90, 120, 120, 179, 40, 43, 150, 12}, //SPEED 7
{ 75, 11, 190, 172, 45, 185, 80, 120, 13, 120, 80, 125, 189, 10, 28, 155, 12}, //SPEED 7
{ 60, 11, 215, 172, 45, 170, 80, 120, 30, 120, 80, 140, 189, -15, 28, 155, 12}, //SPEED 7
{ 60, 11, 220, 167, 55, 150, 130, 110, 50, 70, 90, 140, 189, -20, 33, 145, 12}, //SPEED 7
{ 100, 21, 220, 147, 100, 130, 120, 105, 70, 80, 95, 100, 179, -20, 53, 100, 12}, //SPEED 7
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; //
int16_t left_tumbling[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 41, 185-4, 130, 100, 100, 190, 160, 100, 10, 40, 100, 159, 15, 70, 100, 40}, //slow
{ 114, 41, 185-4, 130, 95, 100, 190, 160, 100, 10, 40, 112, 134, 46, 77, 100, 16}, //SPEED 3
{ 114, 41, 185-4, 130, 95, 100, 190, 160, 100, 10, 40, 111, 159, 15, 70, 100, 16}, //SPEED 3
{ 120, 41, 185-4, 130, 90, 100, 190, 160, 100, 10, 40, 111, 159, 15, 70, 130, 16}, //SPEED 3
{ 120, 41, 185-4, 130, 80, 100, 235, 205, 100, -35, -5, 111, 159, 15, 70, 158, 15}, //SPEED 4
{ 120, 45, 185-4, 130, 15, 100, 235, 205, 100, -35, -5, 80, 155, 15, 70, 183, 11}, //SPEED 8
{ 120, 45, 185-4, 130, 15, 100, 235, 205, 100, -35, -5, 80, 155, 15, 70, 183, 5}, //wait
{ 120, 45, 185-4, 130, 15, 100, 190, 165, 100, 10, 35, 80, 155, 15, 70, 183, 14}, //SPEED 5
{ 86, 64, 172, 134, 99, 100, 190, 165, 100, 10, 35, 95, 155, 15, 70, 183, 13}, //SPEED 6
{ 86, 58, 172, 134, 99, 100, 150, 100, 100, 50, 100, 88, 155, 17, 70, 133, 16}, //SPEED 3
{ 88, 61, 159, 122, 110, 100, 150, 100, 100, 50, 100, 93, 159, 17, 70, 113, 16}, //SPEED 3
{ 100, 41, 185-4, 130, 100, 100, 150, 100, 100, 50, 100, 100, 159, 15, 70, 100, 15}, //SPEED 4
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; //
int16_t right_tumbling[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 100, 41, 185, 130, 100, 100, 190, 160, 100, 10, 40, 100, 159, 15, 70, 100, 40}, //slow
{ 83, 66, 154, 123, 100, 100, 190, 160, 100, 10, 40, 86, 159, 15, 70, 105, 16}, //SPEED 3
{ 89, 41, 185, 130, 100, 100, 190, 160, 100, 10, 40, 86, 159, 15, 70, 105, 16}, //SPEED 3
{ 89, 41, 185, 130, 70, 100, 190, 160, 100, 10, 40, 80, 159, 15, 70, 110, 16}, //SPEED 3
{ 89, 41, 185, 130, 42, 100-1, 235-1, 205-2, 100+1, -35+1, -5+2, 80, 159, 15, 70, 120, 15}, //SPEED 4
{ 120+4, 45-4, 185, 130, 17, 100-1, 235-1, 205-2, 100+1, -35+1, -5+2, 80, 155, 15, 70, 185, 11}, //SPEED 8
{ 120+4, 45-4, 185, 130, 17, 100-1, 235-1, 205-2, 100+1, -35+1, -5+2, 80, 155, 15, 70, 185, 5}, //wait
{ 120+4, 45-4, 185, 130, 17, 100, 190, 165, 100, 10, 35, 80, 155, 15, 70, 185, 14}, //SPEED 5
{ 105, 45-4, 185-2, 130, 17, 100, 190, 165, 100, 10, 35, 114, 136, 28, 66, 101, 13}, //SPEED 6
{ 112-2, 45-4, 183-2, 130, 67, 100, 150, 100, 100, 50, 100, 114, 142, 28, 66, 101, 16}, //SPEED 3
{ 107, 41, 183, 130, 87, 100, 150, 100, 100, 50, 100, 112, 139, 44, 78, 90, 16}, //SPEED 3
{ 100, 41, 185-2, 130, 100, 100, 150, 100, 100, 50, 100, 100, 159, 15, 70, 100, 15}, //SPEED 4
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; //
int16_t left_foot_back_attack[][17] = { // 1 0 0 1 0 1 1 1 0 0 0 0 1 1 0 1
{ 90, 103, 97, 100, 90, 100, 120, 90, 100, 70, 110, 92, 99, 100, 100, 93, 4}, //SPEED 15
{ 93, 45, 222, 164, 95, 100, 120, 90, 100, 27, 66, 83, 99, 100, 100, 99, 4}, //SPEED 15
//{ 93, 45, 222, 164, 95, 100, 120, 90, 100, 27, 66, 83, 99, 100, 100, 99, 20}, //delay 200
{ 94, 83, 162, 141, 38, 103, 178, 139, 100, 13, 54, 83, 99, 100, 104, 133, 11}, //SPEED 8
{ 95, 114, 87, 91, 16, 103, 178, 139, 100, 13, 54, 98, 99, 100, 104, 172, 11}, //SPEED 8
{ 95, 114, 87, 91, 16, 157, 178, 139, 100, 13, 54, 98, 99, 100, 104, 172, 11}, //SPEED 8
{ 95, 114, 87, 91, 16, 181, 86, 43, 100, 13, 54, 98, 99, 7, 104, 172, 11}, //SPEED 8
{ 95, 114, 87, 91, 16, 181, 86, 43, 100, 13, 54, 98, 99, 7, 104, 172, 40}, //wait
{ 101, 15, 223, 138, 103, 100, 174, 132, 98, 29, 63, 101, 182, -28, 57, 101, 14}, //SPEED 5
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 20}, //SPEED 1
{ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 60} //wait 1000
}; //

uint8_t maxRows;
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;
uint8_t nextKeyFrame1;
uint8_t actionMode;
uint8_t prevbtnData1;

//RCB controller 関連
BLEUUID SERVICE_UUID = BLEUUID((uint16_t)0xFFF0); // RCBController Service UUID
BLEUUID CHARACTERISTIC_UUID = BLEUUID((uint16_t)0xFFF1); // RCBController Characteristic UUID

int v120 = 0; //1バイト目 リアルタイムでの取り込み値(BLE通信)
int vv120 = 0; //1バイト目 ボタンが押された時にこの変数に値を格納する(ボタン離したとき0値にならないように)
int ex120 = 0; //1バイト目 前回押されたボタン値を格納しておいて、次に押されたボタン値と比較する
// 変動してなかったら(=ボタンから指を離したままか押したままの時) vv120は0値でリセット
int v121 = 0; //2バイト目 リアルタイムでの取り込み値(BLE通信)
int vv121 = 0; //2バイト目 ボタンが押された時にこの変数に値を格納する
int ex121 = 0; //2バイト目 前回押されたボタン値を格納しておいて、次に押されたボタン値と比較する
// 変動してなかったら(=ボタンから指を離したままか押したままの時) vv121は0値でリセット

uint16_t btnData = 0;
uint16_t prevbtnData = 0; //初期設定

class MyServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
printf("connectionCallback\n\r");
};
void onDisconnect(BLEServer* pServer) {
printf("disconnectionCallback\n\r");
}
};
class MyCallbacks: public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic *pCharacteristic) {
std::string value = pCharacteristic->getValue();
if (value.length() >= 10) {
//Serial.printf("Data received: length = %d, data = 0x",value.length());
for(int x=0; x < value.length(); x++) {
if (x==0) {
v120 = value[x];
if (v120 > 0) { vv120 = v120; }
}
if (x==1) {
v121 = value[x];
if (v121 > 0) { vv121 = v121; }
}
}
}
}
};

void setup() {
// Initialize PCA9685
pca9685.begin();
// Set PWM Frequency to 50Hz
pca9685.setPWMFreq(50);
// I2Cサーボ用端子設定
svPin[0] = 0; //G6A 正回転 左足首ロール
svPin[1] = 1; //G6A 逆回転 左足首ピッチ
svPin[2] = 2; //G6A 逆回転 左膝ピッチ
svPin[3] = 3; //G6A 正回転 左腰ピッチ
svPin[4] = 4; //G6A 逆回転 左腰ロール
svPin[5] = 5; //G6B 正回転 左肩ピッチ
svPin[6] = 6; //G6B 正回転 左肩ロール
svPin[7] = 7; //G6B 正回転 左肘ロール
svPin[8] = 13; //G6C 逆回転 右肩ピッチ
svPin[9] = 14; //G6C 逆回転 右肩ロール
svPin[10] = 15; //G6C 逆回転 右肘ロール
svPin[11] = 8; //G6D 逆回転 右足首ロール
svPin[12] = 9; //G6D 正回転 右足首ピッチ
svPin[13] = 10; //G6D 正回転 右膝ピッチ
svPin[14] = 11; //G6D 逆回転 右腰ピッチ
svPin[15] = 12; //G6D 正回転 右腰ロール

// 動作モード初期化
actionMode = Stop;
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
maxRows = 2; //action配列の初期値に対するmaxRows

// LED用端子設定
pinMode(ledPin, OUTPUT);

// RCBcontroller 関連設定
Serial.begin(115200);
// Create the BLE Device
BLEDevice::init("ROBONOVA-I");
// Create the BLE Server
BLEServer *pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
// Create the BLE Service
BLEService *pService = pServer->createService(SERVICE_UUID);
// Create a BLE Characteristic
BLECharacteristic *pCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID,
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_WRITE_NR
);
pCharacteristic->setCallbacks(new MyCallbacks());
// Start the service
pService->start();
// Start advertising
BLEAdvertising *pAdvertising = pServer->getAdvertising();
pAdvertising->start();

// MPU6050 関連設定
Serial.begin(115200);
while (!Serial)
delay(10); // will pause Zero, Leonardo, etc until serial console opens

Serial.println("Adafruit MPU6050 test!");

// Try to initialize!
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
Serial.println("MPU6050 Found!");

mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
Serial.print("Accelerometer range set to: ");
switch (mpu.getAccelerometerRange()) {
case MPU6050_RANGE_2_G:
Serial.println("+-2G");
break;
case MPU6050_RANGE_4_G:
Serial.println("+-4G");
break;
case MPU6050_RANGE_8_G:
Serial.println("+-8G");
break;
case MPU6050_RANGE_16_G:
Serial.println("+-16G");
break;
}
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
Serial.print("Gyro range set to: ");
switch (mpu.getGyroRange()) {
case MPU6050_RANGE_250_DEG:
Serial.println("+- 250 deg/s");
break;
case MPU6050_RANGE_500_DEG:
Serial.println("+- 500 deg/s");
break;
case MPU6050_RANGE_1000_DEG:
Serial.println("+- 1000 deg/s");
break;
case MPU6050_RANGE_2000_DEG:
Serial.println("+- 2000 deg/s");
break;
}

mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
Serial.print("Filter bandwidth set to: ");
switch (mpu.getFilterBandwidth()) {
case MPU6050_BAND_260_HZ:
Serial.println("260 Hz");
break;
case MPU6050_BAND_184_HZ:
Serial.println("184 Hz");
break;
case MPU6050_BAND_94_HZ:
Serial.println("94 Hz");
break;
case MPU6050_BAND_44_HZ:
Serial.println("44 Hz");
break;
case MPU6050_BAND_21_HZ:
Serial.println("21 Hz");
break;
case MPU6050_BAND_10_HZ:
Serial.println("10 Hz");
break;
case MPU6050_BAND_5_HZ:
Serial.println("5 Hz");
break;
}

Serial.println("");
delay(100);
// MPU6050 関連設定ここまで
}

void loop() {
//チャタリングなのか、新しいボタン入力なのか 判断するための処理
if ((vv120!=ex120)&&(vv121!=ex121)) {
//ボタンが押された時のvv120とvv121値が前回押されたボタンの時と違う場合　vv120とvv121を次回のためにex120とex121に格納しておく
ex120 = vv120;
ex121 = vv121;
}
if ((vv120==ex120)&&(vv121!=ex121)) {
//ボタンが押された時のvv121値だけが　前回押されたボタンの時と違う場合　vv120をリセットしvv120、vv121を次回のためにex120とex121に格納しておく
vv120 = 0;
ex120 = vv120;
ex121 = vv121;
}
if ((vv120!=ex120)&&(vv121==ex121)) {
//ボタンが押された時のvv120値だけが　前回押されたボタンの時と違う場合　vv121をリセットしvv120、vv121を次回のためにex120とex121に格納しておく
vv121 = 0;
ex120 = vv120;
ex121 = vv121;
}
Serial.printf("RCBController Button DATA:%d %d \n\r",vv120,vv121); //ボタンの状態(値)
//確定したRCB Controller Buttonの値をbtnData変数に入れる
btnData = vv120*256 + vv121;
//ボタン値が変わった時に 次は何のアクションか選定する
if (btnData != prevbtnData) { //今の読取り値が前回の値と違う時 以下を実行
switch(btnData){
case 512:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" B ボタン");
actionMode = Left_turn;
memcpy(action, left_turn, sizeof(left_turn));
maxRows = sizeof(left_turn) / sizeof(*left_turn) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 1024:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" R ボタン");
actionMode = Left_shift;
memcpy(action, left_shift, sizeof(left_shift));
maxRows = sizeof(left_shift) / sizeof(*left_shift) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 4096:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" G ボタン");
actionMode = Right_shift;
memcpy(action, right_shift, sizeof(right_shift));
maxRows = sizeof(right_shift) / sizeof(*right_shift) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 2048:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" Y ボタン");
actionMode = Right_turn;
memcpy(action, right_turn, sizeof(right_turn));
maxRows = sizeof(right_turn) / sizeof(*right_turn) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 12:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" SELECT ボタン");
// /*
actionMode = Backward_tumbling;
memcpy(action, backward_tumbling, sizeof(backward_tumbling));
maxRows = sizeof(backward_tumbling) / sizeof(*backward_tumbling) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
// */
break;
case 3:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" POWER ボタン");
break;
case 272:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 1 ボタン");
actionMode = Left_attack;
memcpy(action, left_attack, sizeof(left_attack));
maxRows = sizeof(left_attack) / sizeof(*left_attack) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 16:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 2 ボタン");
actionMode = Left_right_shoot;
memcpy(action, left_right_shoot, sizeof(left_right_shoot));
maxRows = sizeof(left_right_shoot) / sizeof(*left_right_shoot) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 80:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 3 ボタン");
actionMode = Right_attack;
memcpy(action, right_attack, sizeof(right_attack));
maxRows = sizeof(right_attack) / sizeof(*right_attack) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 256:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 4 ボタン");
actionMode = Single_left_arm_push_up;
memcpy(action, single_left_arm_push_up, sizeof(single_left_arm_push_up));
maxRows = sizeof(single_left_arm_push_up) / sizeof(*single_left_arm_push_up) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 128:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 5 ボタン");
actionMode = Double_arms_pushing_up;
memcpy(action, double_arms_pushing_up, sizeof(double_arms_pushing_up));
maxRows = sizeof(double_arms_pushing_up) / sizeof(*double_arms_pushing_up) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 64:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 6 ボタン");
actionMode = Single_right_arm_push_up;
memcpy(action, single_right_arm_push_up, sizeof(single_right_arm_push_up));
maxRows = sizeof(single_right_arm_push_up) / sizeof(*single_right_arm_push_up) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 288:
Serial.println(" 7 ボタン");
Serial.print("btnData = ");
Serial.print(btnData);
actionMode = Left_tumbling;
memcpy(action, left_tumbling, sizeof(left_tumbling));
maxRows = sizeof(left_tumbling) / sizeof(*left_tumbling) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 32:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 8 ボタン");
actionMode = Forward_tumbling;
memcpy(action, forward_tumbling, sizeof(forward_tumbling));
maxRows = sizeof(forward_tumbling) / sizeof(*forward_tumbling) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 96:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 9 ボタン");
actionMode = Right_tumbling;
memcpy(action, right_tumbling, sizeof(right_tumbling));
maxRows = sizeof(right_tumbling) / sizeof(*right_tumbling) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 9:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 10 ボタン");
actionMode = Handstanding;
memcpy(action, handstanding, sizeof(handstanding));
maxRows = sizeof(handstanding) / sizeof(*handstanding) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 1:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 11 ボタン");
actionMode = Fast_walk;
memcpy(action, fast_walk, sizeof(fast_walk));
maxRows = sizeof(fast_walk) / sizeof(*fast_walk) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 5:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" 12 ボタン");
actionMode = Wing_move;
memcpy(action, wing_move, sizeof(wing_move));
maxRows = sizeof(wing_move) / sizeof(*wing_move) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 8:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" + ボタン");
actionMode = Forward_stand_up;
memcpy(action, forward_stand_up, sizeof(forward_stand_up));
maxRows = sizeof(forward_stand_up) / sizeof(*forward_stand_up) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 10:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" - ボタン");
actionMode = Backward_stand_up;
memcpy(action, backward_stand_up, sizeof(backward_stand_up));
maxRows = sizeof(backward_stand_up) / sizeof(*backward_stand_up) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 0;
break;
case 2:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" Sound ボタン");
actionMode = Stop;
digitalWrite(ledPin, LOW);
keyFrame = 0;
nextKeyFrame = 0;
divCounter = 0;
GyroOn = 1;
break;
case 4:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" UP ボタン");
actionMode = Forward_walk;
memcpy(action, forward_walk, sizeof(forward_walk));
maxRows = sizeof(forward_walk) / sizeof(*forward_walk) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
case 6:
Serial.print("btnData = ");
Serial.print(btnData);
Serial.println(" DOWN ボタン");
actionMode = Backward_walk;
memcpy(action, backward_walk, sizeof(backward_walk));
maxRows = sizeof(backward_walk) / sizeof(*backward_walk) - 1;
digitalWrite(ledPin, HIGH);
divCounter = 0;
keyFrame = 0;
nextKeyFrame = 0;
GyroOn = 1;
break;
default:
break;
} //switch
//delay(1000); //for serial monitor
} //if (btnData != prevbtnData)

prevbtnData = btnData ; //次の読み取りにそなえて 今の値をprevbtnDataに入れる
Serial.print("prevbtnData = ");
Serial.print(prevbtnData);

//アクションモードにのっとった動作出力処理
if(actionMode != Stop){ // Stopモード以外
// フレーム処理
divCounter++;
if(divCounter >= action[nextKeyFrame][16]) {
divCounter = 0;
keyFrame = nextKeyFrame;
nextKeyFrame++;
if(nextKeyFrame > maxRows) {
nextKeyFrame = 0;
nextKeyFrame1 = maxRows+1;
}
}// フレーム処理end

// サーボ角度計算
// STOPからaction[][17]に他のアクション配列が読み込まれた時 直立姿勢100度からアクションモードの始まりに移行する時にゆっくり動く
// ただしloop開始時の(divCounter >= action[nextKeyFrame][16])を満たさない間だけ
if(keyFrame == 0 && nextKeyFrame == 0){ //アクションモードの始まり 最初のdivCounter >= action[nextKeyFrame][16]を満たさない時
divCounter++;
if(divCounter <= 60) {
// サーボ角度計算@Stopモード
if (prevbtnData1 == 1) { //前がStopだった時
for(int i=0; i<16; i++) {
tempAngles[i] = 100 +
int8_t((action[nextKeyFrame][i] - 100)
* divCounter / action[nextKeyFrame][16]);
}
for(i = 0; i < 16; i++) { // サーボ信号出力
angle[i] = tempAngles[i]+offset[i];
anglePulse[i] = map(angle[i], 0, 180, SERVOMIN, SERVOMAX);
pca9685.setPWM(svPin[i], 0, anglePulse[i]);
}
delay(30);
}else{
for(int i = 0; i < 16; i++) { //tempAngles[i]はactionmode変化時のservo角度
tempAngles1[i] = tempAngles[i] +
int8_t((action[keyFrame][i] - tempAngles[i])
* divCounter / 60);
}
// サーボ信号出力
for(i = 0; i < 16; i++) {
angle[i] = tempAngles1[i]+offset[i];
anglePulse[i] = map(angle[i], 0, 180, SERVOMIN, SERVOMAX);
pca9685.setPWM(svPin[i], 0, anglePulse[i]);
}
delay(30);
}
}
if ((divCounter > 60) && (prevbtnData != 1)) { //同じactionが連続する時のため前のtempAngles1[i]をtempAngles[i]に入れておく
for(int i = 0; i < 16; i++) {
tempAngles[i] = tempAngles1[i];
}
}
}else{
prevbtnData1 = 0; //1から0に戻す
for(int i = 0; i < 16; i++) { //通常のアクションモード中の角度計算(keyFrame = 0 nextKeyFrame = 1 からは)
tempAngles[i] = action[keyFrame][i] +
int8_t((action[nextKeyFrame][i] - action[keyFrame][i])
* divCounter / action[nextKeyFrame][16]);
}
if (GyroOn == 1) {
// Get new sensor events with the readings
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
/*
// Print out the values
Serial.print("Acceleration X: ");
Serial.print(a.acceleration.x);
Serial.print(", Y: ");
Serial.print(a.acceleration.y);
Serial.print(", Z: ");
Serial.print(a.acceleration.z);
Serial.println(" m/s^2");

Serial.print("Rotation X: ");
Serial.print(g.gyro.x);
Serial.print(", Y: ");
Serial.print(g.gyro.y);
Serial.print(", Z: ");
Serial.print(g.gyro.z);
Serial.println(" rad/s");

Serial.print("Temperature: ");
Serial.print(temp.temperature);
Serial.println(" degC");

Serial.println("");
delay(500);
*/
//Serial.print(", gY: ");
//Serial.println(g.gyro.y);
int gyro=(int)g.gyro.y;
//int gyro=((int)g.gyro.y>>1); //Gyroの効き幅を半分にした
tempAngles[1]-=gyro; //tempAngles[1]=tempAngles[1]-gyro; //左足首ピッチ
tempAngles[12]+=gyro; //tempAngles[12]=tempAngles[12]+gyro; //右足首ピッチ
}
// サーボ信号出力
for(i = 0; i < 16; i++) {
angle[i] = tempAngles[i]+offset[i];
anglePulse[i] = map(angle[i], 0, 180, SERVOMIN, SERVOMAX);
pca9685.setPWM(svPin[i], 0, anglePulse[i]);
}
delay(30);
}
} //if(actionMode != Stop)

if(actionMode == Stop){ //Stopを受け取った時 直立姿勢100度にゆっくり移行する(差分を60分割したステップで)
prevbtnData1 = 1;
divCounter++;
if(divCounter <= 60) {
// サーボ角度計算@Stopモード
for(int i = 0; i < 16; i++) { //tempAngles[i]は動作停止した時の角度または連続STOP時の100
tempAngles1[i] = tempAngles[i] +
int8_t((100 - tempAngles[i])
* divCounter / 60);
}
// サーボ信号出力
for(i = 0; i < 16; i++) {
angle[i] = tempAngles1[i]+offset[i];
anglePulse[i] = map(angle[i], 0, 180, SERVOMIN, SERVOMAX);
pca9685.setPWM(svPin[i], 0, anglePulse[i]);
}
}
delay(30);
if (divCounter > 60) { //Stopが連続する時のため前のStopの100をtempAngles[i]に入れておく
for(int i = 0; i < 16; i++) {
tempAngles[i] = 100;
}
}
}//if(actionMode == Stop){ // Stopモード

}//loop