Skip to content

Instantly share code, notes, and snippets.

Created Jun 26, 2015
Embed
What would you like to do?
/* setpointConfigurator()
*
* IN: - Direc is the decision variable defining left (Direc==0), straight (Direc==1), right(Direc==2)
*
* ACT: setPointConfigurator() selects a setpoint from the list of setpoints in setpointPath which is
* not reached yet (setpointPath[i][2]==0) and skips over reached setpoints (setpointPath[i][2]==1)
*
* OUT: - sets the new intermediate marker to reach the end setpoint in setpointPath[n-1][2]
*/
go_to setpointConfigurator(int Direc)
{
go_to setpoint;
int index =setpointPath.size()-1;
// find next setpoint to work on
for(int i = 0;i<setpointPath.size();i++)
{
if(setpointPath[i][2] == 0)
{
index = i;
i = setpointPath.size();
}
}
// Dealing with left setpoint
if(Direc==0)
{
setpoint.x = setpointPath[index][1];
setpoint.y = setpointPath[index][0];
xPoint = setpointPath[index][1];
yPoint = setpointPath[index][0];
}
else
{
setpoint.x = setpointPath[index][1];
setpoint.y = -setpointPath[index][0];
xPoint = setpointPath[index][1];
yPoint = -setpointPath[index][0];
}
return setpoint;
}
/* setPointUpdater()
*
* IN: - Direc is the decision variable defining left (==0), straight (==1), right(==2)
* - global go_to setpoint when setpoint is updated using odom_data
* - emc::LRFdata scan
* - emc::Odometry odom
*
* ACT: setPointUpdater corrects the setpoint based on its current position
* and traveled distance from the global marker.
*
* OUT: - gives the new setpoint if nothing weird happens
* - else it gives setpoint at zero so the robot no longer drives
*
*/
go_to setPointUpdater(int Direc, go_to globalSetPoint, emc::OdometryData &odom, double* straight_data)
{
go_to setpoint;
int index;
// left == 0
if(Direc == 0)
{
double r = sqrt(pow(xPoint,2)+pow(yPoint,2));
double theta1 = atan2(yPoint,xPoint);
double deltaSy;
double deltaSx;
// Incremental change in x and y of the setpoint
if (fmod(fabs(counterLeft-counterRight),2)==0)
{
deltaSy = fabs(absMarkerX-odom.x);
deltaSx = fabs(absMarkerY-odom.y);
}
else
{
deltaSy = fabs(absMarkerX-odom.y);
deltaSx = fabs(absMarkerY-odom.x);
}
double dthetaa = -absMarkerA+odom.a;
if (deltaSx==0)
{
return globalSetPoint;
}
else
{
// calculate preliminaries
double dtheta = atan2(deltaSy,deltaSx);
double rdelta = sqrt(pow(deltaSx,2)+pow(deltaSy,2));
double theta = -dtheta+theta1;
// calculate r
double rperp = rdelta*sin(theta);
double rL = r-rdelta*cos(theta);
double theta4 = atan2(rperp,rL);
double rt = rL/cos(theta4);
// calculate theta2
double theta2 = dtheta-dthetaa+theta+theta4;
// update setpoint
globalSetPoint.x = rt*cos(theta2);
globalSetPoint.y = rt*sin(theta2);
return globalSetPoint;
}
}
// straight == 1
else if(Direc == 1)
{
double delta_y = (straight_data[2] + sin(straight_data[3])) ;
setpoint = driveStraightInitializer();
setpoint.y = delta_y;
return setpoint;
}
// right == 2
else if(Direc == 2)
{
double r = sqrt(pow(xPoint,2)+pow(yPoint,2));
double theta1 = atan2(fabs(yPoint),xPoint);
double deltaSy;
double deltaSx;
// Incremental change in x and y of the setpoint
if (fmod(fabs(counterLeft-counterRight),2)==0)
{
deltaSy = fabs(absMarkerX-odom.x);
deltaSx = fabs(absMarkerY-odom.y);
}
else
{
deltaSy = fabs(absMarkerX-odom.y);
deltaSx = fabs(absMarkerY-odom.x);
}
double dthetaa = -absMarkerA+odom.a;
if (deltaSx==0)
{
return globalSetPoint;
}
else
{
// calculate preliminaries
double dtheta = atan2(deltaSy,deltaSx);
double rdelta = sqrt(pow(deltaSx,2)+pow(deltaSy,2));
double theta = -dtheta+theta1;
// calculate r
double rperp = rdelta*sin(theta);
double rL = r-rdelta*cos(theta);
double theta4 = atan2(rperp,rL);
double rt = rL/cos(theta4);
// calculate theta2
double theta2 = -(dtheta+dthetaa+theta+theta4);
// update setpoint
globalSetPoint.x = rt*cos(theta2);
globalSetPoint.y = rt*sin(theta2);
return globalSetPoint;
}
}
// something else happened
else
{
setpoint.x = 0;
setpoint.y = 0;
return setpoint;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment