#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