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 舵机输出角度表，然后从角度表统一输出。 此处的跳过的动作不利于串口的输出，因为打破了对齐。
}
}
网友评论