代码拉取完成，页面将自动刷新
//////////////// 宏定义 勿改动 //////////////////
#define DEMO_MODE_CYCLE 0 //demo 循环播放
#define DEMO_MODE_ONCE 1 //demo 只播放一次
#define ABSA 0 // 动作描述中角度信息的类型: 绝对值 (无视舵机当前角度，直接指定舵机角度)
#define RELA 1 // 动作描述中角度信息的类型: 相对值 (以舵机当前角度为基准偏移指定角度)
#define SERVO_SUM_MAX 9 // 参与动作的舵机数量
//////////////// 功能配置宏定义 充分理解含义后再改动 //////////////////
#define SERVO_SUM 9
#define DEMO_MODE DEMO_MODE_ONCE
#define delay_ms(ms) delay((ms))
//舵机动作描述, 舵机动作库的基本单元
typedef struct _servo_motion_
{
int16_t angle; // 角度调整信息 单位: deg //
int8_t type; // ABSA or RELA
int16_t time; // 指定动作完成时间
}ServoMotion;
//舵机描述, 用以管理记录某个舵机的工作状态
typedef struct _servo_
{
uint8_t id; // 舵机ID
int16_t angle; // 舵机当前角度 单位: 度
int16_t adjust; // 角度偏移校正值
}ServoObj;
//舵机群复杂动作描述基本单元
typedef struct _servo_animation_unit_
{
int16_t mid; //选定的基础动作ID 即在动作库中的坐标
int16_t delay; //给舵机群发出这套动作指令之后(不等动作完成)，需要等待多久才能进行下一个基础动作 单位 毫秒
uint8_t speed; //变速因子(25~50)，基础动作库中的每个动作都是1秒完成90度的转动，实际执行动作时可以适当加快
//speed=50时完成动作耗时1000ms，speed=49耗时980ms , 40耗时800ms 以此类推
//但舵机转动速度总是有个上限的，所以speed也有个下限，具体我没测过，暂定为25吧，即500ms完成动作
}AnimationUnit;
//舵机群基础动作库 (存储在FLASH中)
const ServoMotion servo_motion_lib[][SERVO_SUM_MAX] PROGMEM =
{
//servoID: 1# 2# 3# 4# 5# 6# 7# 8# 9#
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//0#动作: 1#舵机角度归零,耗时1秒 其他不变
{{ 0,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//1#动作: 2#舵机角度归零,耗时1秒 其他不变
{{ 0,RELA, 0},{ 0,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//2#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//3#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//4#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//5#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//6#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0}, },
//7#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,ABSA, 1000},{ 0,RELA, 0}, },
//8#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,ABSA, 1000}, },
//9#动作: 9#舵机顺时针+90度 耗时1秒 其他不变
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 90,RELA, 1000}, },
//10#动作: 8#舵机顺时针+90度 耗时1秒 其他不变
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 90,RELA, 1000},{ 0,RELA, 0}, },
//11#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0}, },
//12#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//13#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//14#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//15#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//16#
{{ 0,RELA, 0},{ 90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//17#
{{ 90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//18# 所有舵机顺时针+90度 耗时1秒
{{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000},{ 90,RELA, 1000}, },
//19# 所有舵机逆时针-90度 耗时1秒
{{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000},{-90,RELA, 1000}, },
//20#动作: 1#舵机逆时针-90度 其他不变
{{-90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//21#动作: 2#舵机逆时针-90度 其他不变
{{ 0,RELA, 0},{-90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//22#
{{ 0,RELA, 0},{ 0,RELA, 0},{-90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//23#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{-90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//24#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{-90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//25#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{-90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//26#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{-90,RELA, 1000},{ 0,RELA, 0},{ 0,RELA, 0}, },
//27#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{-90,RELA, 1000},{ 0,RELA, 0}, },
//28#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{-90,RELA, 1000}, },
//29#动作: 1#舵机角度180,耗时1秒 其他不变
{{180,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//30#动作: 2#舵机角度180,耗时1秒 其他不变
{{ 0,RELA, 0},{180,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//31#
{{ 0,RELA, 0},{ 0,RELA, 0},{180,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//32#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{180,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//33#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{180,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//34#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{180,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0}, },
//35#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{180,ABSA, 1000},{ 0,RELA, 0},{ 0,RELA, 0}, },
//36#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{180,ABSA, 1000},{ 0,RELA, 0}, },
//37#
{{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{ 0,RELA, 0},{180,ABSA, 1000}, },
};
//舵机群动作序列 (存储在FLASH中)
//将各种基础动作通过不同的延时粘合在一起，构成复杂的动作序列
const AnimationUnit servo_demo[] PROGMEM =
{
///////////////////////////// 位置初始化 全部躺下 //////////////////////////////////////
////全部角度归零(横躺) 标准速度
{ 0, 0,100}, { 1, 0,100}, { 2, 0,100}, { 3, 0,100}, { 4, 0,100}, { 5, 0,100}, { 6, 0,100}, { 7, 0,100}, { 8, 2000,100},
///////////////////////////////////////////////////////////////////
////反序挨个竖起 110% speed
{ 9, 900, 90}, {10, 900, 90}, {11, 900, 90}, {12, 900, 90}, {13, 900, 90}, {14, 900, 90}, {15, 900, 90}, {16, 900, 90}, {17, 1500, 90},
// ////顺时针全部躺下 120%
{18, 1500, 80},
// ////正序挨个竖起 130%
{20, 700, 70}, {21, 700, 70}, {22, 700, 70}, {23, 700, 70}, {24, 700, 70}, {25, 700, 70}, {26, 700, 70}, {27, 700, 70}, {28, 1500,70},
// ///////////////////////////////////////////////////////////////////
// ////逆时针正序挨个躺下 但不等前一个完全躺下 下一个就要开始动作 标准速度
{20, 130, 60}, {21, 130, 60}, {22, 130, 60}, {23, 130, 60}, {24, 130, 60}, {25, 130, 60}, {26, 130, 60}, {27, 130, 60}, {28, 1500, 60},
// ////正序 奇数舵机先竖起 反序偶数舵机后竖起
{17, 800, 80}, {15, 800, 80}, {13, 800, 80}, {11, 800, 80}, { 9, 800, 80},
{10, 800, 80}, {12, 800, 80}, {14, 800, 80}, {16, 800, 80},
// ////反序两两一组依次顺时针躺下
{29, 0, 50}, {30, 600, 50}, {31, 0, 80}, {32, 1000, 80}, {33, 0,110}, {34, 1200,110}, {35, 0, 150}, {36, 0, 150}, {37,2500,150},
// ////反序逆时针起来且执行速度越来越快
{28, 1000,100}, {27, 960, 96}, {26, 920, 92}, {25, 880, 88}, {24, 840, 84}, {23, 800, 80}, {22, 720, 72}, {21, 600, 60}, {20, 2500, 50},
// // ////摇晃速度不变 幅度越来越大
{19, 60,200}, {18, 120,200}, {19, 140,200}, {18, 140,200}, {19, 160,200}, {18, 160,200}, {19, 180,200}, {18, 180,200}, {19, 200,200},
{18, 200,200}, {19, 220,200}, {18, 220,200}, {19, 220,200}, {18, 240,200}, {19, 260,200}, {18, 260,200}, {19, 280,200},
{18, 300,200}, {19, 300,200}, {18, 320,200}, {19, 340,200}, {18, 340,200}, {19, 360,200}, {18, 360,200}, {19, 360,200},
{18, 400,200}, {19, 440,200}, {18, 500,200}, {19, 500,200}, {18, 560,200}, {19, 600,200}, {18, 600,200}, {19, 600,200},
{18, 660,200}, {19, 660,200}, {18, 720,200}, {19, 720,200}, {18, 780,200}, {19, 780,200}, {18, 840,200}, {19, 840,200},
{ 0, 0,100}, { 1, 0,100}, { 2, 0,100}, { 3, 0,100}, { 4, 0,100}, { 5, 0,100}, { 6, 0,100}, { 7, 0,100}, { 8, 2000,100},
// // WAVE: T/2 = 1920ms, T_90deg = 960ms, T_delta = 240ms
{17, 360, 96}, {16, 360, 96}, {15, 360, 96}, {14, 360, 96}, {13, 360, 96}, {12, 360, 96}, {11, 360, 96}, {10, 360, 96}, { 9, 360, 96},
{20, 360, 96}, {21, 360, 96}, {22, 360, 96}, {23, 360, 96}, {24, 360, 96}, {25, 360, 96}, {26, 360, 96}, {27, 360, 96}, {28, 360, 96},
{17, 280, 96}, {16, 280, 96}, {15, 280, 96}, {14, 280, 96}, {13, 280, 96}, {12, 280, 96}, {11, 280, 96}, {10, 280, 96}, { 9, 280, 96},
{20, 280, 96}, {21, 280, 96}, {22, 280, 96}, {23, 280, 96}, {24, 280, 96}, {25, 280, 96}, {26, 280, 96}, {27, 280, 96}, {28, 280, 96},
{17, 200, 96}, {16, 200, 96}, {15, 200, 96}, {14, 200, 96}, {13, 200, 96}, {12, 200, 96}, {11, 200, 96}, {10, 200, 96}, { 9, 200, 96},
{20, 200, 96}, {21, 200, 96}, {22, 200, 96}, {23, 200, 96}, {24, 200, 96}, {25, 200, 96}, {26, 200, 96}, {27, 200, 96}, {28, 200, 96},
{17, 120, 96}, {16, 120, 96}, {15, 120, 96}, {14, 120, 96}, {13, 120, 96}, {12, 120, 96}, {11, 120, 96}, {10, 120, 96}, { 9, 120, 96},
{20, 120, 96}, {21, 120, 96}, {22, 120, 96}, {23, 120, 96}, {24, 120, 96}, {25, 120, 96}, {26, 120, 96}, {27, 120, 96}, {28, 120, 96},
{17, 80, 96}, {16, 80, 96}, {15, 80, 96}, {14, 80, 96}, {13, 80, 96}, {12, 80, 96}, {11, 80, 96}, {10, 80, 96}, { 9, 80, 96},
{20, 80, 96}, {21, 80, 96}, {22, 80, 96}, {23, 80, 96}, {24, 80, 96}, {25, 80, 96}, {26, 80, 96}, {27, 80, 96}, {28, 80, 96},
{17, 120, 96}, {16, 120, 96}, {15, 120, 96}, {14, 120, 96}, {13, 120, 96}, {12, 120, 96}, {11, 120, 96}, {10, 120, 96}, { 9, 120, 96},
{20, 120, 96}, {21, 120, 96}, {22, 120, 96}, {23, 120, 96}, {24, 120, 96}, {25, 120, 96}, {26, 120, 96}, {27, 120, 96}, {28, 1500, 96},
////全部角度归零(横躺) 标准速度
{ 0, 0,100}, { 1, 0,100}, { 2, 0,100}, { 3, 0,100}, { 4, 0,100}, { 5, 0,100}, { 6, 0,100}, { 7, 0,100}, { 8, 3000,100},
// ////顺时针正序挨个竖起 但不等前一个完全竖起 下一个就要开始动作 标准速度
{17, 130,200}, {16, 130,200}, {15, 130,200}, {14, 130,200}, {13, 130,200}, {12, 130,200}, {11, 130,200}, {10, 130,200}, {9, 130,200},
};
//舵机对象列表
//每个列表项代表一个舵机 里面包含了描述舵机关键属性/状态的变量
ServoObj theServos[SERVO_SUM_MAX] =
{//设定每个舵机的初始状态
{ 1, 0, 0 }, //1#舵机
{ 2, 0, 0 }, //2#舵机
{ 3, 0, 0 }, //3#舵机
{ 4, 0, 0 }, //4#舵机
{ 5, 0, 0 }, //5#舵机
{ 6, 0, 0 }, //6#舵机
{ 7, 0, 0 }, //7#舵机
{ 8, 0, 0 }, //8#舵机
{ 9, 0, 0 }, //9#舵机
};
//串口发送指定数据，然后接收数据
void serial_write_read(uint8_t* recv, uint8_t rsz, uint8_t* send, uint8_t ssz)
{
delayMicroseconds(50);
for(uint8_t i=0; i<ssz; i++)
{
Serial.write(send[i]);
Serial.flush();
}
Serial.readBytes(recv, rsz);
}
//初始化串口设备
void init_serial_port(void)
{
Serial.begin(115200, SERIAL_8N1);
Serial.setTimeout(5); //5ms
}
void init_servos(void)
{//读回每个舵机的当前角度 设置每个舵机的角度校正参数
}
//flash 加载动作数据到 ram
void load_animation_unit( AnimationUnit* ram, AnimationUnit* flash)
{
memcpy_P((void*)ram, (PGM_VOID_P)flash, sizeof(AnimationUnit));
}
//flash 加载基础动作库某个动作数据到 ram中
//motion_id - 动作ID
//ram - 接收数据的内存地址
//srvsum - 读取几个舵机动作数据 这个值必须要小于等于最大舵机数量 SERVO_SUM_MAX
void load_servo_motion_lib(int motion_id, ServoMotion* ram, int srvsum)
{
ServoMotion* src = (ServoMotion*)&servo_motion_lib[motion_id][0];
memcpy_P((void*)ram, (PGM_VOID_P)src, srvsum*sizeof(ServoMotion));
}
//计算校验和
uint8_t checksum(uint8_t* buf, uint8_t sz)
{
uint16_t sum = 0;
for(uint8_t i=0; i<sz; i++)
sum += buf[i];
return (uint8_t)(sum&0xff);
}
//给指定的舵机发送动作命令
//servo - 舵机对象
//motion - 动作参数
//speed - 变速因子 解释参见结构体声明 AnimationUnit
int send_servo_motion(ServoObj* servo, ServoMotion* motion, uint8_t speed)
{
//舵机命令组包缓冲区
static uint8_t cmd[10] = { 0xFA, 0xAF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xED };
uint8_t srvid = servo->id;
if((RELA==motion->type)&&(0==motion->angle))
{//在舵机当前角度基础上偏移0度， 即动作为空 直接返回
return 0;
}
if(RELA==motion->type)
{//相对角度 先读取舵机当前角度
cmd[2] = srvid;
cmd[3] = 0x02; //角度回读
cmd[4] = 0x00;
cmd[5] = 0x00;
cmd[6] = 0x00;
cmd[7] = 0x00;
cmd[8] = checksum(&cmd[2], 6);
serial_write_read(cmd, 10, cmd, 10); //发送10字节 接收10字节
servo->angle = cmd[7]; //保存读回的角度值
servo->angle += motion->angle;
}
else
{
servo->angle = motion->angle; //直接指定角度值
}
if(servo->angle<0)
{
servo->angle = 0;
}
//发送命令 调整角度
cmd[2] = srvid;
cmd[3] = 0x01; //设置角度
cmd[4] = servo->angle&0xFF; //最终的目标角度
{//运动时间
uint32_t v = motion->time;
v *= speed;
v /= (100*20);
cmd[5] = v&0xFF;
}
cmd[6] = 0x00;
cmd[7] = 0x00;
cmd[8] = checksum(&cmd[2], 6);
serial_write_read(&cmd[3], 1, cmd, 10);//发送10字节 接收1字节
return 0;
}
//舵机demo的驱动函数
//实际上是一个动作序列解释器
//负责将序列中的配置数据转换成一条条具体的总线舵机控制命令
//mode - 演示模式, 0 - 循环播放
// 1 - 只播放一次
//return ： 0 - 演示正常结束
// 1 - 异常出错 意外中止
int animation_start( int mode, ServoObj* srvlst, int srvsum, AnimationUnit* animate, int animate_len )
{
int step = 0; //执行到哪个动作了
AnimationUnit au;
ServoMotion srvmts[SERVO_SUM_MAX]; //用于基础动作库数据的暂存
do{
while(step<animate_len)
{
load_animation_unit(&au, &animate[step]); //从flash读取动作数据到ram
load_servo_motion_lib(au.mid, srvmts, srvsum); //根据基础动作ID，从动作库中加载指定数量的舵机数据
//给每个舵机发送动作命令
for(int i=0; i<srvsum; i++)
{
if(send_servo_motion(&srvlst[i], &srvmts[i], au.speed))
{//返回非0值，表示命令发送出错
return 1;
}
}
delay_ms(au.delay); //按照动作的要求延时
step++; //指向下一个动作
}
//动作序列执行完成 重置状态
step = 0;
}while(DEMO_MODE_CYCLE==mode);
return 0; //demo演示完成
}
void setup()
{
init_serial_port();
//获取各个舵机当前角度，修正每个舵机的角度校正值
init_servos();
}
void loop()
{
static int stop = 0; //是否停止执行demo
if(!stop)
{
int ret = animation_start
(
DEMO_MODE, theServos, SERVO_SUM, (AnimationUnit*)servo_demo, sizeof(servo_demo)/sizeof(AnimationUnit)
);
if((1==ret)||(DEMO_MODE_ONCE==DEMO_MODE))
{
stop = 1; //中止下次循环
}
}
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。