20230326

// 本程序驱动了悟空机器人右腿的髋关节（ID=5），膝关节（ID=6），踝关节（ID=7）。和左腿髋关节（ID=8），膝关节（ID=9），踝关节（ID=10）

// 这个步态不能走起来，只能在架子上摆腿

// 本程序将 ActionArray[17][8] 增加了小周期保持位角度，因为小周期填不满大周期 ，所以要在间期填充角度 ， 增加到 ActionArray[17][9]

// bug ， 上传本程序，运行正常。 打开PCarduino软件的串口监视器，再关闭串口监视器后，舵机异常，舵机表现为一跳一跳的分段快速转动。

    // debug  可能的解释-- 打开再关闭PC arduinoIDE软件的串口监视器后，输出到舵机的信号间隔变长

    // debug 程序最后的 delay(2); 改成delay(20);  打开再关闭PC arduinoIDE软件的串口监视器后，经过更长时间，舵机才开始一跳一跳的分段快速转动。说明PC串口有某种缓存。

    // debug 这是 Leonardo 板子独有的问题，打开再关闭串口监视器后， Leonardo的运行速度大幅降低。 

    // debug 原因是：Leonardo 和 PC 之间的链接与 Uno 或 Mega 之间的链接工作方式不同，如果 PC 不接受数据，它的输出缓冲区可能会填满并阻塞

            //（https://forum.arduino.cc/t/serial-print-on-leonardo-very-slow-after-disconnection-from-serial/483017）。

    // debug  通过  Serial.println(USB_EP_SIZE);可知 leonardo 板子，usb的串口buffer size =64。

            //（https://forum.arduino.cc/t/serial-print-on-leonardo-very-slow-after-disconnection-from-serial/483017/8）

    // debug 如果串口连接尚未建立，打印/写入基本上会被忽略；当再次关闭串口时，问题就来了。

            //（https://forum.arduino.cc/t/serial-monitor-causes-lagging/954328/2）

    // debug 打开再关闭PC串口， arduino leonardo  串口就会堵塞， arduino leonardo 发送到舵机的频率变慢，  发送到机器人舵机命令间隔时间变长。

// bug  outPutToServo[5] 大--0x70，unsigned int CycleTime=2000小， 同样的程序，为何不开串口监视器就能很好的工作，打开了串口监视器，舵机运动范围变小，并且吱吱响，

    // debug ， outPutToServo[5] 大--0x70，舵机运动幅度小，达不到指令的角度。 关闭PC 串口监视器后，舵机的表现趋于正常，转动幅度更大，舵机角度相位更加符合指令。

    //debug  outPutToServo[5]  大--0x70，会让转速下降，还没有转动到目标角度，就需要向回转动，造成舵机指令相位和舵机实际角度相位 不符。

    // debug  可能的解释-- 关闭PC arduinoIDE软件的串口监视器后，arduino 的速率反而降低，输出到舵机的信号间隔变长，舵机角度误差变大，舵机加速运行

    //关闭串口监视器也不能马上变好，10秒后才能好。

    //使用pc串口波特率1M,没有改善

// todo  微调各种参数，试图让机器人从吊架上拿下来，开始在平地上走路

//Wave=sin(sysTimeWithPhase % CycleTime / (CycleTime/6.28)); // 修改了这个语句，验证了不同舵机可以有不同的周期

//  todo bug 舵机偶尔抽风，然后恢复，  todo 抓包分析                  ---更换leonardo 板子后没有问题

//  todo bug 另外在不关机器人，更新上传arduino 程序时，舵机大幅摆动    ---更换leonardo 板子后仍然有问题

//  todo bug 舵机有吱吱声，也许因为角度太小，空心杯电机不停的来回摆动,

//  todo 用print（）简单测试每个loop周期耗时。sin（）函数等较费时间，

//  todo bug 舵机角度幅度 大于0x76 就开始不正常 ，右髋舵机的表现是从向后踢腿变成向前踢腿。    ---更换leonardo 板子后没有问题

/*

  ServoNtrlPos[1]= 0x70 ;      //左肩    //每个舵机的中立位（将机器人站直，然后读取角度，写入本程序中）

  ServoNtrlPos[2]= 0x70 ;      //左肘

  ServoNtrlPos[3]= 0x70 ;      //右肩

  ServoNtrlPos[4]= 0x70 ;      //右肘

  ServoNtrlPos[5]= 0x60 ;            //左髋

  ServoNtrlPos[6]= 0x70 ;            //左膝

  ServoNtrlPos[7]= 0x60 ;            //左踝

  ServoNtrlPos[8]= 0x75 ;                  //右髋

  ServoNtrlPos[9]= 0x70 ;                  //右膝

  ServoNtrlPos[10]= 0x80 ;                //右踝

  ServoNtrlPos[11]= 0x70 ;      //头x

  ServoNtrlPos[12]= 0x70 ;      //头y

  ServoNtrlPos[13]= 0x70 ;      //头z

  ServoNtrlPos[14]= 0xF0 ;      //腰

*/

