void CreateTwoSegmentTraj(float V0, float distD1, float distD2, float A0, float beta, float epsilon)
{
RobotCommand cmdLine1, cmdLine2, cmdClot1, cmdClot2;
float deltaX, accel, T;
float D;
if(beta > 0)
{
deltaX = epsilon * ( 1/(sin((PI-beta)/2.0) / cos((PI-beta)/2.0) ) + FresnelCSSqrt(beta) );
accel = distEncoderMeter * V0*V0/2.0 * PI/(epsilon*epsilon) * FresnelSSqrt(beta);
T = sqrt((distEncoderMeter*beta)/(2*accel));
}
else
{
beta = -beta;
deltaX = epsilon * ( 1/ (sin((PI-beta)/2.0) / cos((PI-beta)/2.0)) + FresnelCSSqrt(beta)); //epsilon * ( 1/tan((PI-beta)/2.0) + FresnelCSSqrt(beta));
accel = -distEncoderMeter * V0*V0/2.0 * PI/(epsilon*epsilon) * FresnelSSqrt(beta);
T = sqrt((distEncoderMeter*beta)/(-2*accel));
}
D = distD1-deltaX;
if(D<0.0)
D=0.0;
motion_SpeedControlLR( &cmdLine1,
V0, D, A0,
V0, D, A0);
motion_SpeedControlLRTime( &cmdClot1,
V0, T, -accel,
V0, T, accel);
motion_SpeedControlLRTime( &cmdClot2,
V0-accel*T, T, accel,
V0+accel*T, T, -accel);
D = distD2-deltaX;
if(D<0.0)
D=0.0;
motion_SpeedControlLRV0( &cmdLine2,
V0, V0, D, A0,
V0, V0, D, A0);
path_LaunchTrajectory(&cmdLine1);
path_WaitEndOfTrajectory();
path_LaunchTrajectory(&cmdClot1);
path_WaitEndOfTrajectory();
path_LaunchTrajectory(&cmdClot2);
path_WaitEndOfTrajectory();
path_LaunchTrajectory(&cmdLine2);
path_WaitEndOfTrajectory();
}
void mouse_Calibration2(int turnNb)
{
int i;
static int mvt = 0;
RobotCommand cmd;
//reset values of calibration
for(i=0; i<MOUSE_NUMBER; i++)
{
reset(i);
}
switch(mvt++%4)
{
case 0:
LOG_DEBUG("Calibration rotation");
motion_Rotate(&cmd, 2*M_PI*turnNb);
break;
case 1:
LOG_DEBUG("Calibration line");
motion_Line(&cmd, 2.0f);
break;
case 2:
LOG_DEBUG("Calibration -line");
motion_Line(&cmd, -2.0f);
break;
case 3:
LOG_DEBUG("Calibration -rotation");
motion_Rotate(&cmd, -2*M_PI*turnNb);
break;
};
path_LaunchTrajectory(&cmd);
path_WaitEndOfTrajectory();
for(i=0; i<MOUSE_NUMBER; i++)
{
LOG_DEBUG4("M%d a= %g,%g / d= %g", i, (float)getCalibAlpha1(i), (float)getCalibAlpha2(i), (float)getCalibDelta(i)*valueVTops);
}
}
//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;
}
