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;
}
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;
}