int main () {
// just iterate through array of moves and check that they display correctly
// in the model
PhysicsModel testMod = PhysicsModel();
testMod.init();
char testMoves[] = {'+','+','+','+','+','+','+',
'd','d','d','a','a','a',
'w','w','w','x','x', 'x',
'q','q','q','e','e','e',
'-','-','-','-','-','-','-'
};
// Used to see result of move
double positionBefore[sizeof(testMoves)/sizeof(testMoves[0])][3];
double positionAfter[sizeof(testMoves)/sizeof(testMoves[0])][3];
usleep(2000000);
if(testMod.err != simx_error_noerror) {
cout << "Test failed: " << testMod.err << endl;
return 0;
}
for(int i = 0; i < (sizeof(testMoves)/sizeof(testMoves[0])); i++) {
testMod.getPosition(positionBefore[i]);
testMod.sendCommand(testMoves[i]);
usleep(500000); // Slowed down for clarity
testMod.getPosition(positionAfter[i]);
}
for(int i = 0; i < (sizeof(testMoves)/sizeof(testMoves[0])); i++) {
switch(testMoves[i]) {
case 'a' :
test_a(positionBefore[i], positionAfter[i]);
break;
case 'd' :
test_d(positionBefore[i], positionAfter[i]);
break;
case 'w' :
test_w(positionBefore[i], positionAfter[i]);
break;
case 'x' :
test_x(positionBefore[i], positionAfter[i]);
break;
case 'q' :
test_q(positionBefore[i], positionAfter[i]);
break;
case 'e' :
test_e(positionBefore[i], positionAfter[i]);
break;
case '+' :
test_up(positionBefore[i], positionAfter[i]);
break;
case '-' :
test_down(positionBefore[i], positionAfter[i]);
break;
}
}
testMod.getPosition(positionBefore[0]);
double target_move[] = {3, 3, 3};
testMod.moveTarget(target_move);
usleep(2000000);
testMod.getPosition(positionAfter[0]);
if(positionBefore[0][0] < positionAfter[0][0] &&
positionBefore[0][1] < positionAfter[0][1] &&
positionBefore[0][2] < positionAfter[0][2]) {
cout << "Target move successful, and quad follows it as planned" << endl;
}
else cout << "Target move FAILED" << endl;
usleep(1000000);
//Quad should jump back to(wards?) where it was before the target was moved;
testMod.rectify(positionBefore[0][0], positionBefore[0][1], positionBefore[0][2], 1.1);
usleep(1000000);
//and back again
testMod.setPosition(positionAfter[0]);
testMod.stop();
return 0;
}
