机械臂两点间的运动（满足速度曲线）

记录用。不作为其他舵机的使用。

 

 给两组机械臂的关节角度值：

速度满足梯形曲线，已知总时间，加速度，求最大速度

我这里有四个机械臂串联：

                std::vector<double> angle_inital = angle_traj[i-1];
		std::vector<double> angle_target = angle_traj[i];
		int TotalTime = 50;
		double accelerated = 1;
		double duration[4] = { 0,0,0,0 };
		double temp = 0;
		for (int i = 0; i < 4; i++)
		{       //这里是针对我的舵机的参数计算的变量，这是有公式的，大致意思就是根据T计算可以达到的最大速度
			duration[i] = 4 * 64 * 64 * accelerated * abs(angle_target[i] - angle_inital[i]);
			temp = accelerated * accelerated* TotalTime * TotalTime;
			if (duration[i] > temp)
				TotalTime = sqrt(duration[i] / (pow(accelerated, 2)));
			std::cout << 4 * 64 * 64 * abs(angle_target[i] - angle_inital[i]) << std::endl;
		}
		double velocity[4] = { 0,0,0,0 };
//计算完时间后根据速度和总时间T检测是否有匀速的段
		for (int i = 0; i < 4; i++)
		{
			std::cout << abs(angle_target[i] - angle_inital[i]);
			velocity[i] = (accelerated * TotalTime + sqrt(pow(temp, 2) - duration[i])) / (2 * 64);
			if (abs(angle_target[i] - angle_inital[i]) >= (pow(velocity[i], 2) / accelerated))
			{
				std::cout << i << " 存在匀速段" << std::endl;
			}
			else
				std::cout << i << " 不存在匀速段" << std::endl;
		}