void motion_CalibrationInit(void)
{
REGISTER_LOG_CLIENT("MOTION");
mtxDefaultSpeeds = OSMutexCreate(DEF_SPEED_MTX_PRIO, &OSLastError);
LOG_TEST_OS_ERROR(OSLastError);
encoder_SetDist(DEFAULT_ENCODER_DIST);
encoder_SetResolution(DEFAULT_LEFT_TICKS_PER_M, DEFAULT_RIGHT_TICKS_PER_M);
slippageThreshold = DEFAULT_SLIPPAGE;
isSendErrorsEnabled = FALSE;
odometrySensorUsed = MOUSE_SENSOR;
pid_ConfigKP(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 3000);
pid_ConfigKI(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 10);
pid_ConfigKD(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 10000);
pid_ConfigDPeriod(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, 3);
pid_ConfigKP(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 3000);
pid_ConfigKI(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 10);
pid_ConfigKD(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 5000);
pid_ConfigDPeriod(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, 3);
pid_ConfigKP(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, 2500);
pid_ConfigKP(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, 2500);
//register handlers
protocol_SetHandler(M32_WHEEL_PID_TUNING, motion_wheelPIDTuning_handler);
protocol_SetHandler(M32_ALPHADELTA_PID_TUNING, motion_alphaDeltaPIDTuning_handler);
protocol_SetHandler(M32_ODOMETRY_TUNING, motion_odometryTuning_handler);
}
//handler for MSG_ALPHADELTA_PID_TUNING
static void motion_alphaDeltaPIDTuning_handler(const CANMessage_uart *msg)
{
switch(GET_APP_SPE_CODE(msg->title))
{
case PID_ALPHA_KP:
pid_ConfigKP(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_ALPHA_KI:
pid_ConfigKI(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_ALPHA_KD:
pid_ConfigKD(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_ALPHA_PERIOD_D:
pid_ConfigDPeriod(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, msg->data.uchars[0]);
break;
case PID_ALPHA_IMAX:
pid_ConfigIMax(motors[ALPHA_DELTA][ALPHA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_DELTA_KP:
pid_ConfigKP(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_DELTA_KI:
pid_ConfigKI(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_DELTA_KD:
pid_ConfigKD(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_DELTA_PERIOD_D:
pid_ConfigDPeriod(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, msg->data.uchars[0]);
break;
case PID_DELTA_IMAX:
pid_ConfigIMax(motors[ALPHA_DELTA][DELTA_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
default:
LOG_ERROR("Unknown msg");
break;
};
}
// handler for MSG_WHEEL_PID_TUNING
static void motion_wheelPIDTuning_handler(const CANMessage_uart *msg)
{
switch(GET_APP_SPE_CODE(msg->title))
{
case PID_LEFT_KP:
pid_ConfigKP(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_LEFT_KI:
pid_ConfigKI(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_LEFT_KD:
pid_ConfigKD(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_LEFT_PERIOD_D:
pid_ConfigDPeriod(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, msg->data.uchars[0]);
break;
case PID_LEFT_IMAX:
pid_ConfigIMax(motors[LEFT_RIGHT][LEFT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_RIGHT_KP:
pid_ConfigKP(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_RIGHT_KI:
pid_ConfigKI(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_RIGHT_KD:
pid_ConfigKD(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
case PID_RIGHT_PERIOD_D:
pid_ConfigDPeriod(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, msg->data.uchars[0]);
break;
case PID_RIGHT_IMAX:
pid_ConfigIMax(motors[LEFT_RIGHT][RIGHT_MOTOR].PIDSys, msg->data.ulongs[0]);
break;
default:
LOG_ERROR("Unknown msg");
break;
};
}
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();
}
