Ok so I’m trying really hard to make a working position tracking program, but its not working properly. The x and y positions don’t update correctly and I have no idea why. The facing theta angle seems to be just fine though. I’m using three wheels and an inertial sensor. I’m using the inertial sensor to calculate the heading instead of the two wheels.
I’m going to link my updatePostion function here. This method is run in the competition userControl while loop. All variables are default set to zero accept for the Sl Ss Sr and wheel diameter.
I would greatly appreciate if someone could take a look or try running it to see what’s going on lol. I’m thinking it might be in my equations but I honestly have no clue at this point.
Thanks so much, - Team 23880A
int curLeft = 0;
int curRight = 0;
int curSide = 0;
int deltaLeft = 0;
int deltaRight = 0;
int deltaSide = 0;
int lastLeftPos = 0;
int lastRightPos = 0;
int lastSidePos = 0;
double pi = 3.14;
double wheelDiameter = 2;
double deltaLr = 0;
double deltaRr = 0;
double thetaNew = 0;
double Sl = 1.5;
double Sr = 1.5;
double Ss = 5;
double deltaTheta = 0;
double ang = 0;
double deltaX = 0;
double deltaY = 0;
double thetaM = 0;
double theta = 0;
double radius = 0;
double x = 0;
double y = 0;
void updatePosition(){
curLeft = lefter.position(degrees);
curRight = righter.position(degrees);
curSide = back.position(degrees);
deltaLeft = (curLeft - lastLeftPos) / 360 * pi * wheelDiameter;
deltaRight = (curRight - lastRightPos) / 360 * pi * wheelDiameter;
deltaSide = (curSide - lastSidePos) / 360 * pi * wheelDiameter;
lastLeftPos = curLeft;
lastRightPos = curRight;
lastSidePos = curSide;
deltaLr = (curLeft) / 360 * pi *wheelDiameter;
deltaRr = (curRight) / 360 * pi * wheelDiameter;
//thetaNew = (thetaReset + (deltaLr - deltaRr)/ (Sl + Sr));
thetaNew = Inertial1.heading(degrees) * (pi / 180);
deltaTheta = thetaNew - ang;
if(deltaTheta == 0){
deltaX = deltaSide;
deltaY = deltaRight;
}else{
deltaX = (2*sin(deltaTheta/2))*(deltaSide/deltaTheta + Ss);
deltaY = (2*sin(deltaTheta/2))*(deltaRight/deltaTheta + Sr);
}
thetaM = ang + deltaTheta/2;
theta = deltaTheta/2;
radius = sqrt(deltaX*deltaX + deltaY*deltaY);
theta = theta-thetaM;
deltaX = radius *cos(theta);
deltaY = radius*sin(theta);
ang = thetaNew;
x = x - deltaX;
y = y + deltaY;
Brain.Screen.setCursor(1,1);
Brain.Screen.print(x);
Brain.Screen.setCursor(2,1);
Brain.Screen.print(y);
Brain.Screen.setCursor(3,1);
Brain.Screen.print(ang);
}
edit: mods added code tags