// ActionID  ServoID  ServoNtrlPos  TimePosition1          TimePosition2        ActionPhase  ActionTime                  AngleRange

// 动作编号  舵机编号    舵机中立位置  时间轴位置（开始）      时间轴位置（结束）    动作相位      动作从开始到结束的总时间    舵机角度幅度

//0编号1舵机2中立位 3始时4末时  5相位  6时间  7幅度 8小周期保持位角度  // todo 加入 小周期的sin（）周期长度，现在只有起始时间和结束时间和相位

int ActionArray[17][9] = {

    {0,20,  0,    0,    0,    0,    0,    0,    0    },  //

    {1,5,  0x76,  0,    4000,  0,    0x80,  0x10,  0x76 },  //  右髋 舵机5  <<<<<bug 角度>0x76 异常 ，后踢变成前踢。debug sin*幅度>255，溢出断层，溢出结束后再次断层

    {2,6,  0x82,  0,    4000,  0,    0x20,  0x10,  0x82 },  //  右膝 舵机6                           

    {3,7,  0x78,  1000, 2000,  500,  0x20,  0x15,  0x78 },  //  右踝 舵机7  数值减小，脚向内 // <<<<<<bug 与下一行参数相同，串口数据相同，舵机不摆动，debug见241行

    {4,7,  0x78,  2000, 4000, 1000,  0x20,  0x15,  0x78 },  //  右踝 舵机7 

    {5,8,  0x76,  0,    4000,  0,    0x80,  0x10,  0x76 },  //  左髋 舵机8

    {6,9,  0x72,  0,    4000,  0,    0x20,  0x10,  0x72 },  //  左膝 舵机9

    {7,10,  0x78,  1000, 2000,  0,    0x20,  0x15,  0x78 },  //  左踝 舵机10  数值增大，脚向内 // <<<<<<bug 与下一行参数相同，串口数据相同，舵机不摆动，debug见241行

    {8,10,  0x78,  2000, 4000,  0,    0x20,  0x15,  0x78 },  //  左踝 舵机10

    {9,20,  0,    0,    0,    0,      0,      0,    0  },

    {10,20, 0,    0,    0,    0,      0,      0,    0  },

    {11,20, 0,    0,    0,    0,      0,      0,    0  },

    {12,20, 0,    0,    0,    0,      0,      0,    0  },

    {13,20, 0,    0,    0,    0,      0,      0,    0  },

    {14,20, 0,    0,    0,    0,      0,      0,    0  },

    {15,20, 0,    0,    0,    0,      0,      0,    0  },

    {16,20, 0,    0,    0,    0,      0,      0,    0  }  //  腰 舵机14

    };

//常量===============

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 将会反相

String printDATA = "";

//步进 度

//变量=================

//定义 机器人结构，舵机，传感器

//定义 步态，腿 走路速度， 抬腿高度，抬腿距离，

//定义 步态，臂

//定义 步态，头

//定义 步态，腰

byte outPutToServo[10];

byte inByte[10];

byte sum;

unsigned long sysTime;

unsigned int sysTimeWithPhase;

unsigned int CycleTime=4000;                      // 用于取余，类似于钟表表盘刻度，6秒一个周期就是 6000ms, <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<Time=2000

float Wave;

//setup()===========

void setup() {

  ///////////////////// 通讯接口

  Serial.begin(1000000);                                  //这个是与电脑通讯的比特率  // start  Serial  port 

  Serial1.begin(1000000);                                  //这个是与舵机通讯的比特率  // start  Serial1 port

    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] = ActionArray[ActionID][6];                                  //  对于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==8) {ActionID=0;}   

ActionID = ActionID+1  ;

/**/

  ////////////////////// 读传感器

  ////////////////////// 读遥控器

  /////////////////////// 读时间

sysTime = millis();                                      //存储 32 位（4 字节）

int SmallCycleTime = ActionArray[ActionID][4]  -  ActionArray[ActionID][3];

int  BigPhaseTime = sysTime % CycleTime;

/*

if (  (BigPhaseTime <= ActionArray[ActionID][3] ) || (BigPhaseTime >= ActionArray[ActionID][4] )  ) {

      // Serial.print(' move to next Action ');

      // return ;              // 实际运行时需要。<<<<<<<<<<<<<<<<

      outPutToServo[4] =0xFF;  // 为了保证  串口绘图器数据对齐 的要求，插入的语句。实际运行时去掉。<<<<<<<<<<<<<<<<

}

*/

  Wave=sin( (BigPhaseTime-ActionArray[ActionID][3]+ ActionArray[ActionID][5]) / (SmallCycleTime/6.28));

                                                            // 在周期内，Wave取值区间：负一到正一 ,作为步态 的核心发生器   

                                                            //  % 取模 

                                                            //  (CycleTime/6.28)是 2000ms对应了2pi（6.28），所以要除以2000/6.28 

                                                            //  sin(rad)  正弦函数， （以弧度为单位）。结果将介于 -1 和 1 之间

  /////////////////////////计算步态

  /////////////////////// 写舵机

  outPutToServo[2] = ActionArray[ActionID][1] ;

  outPutToServo[4] =  ActionArray[ActionID][2]  +  int(  Wave *  (ActionArray[ActionID][7])  ) ;     

    //  中立位 + 时间相位*幅度  <<<<<<<<<<<<  bug 计算报错    debug  舵机运动角度的幅度，不能没有[下标]

  //为了数据对齐，加入了outPutToServo[4] =ActionArray[ActionID][8] ， 而不是直接用return 略过

  if (  (BigPhaseTime <= ActionArray[ActionID][3] ) || (BigPhaseTime >= ActionArray[ActionID][4] )  ) {

      // return ;                                          //  move to next Action    <<<<<<<<<<<<<<<<

      outPutToServo[4] =ActionArray[ActionID][8] ;          // 为了保证  串口绘图器数据对齐 的要求，插入的语句。

        // <<<<<<<<<<<<<<<< bug  debug  =ActionArray[ActionID][9] 

}

  //todo    outPutToServo[4] = 如果数据大于255就写255，

  //向右移8位后大于1，说明大于255 (ActionArray[ActionID][2]  +  int(  Wave *  (ActionArray[ActionID][7])  )  ) >> 8  ;

  // 防止向上溢出,  但是不能防止向下溢出，--负数跳变成255，导致数据断层

  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++) {                                         

    Serial1.write(outPutToServo[i]);                                //  <<<<<<<    arduino输出到虚拟串口，控制舵机, ，写10 byte

    //Serial.write(outPutToServo[i]);                        //  <<<<<<<  写舵机数据  ，写10 byte

    }

/*

  if (ActionID ==3  ){                                      //打印到串口监视器

    Serial.println("Servo= "  ) ;

    Serial.println(outPutToServo[2]  ) ;

    Serial.println( "Angle=" ) ;

    Serial.println(  outPutToServo[4]  ) ;

    }

*/

/**/

if (ActionID <4  ){                                      //准备数据， 串口绘图器

    printDATA.concat(outPutToServo[4] );

    printDATA.concat(",");

    }

if (ActionID ==4  ){                                        //打印数据，串口绘图器

    printDATA.concat(outPutToServo[4] );

    Serial.println(  printDATA ) ;                          //Serial.println("");

    printDATA="";

    //Serial.println(USB_EP_SIZE);                                                        //  Serial 串口缓冲区 总共大小

    //Serial.print( "SerialAvailableForWrite");Serial.println( Serial.availableForWrite());//  Serial 串口缓冲区剩余大小

    }

delay(1);                            // <<<<<<<<<<<<<<<<<<<< bug  delay(20)卡顿严重， debug  减小延迟delay(1)，  或取消延时 ，明显改善。

                                      // <<<<<<<<<<<<<<<<<<<< bug  打开pc串口再关闭pc串口，机器人卡顿延时较长， debug  减小或取delay（） ，延时稍好。

  if (  (ActionID ==3 ) && (BigPhaseTime >= ActionArray[ActionID][3] ) && (BigPhaseTime <= ActionArray[ActionID][4] )  ) {

      ActionID =ActionID+1 ;          // <<<<<<<< bug在63行， debug为了保证  刚写下的角度不会被后面的动作覆写，跳过下面的动作。

                                      // todo 舵机输出角度表，然后从角度表统一输出。  此处的跳过的动作不利于串口的输出，因为打破了对齐。

}

  if (  (ActionID ==7 ) && (BigPhaseTime >= ActionArray[ActionID][3] ) && (BigPhaseTime <= ActionArray[ActionID][4] )  ) {

      ActionID =ActionID+1 ;          // <<<<<<<< bug在63行， debug为了保证  刚写下的角度不会被后面的动作覆写，跳过下面的动作。

                                      // todo 舵机输出角度表，然后从角度表统一输出。  此处的跳过的动作不利于串口的输出，因为打破了对齐。

}

}