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