void motion_SetDefaultSpeed(float speed)
{
int i;
BOOL found = FALSE;
OSMutexPend(mtxDefaultSpeeds, 0, &OSLastError);
LOG_TEST_OS_ERROR(OSLastError);
defSpeed = speed;
//find appropriate speed and set mouse resolution
for(i=0; i<CALIB_SPEED_NB; i++)
{
if(speed == calibSpeed[i])
{
encoder_SetResolution(calibMouse[i][0], calibMouse[i][1]);
found = TRUE;
break;
}
}
motors_ConfigAllIMax(SLIPPAGE_RATIO * MAX(speed, 0.3f));
if(!found)
{
encoder_SetResolution(DEFAULT_LEFT_TICKS_PER_M, DEFAULT_RIGHT_TICKS_PER_M);
}
OSMutexPost(mtxDefaultSpeeds);
}
task main()
//task testSubtask()
{
//Display NXT
nxtDisplayCenteredTextLine(0, "PMX robot-test");
nxtDisplayCenteredBigTextLine(1, "MOTOR");
nxtDisplayCenteredTextLine(3, "pmSquareTest");
writeDebugStreamLine("PMX robot-test | MOTOR | pmSquareTest");
//float dist = 1.000; // in meters
float x1, y1, theta1;
TRAJ_STATE ts;
motion_Init();
pid_ConfigKP(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 800);
pid_ConfigKI(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 0);
pid_ConfigKD(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 400);
pid_ConfigDPeriod(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 3);
pid_ConfigKP(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 500);
pid_ConfigKI(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 0);
pid_ConfigKD(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 500);
pid_ConfigDPeriod(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 3);
pid_ConfigKP(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, 75);
pid_ConfigKI(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, 0);
pid_ConfigKD(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, 50);
pid_ConfigKP(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, 75);
pid_ConfigKI(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, 5);
pid_ConfigKD(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, 50);
motors_ConfigAllIMax(90000);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
/*
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoLine(-0.10);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
*/
ts = motion_DoRotate(PI / 2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
/*
ts = motion_DoRotate(-(PI / 2.0));
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(-PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(-PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
ts = motion_DoRotate(-PI/2.0);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x=%f y=%f theta=%f",x1, y1, theta1);
*/
Sleep(1000);
odo_GetPositionXYTheta(&x1, &y1, &theta1);
writeDebugStreamLine("x1=%f y1=%f theta1=%f",x1, y1, theta1);
writeDebugStreamLine("pmSquareTest.c : END OF PMX !!!!");
motion_StopTimer();
}
