I’m trying to develop a positioning system for our skills runs and to use odometry with three senor wheels. I understand the code isn’t completely done, but its not even changing my x and y values. Ha if anyone knows what I’m doing wrong it would be greatly appreciated thanks. Code is under this.
double tl = 8.125;
double tr = 8.125;
double ts = 6;
double delR = 0;
double delL = 0;
double delS = 0;
double x = 0;
double y = 0;
double facing = 0;
void usercontrol(void) {
// User control code here, inside the loop
while (1) {
delR = (RIGHT.position(degrees)/360)/6.283; //6.283 being the circumference of the 2 in tracking wheels
delL = ((LEFT.position(degrees))/360)*6.283;
delS = ((BACK.position(degrees))/360)*6.283;
facing += (delL - delR)/(16.25);
if (facing == 0){
x += delS;
y += delR;
} else{
x += (2*(sin(facing/2))*((delS/facing)+ts)); //add value to positions
y += (2*(sin(facing/2))*((delR/facing)+tr));
}
Brain.Screen.setCursor(5,1);
Brain.Screen.print("facing: ");
Brain.Screen.print(facing);
Brain.Screen.setCursor(6,1);
Brain.Screen.print("x: ");
Brain.Screen.print(x);
Brain.Screen.setCursor(7,1);
Brain.Screen.print("y: ");
Brain.Screen.print(y);
Brain.Screen.setCursor(1,1);
Brain.Screen.print("RIGHT: ");
Brain.Screen.print((double)(RIGHT.position(degrees)));
Brain.Screen.setCursor(2,1);
Brain.Screen.print("LEFT: ");
Brain.Screen.print((double)(LEFT.position(degrees)));
Brain.Screen.setCursor(3,1);
Brain.Screen.print("BACK: ");
Brain.Screen.print((double)(BACK.position(degrees)));
RIGHT.setPosition(0,degrees); //reset sensors for re analyzation
LEFT.setPosition(0,degrees);
BACK.setPosition(0,degrees);
}