C++ (Cpp) readMotion Examples

Example #1

File: mpu.c Project: QuantumDynamics/Perun

msg_t mpuPositionUpdate(void* arg)
{
	systime_t lastTime = 0;

	while (1)
	{
		mpuValues motion = readMotion();

		position.x = kalman_calculate((float) motion.A_X, (float) motion.G_X, 11, 0) / 2048.0;
		position.y = kalman_calculate((float) motion.A_Y, (float) motion.G_Y, 11, 1) / 2048.0;
		position.z = kalman_calculate((float) motion.A_Z, (float) motion.G_Z, 11, 2) / 2048.0;

		chThdSleepMilliseconds(10);
	}

	return 0;
}

Example #2

File: main.cpp Project: masiko/humanoid

int main() {
	int offset[DOF];
	int dor[DOF];
	if (setInitPos(offset, dor))	return 1;
	std::vector<ServoControl> s;
	
	s.push_back(ServoControl(p15));	//left foot role
	s.push_back(ServoControl(p16));
	s.push_back(ServoControl(p17));
	s.push_back(ServoControl(p18));
	s.push_back(ServoControl(p19));
	s.push_back(ServoControl(p20));
	
	s.push_back(ServoControl(p26));	//right foot role
	s.push_back(ServoControl(p25));
	s.push_back(ServoControl(p24));
	s.push_back(ServoControl(p23));
	s.push_back(ServoControl(p22));
	s.push_back(ServoControl(p21));
	
	for (int i=0; i<DOF; i++)	s[i].setPos(offset[i]);
	for (int i=0; i<DOF; i++)	s[i].setPower(1);

	std::vector<int> motionframe;
	char fname[] = "/local/motion.txt";
	if (readMotion(fname, &motionframe))	return 1;
//	int fnum = motionframe.size()/13;

//	motion_interpolation(s,motionframe,offset,dor,50);

	motion_interpolation_infinit(s,motionframe, offset, dor, 20);

/*
	L=1;
	for (int j=0; j<DOF; j++) {
			s[j].setPos(offset[j]);
	}
	wait_ms(5000);
*/

	for (int i=0; i<DOF; i++)	s[i].setPower(0);
	return 0;
}