20230320
起因
分析了OpenCat 4足机器人的步态，用视觉化的方式展示。
看OpenCat 步态控制源码，写死的步态，只能变换速度，并不能变换轨迹。
因此，觉得有改进的空间。
也许是看了傅里叶变换的视频，有了用三角函数作为周期发生器的思路。用这个作为中心函数驱动和同步各个关节。
https://www.bilibili.com/video/BV1m7411n7Ke
肢体的运动有节律，有周期，有重复，比较适合用统一的中心函数驱动，类似于用一个电机带动一组或多组齿轮。
这个算法在每个loop里读取系统时间，在此基础上计算一个舵机的角度。 因为系统时间相同，即使所有舵机依次分别计算，仍旧能够保持（相位）同步
三角函数作为周期发生器，比三角波函数好，加速度更平滑。
成果
用图解释下面的程序
// 成功的实现了大周期里的小周期。 一条腿在大周期里始终动，另一条腿在大周期里的小周期里才动。 Wave=sin( (PhaseTime -ActionArray[ActionID][4]) / (SmallCycleTime/6.28)); // 在周期内，Wave取值区间：负一到正一 ,作为步态的核心发生器
// todo bug 舵机偶尔抽风，然后恢复，抓包分析
// todo bug 另外在不关机器人电源情况下，更新上传arduino 程序时，舵机大幅摆动
// todo bug 舵机有吱吱声，貌似不停的来回摆动
// todo 用print（）简单测试每个loop周期耗时。sin（）函数等较费时间，
// memo 在每个大周期里（2000mS 大周期从 time 取余得来。），一个舵机可以有多个小周期（小周期从大周期截取得来）。所以，void loop() 不应该用 ServoID做循环指示器，应该用ActionID做循环指示器。
// ActionID ServoID ServoNtrlPos TimePosition1 TimePosition2 ActionPhase ActionTime AngleRange
// 动作编号 舵机编号 舵机中立位置 时间轴位置（开始） 时间轴位置（结束） 动作相位 动作从开始到结束的总时间 舵机角度幅度
int ActionArray[7][8] = {
{0,20,0,0,0,0,0,0},
{1,5,0x70,0,2000,0,0x30,0x70}, //bug 舵机角度幅度 大于0x76 就开始不正常 ，右髋舵机的表现是从向后踢腿变成向前踢腿。
{2,6,0x70,0,2000,0,0x20,0x20},
{3,7,0x70,0,2000,0,0x20,0x20},
{4,8,0x70,1000,2000,1000,0x20,0x70},
{5,9,0x70,0,2000,0,0x20,0x20},
{6,10,0x70,0,2000,0,0x20,0x20} };
//常量===============
int ServoID ; //每个舵机的编号 ，从1-14
int ActionID=0; //每个动作的编号 ，从1-32
int ServoNtrlPos[15]; //每个舵机的中立位（将机器人站直，然后读取角度，写入本程序中）
int AngleRange[15]; //每个舵机的运动幅度，取值大，来回运动范围大速度大 也可以用float AngleRange[15]; 也可以用int AngleRange[15];
int AngleRangeFix=0x20; //舵机的运动范围（固定值），
int ActionTime[15]; //每个舵机的转动时间，取值大，速度慢
int ActionTimeFix=0x20; //舵机的转动时间（固定值），
int ActionPhase[15]; //每个舵机的运动相位，取值大，时间相位推后。取值范围 与 Time 相关。 如果=1/2 Time 将会反相
//步进 度
//变量=================
//定义 机器人结构，舵机，传感器
//定义 步态，腿 走路速度， 抬腿高度，抬腿距离，臂，头，腰
byte outPutToServo[10];
byte inByte[10];
byte sum;
unsigned long sysTime; // 4字节，系统时间，ms。 直到49天回到0ms。
unsigned int sysTimeWithPhase; //4字节，系统时间，ms。
unsigned int BigCycleTime=2000; // 用于取余，类似于钟表表盘刻度，2秒一个周期就是 2000ms,
float Wave; // 动作的正弦波发生器
//setup()===========
void setup() {
///////////////////// 通讯接口
Serial.begin(1000000);
delay(20);
///////////////////// 机器人各个关节姿态初始化，
/////////////////////舵机变量初始化
outPutToServo[0] = 0xFA; // 帧开始码 FA AF，FC CF，
outPutToServo[1] = 0xAF; // 帧开始码
outPutToServo[2] = 0; // 舵机ID , ID =0 是广播到所有舵机
outPutToServo[3] = 0x1; // 命令01 舵机转动 // 命令02--读取舵机角度 // 命令04-- LED
outPutToServo[4] = 0x90; // 命令1参数--angle 目标角度, 取值范围[0-240]，单位：度 --取值 FF ,停止
outPutToServo[5] = 0x30; // 对于01命令 舵机转动时间
outPutToServo[6] = 0x00; // Hight byte //命令1参数，不应期锁定时间
outPutToServo[7] = 0x00; // Low byte， //命令1参数，不应期锁定时间
outPutToServo[8] = 0x1; // 校验字节，sum（byte 2--byte 7） ， 此处数值稍后在loop中被更新为正确的值
outPutToServo[9] = 0xED; // 帧结束码 237
}
//loop()==============
void loop() {
/////////// 从1-32个动作，每个loop（）执行一个动作。 //if (ActionID==32) {ActionID=0;}
if (ActionID==6){ActionID=0;}
ActionID =ActionID+1 ;
////////////////////// 读传感器
////////////////////// 读遥控器
/////////////////////// 读时间
int SmallCycleTime =ActionArray[ActionID][4] - ActionArray[ActionID][3];
sysTime =millis(); //存储 32 位（4 字节）
int BigPhaseTime = (sysTime+ActionArray[ActionID][5]) % BigCycleTime;
if ( (BigPhaseTime <= ActionArray[ActionID][3]) || (BigPhaseTime >= ActionArray[ActionID][4] ) ) {
return;// debug <<<<<<<<<<<可以用while( ){} // 不能用continue 会有报错：continue statement not within a loop,更不能用 break，试图退出loop（）
}
Wave=sin((BigPhaseTime - ActionArray[ActionID][3]) / (SmallCycleTime/6.28)); //在周期内，Wave取值区间：负一到正一,作为步态 的核心发生器
// %取模
// (CycleTime/6.28)是 2000ms对应了2pi（6.28），所以要除以2000/6.28
// sin(rad) 正弦函数， （以弧度为单位）。结果将介于 -1 和 1 之间
// bug 为何 ActionArray[ActionID][4] 也能运行？？？？？？？？？？
/////////////////////////计算步态
//////////////////////// 写舵机
outPutToServo[2] = ActionArray[ActionID][1] ;
outPutToServo[4] = 0x70+ int( Wave * (ActionArray[ActionID][7]) ) ; // <<<<<<< // bug 计算报错 debug 舵机运动角度的幅度，不能没有[下标]
sum = 0; // 计算校验字节，sum（byte 2--byte 7）
for (int i = 2; i < 8; i++) {
sum +=outPutToServo[i];
}
outPutToServo[8]=sum;
for (int i = 0; i <=9; i++){
//mySerial.write(outPutToServo[i]);
Serial.write(outPutToServo[i]); // <<<<<<< 写舵机数据 ，写10byte
}
delay(2);
}
经验
自顶到底的方法编程，
中心函数作为步态的发生器，
统一的变量命名（直观，简洁），
2维的变量数组（表格），压缩程序篇幅，方便调试
教训
没有完全采用自顶到底的方法编程，导致有些bug错误卡住，浪费了时间。
没有每一步输出一个结果，一大段编程debug不易。
没有完善的监视器，包括硬件不支持（todo修改2线转1线，并将pc串口与舵机串口分开）
