void motion_LineSpeed(RobotCommand *out_cmd, float dist, float VMax) {
motion_LineSpeedAcc(out_cmd, dist, VMax, motion_GetDefaultAccel(),
motion_GetDefaultDecel());
}
//calibration function for mouse sensor
void mouse_Calibration(int lineNb, float lineDist, int turnNb)
{
int i, k;
RobotCommand cmd;
int32 DeltaUPlus[MOUSE_NUMBER];
int32 DeltaUMinus[MOUSE_NUMBER];
int32 DeltaVPlus[MOUSE_NUMBER];
int32 DeltaVMinus[MOUSE_NUMBER];
int step = 0;
mouseCalibInitStep();
//reset values of calibration
for(i=0; i<MOUSE_NUMBER; i++)
{
uint8 code;
DeltaUPlus[i] = 0;
DeltaUMinus[i] = 0;
DeltaVPlus[i] = 0;
DeltaVMinus[i] = 0;
reset(i);
//set mouse in calibration mode
code = MOUSE_CALIBRATION_MODE;
uart_SendAll(mice[i].uart, &code, sizeof(code));
}
odometrySensorUsed = MOUSE_CALIBRATION;
LOG_DEBUG("Mouse Calibration");
//make the robot run N times x meters forward and backward
for(k=0; k<lineNb; k++)
{
LOG_DEBUG1("Calibration line %d", k);
//run forward
motion_LineSpeedAcc(&cmd, lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
//wait user input
LOG_DEBUG1("Calib : %4.2f m forward, click next when done", lineDist);
mouseCalibWaitNextStep(step++);
//accumulate values of displacement
for(i=0; i<MOUSE_NUMBER; i++)
{
DeltaUPlus[i] += getU(i);
DeltaVMinus[i] += getV(i);
reset(i);
}
//run backward
motion_LineSpeedAcc(&cmd, -lineDist, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
//wait user input
LOG_DEBUG1("Calib : %4.2f m backward, click next when done", lineDist);
mouseCalibWaitNextStep(step++);
//accumulate values of displacement
for(i=0; i<MOUSE_NUMBER; i++)
{
DeltaUMinus[i] += getU(i);
DeltaVPlus[i] += getV(i);
reset(i);
}
}
//compute ratio for each mouse
for(i=0; i<MOUSE_NUMBER; i++)
{
float theta;
LOG_DEBUG5("Mouse %d => Du+ = %ld | Du- = %ld | Dv+ = %ld | Dv- = %ld", i, DeltaUPlus[i], DeltaUMinus[i], DeltaVPlus[i], DeltaVMinus[i]);
mice[i].RatioKU = -DeltaUMinus[i]/(float)DeltaUPlus[i];
mice[i].RatioKV = -DeltaVPlus[i]/(float)DeltaVMinus[i];
theta = atan2f( DeltaVPlus[i]-DeltaVMinus[i],
DeltaUPlus[i]-DeltaUMinus[i] );
mice[i].theta = theta;
mice[i].cosTheta = cosf(mice[i].theta);
mice[i].sinTheta = sinf(mice[i].theta);
mice[i].Delta0 = (lineNb*lineDist) / (valueVTops * ((float)DeltaUPlus[i]*cosf(theta) - (float)DeltaVMinus[i]*sinf(theta)));
LOG_DEBUG4("Ratio : coef = %f | U = %f | V = %f | Theta = %f", mice[i].Delta0, mice[i].RatioKU, mice[i].RatioKV, theta);
}
//make N * 2*Pi rotation
LOG_DEBUG("Calibration rotation");
motion_RotateSpeedAcc(&cmd, 2*M_PI*turnNb, CALIBRATION_SPEED, CALIBRATION_ACCELERATION, CALIBRATION_ACCELERATION);
path_LaunchTrajectory(&cmd);
/*if(path_WaitEndOfTrajectory() != TRAJ_OK)
{
LOG_ERROR("Unable to complete calibration");
return;
}*/
LOG_DEBUG1("Calib : %d turn anticlockwise, click next when done", turnNb);
mouseCalibWaitNextStep(step++);
//compute mouse positions
for(i=0; i<MOUSE_NUMBER; i++)
{
int32 deltaU, deltaV;
int32 deltaX, deltaY;
deltaU = getU(i);
deltaV = getV(i);
reset(i);
if(deltaU < 0)
{
deltaU *= mice[i].RatioKU;
}
if(deltaV < 0)
{
deltaV *= mice[i].RatioKV;
}
deltaX = deltaU*cosf(mice[i].theta) - deltaV*sinf(mice[i].theta);
deltaY = deltaU*sinf(mice[i].theta) + deltaV*cosf(mice[i].theta);
mice[i].RX0 = deltaY / (2*M_PI*turnNb);
mice[i].RY0 = -deltaX / (2*M_PI*turnNb);
LOG_DEBUG5("Mouse %d | dU = %ld | dV = %ld | dX = %ld | dY = %ld", i, deltaU, deltaV, deltaX, deltaY);
LOG_DEBUG2("RX = %f | RY = %f", mice[i].RX0, mice[i].RY0);
sendCalibParameters(i);
}
odometrySensorUsed = MOUSE_SENSOR;
}
