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