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;
	};
}
Пример #4
0
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();
}