Odometry no worky

Programming Support VEXcode Pro V5 Text Tech Support
#1

So, I’ve attempted odom, but I guess maybe I’ve misunderstood some pretty basic concepts about it. Could you guys tell me what is wrong with this (super boof) code >>
CODE] #include “vex.h”

void GoToPoint ( double XPosition, double YPosition, double FinalDesiredAngle){

double Kp = 0.82;
double Kd = 0.0007;

int SpeedR;
int SpeedL;

double StartingPosX = 0;
double StartingPosY = 0;

int Field[144];

double Point = sqrt((pow(XPosition, 2)) + (pow(YPosition, 2))); // FInds point to drive to
double Angle = atan(XPosition/YPosition); // FInds angle to turn to to get there

//___________________________________________________________________________________________________________________//
if ( Gyro.heading(deg) != Angle){

if (Angle <= 180) {

if ( Gyro.heading(deg) < 180){
Gyro.setHeading(359, deg);
}

while (Gyro.heading(deg) < Angle) {                        
  double gError = Angle - Gyro.heading(deg);                    
  float Speed = gError * Kp + Gyro.heading(deg) * Kd;             
    FL.spin(fwd,   Speed , pct);                                   
    BL.spin(fwd,   Speed , pct);                             
    //BLL.spin(fwd, -Speed , pct);         //Right                   
    FR.spin(fwd,  -Speed , pct);                               
    BR.spin(fwd,  -Speed , pct);                             
    //BRR.spin(fwd,  Speed , pct);                             
}

}

if (Angle > 180 && Angle < 359) {

if ( Gyro.heading(deg) < 180){                              
  Gyro.setHeading(359, deg);                                
}                                                                                                   
                                                            
while (Gyro.heading(deg) < Angle) {                        
  double gError = Angle - Gyro.heading(deg);                    
  float Speed = gError * Kp + Gyro.heading(deg) * Kd;             
    FL.spin(fwd,   Speed , pct);                                   
    BL.spin(fwd,   Speed , pct);                             
    //BLL.spin(fwd, -Speed , pct);         //Left                   
    FR.spin(fwd,  -Speed , pct);                               
    BR.spin(fwd,  -Speed , pct);                             
    //BRR.spin(fwd,  Speed , pct);                             
}

}

//___________________________________________________________________________________________________________________//

}else if (Gyro.heading(deg) != Angle){

while( FR.position(deg) + 14 < Point && FL.position(deg) + 14 < Point ){

//Potential
double Error = Point - (FR.position(deg) + FL.position(deg)) / 2 ;

//Derivitave 
double DerivativeR = (FR.position(deg) + FL.position(deg)) / 2 ;

SpeedR = Error * Kp + DerivativeR * Kd;
SpeedL = Error * Kp + DerivativeR * Kd;

wait( 50, msec);

FL.spin(fwd, SpeedL, pct);      
BL.spin(fwd, SpeedL, pct);
//BLL.spin(fwd, -SpeedL, pct);
FR.spin(fwd, SpeedR, pct);      
BR.spin(fwd, SpeedR, pct);
//BRR.spin(fwd, -SpeedR, pct);

  wait(20, msec);

}
}

FL.stop(brakeType::hold);
BL.stop(brakeType::hold);
//BLL.stop(brakeType::hold);
FR.stop(brakeType::hold);
BR.stop(brakeType::hold);
//BRR.stop(brakeType::hold);

if ( FinalDesiredAngle < 360){

if (Angle <= 180) {

if ( Gyro.heading(deg) < 180){
Gyro.setHeading(359, deg);
}

while (Gyro.heading(deg) < Angle) {                        
  double gError = Angle - Gyro.heading(deg);                    
  float Speed = gError * Kp + Gyro.heading(deg) * Kd;             
    FL.spin(fwd,   Speed , pct);                                   
    BL.spin(fwd,   Speed , pct);                             
    //BLL.spin(fwd, -Speed , pct);         //Right                   
    FR.spin(fwd,  -Speed , pct);                               
    BR.spin(fwd,  -Speed , pct);                             
    //BRR.spin(fwd,  Speed , pct);                             
}

}

if (Angle > 180 && Angle < 359) {

if ( Gyro.heading(deg) < 180){                              
  Gyro.setHeading(359, deg);                                
}                                                                                                   
                                                            
while (Gyro.heading(deg) < Angle) {                        
  double gError = Angle - Gyro.heading(deg);                    
  float Speed = gError * Kp + Gyro.heading(deg) * Kd;             
    FL.spin(fwd,   Speed , pct);                                   
    BL.spin(fwd,   Speed , pct);                             
    //BLL.spin(fwd, -Speed , pct);         //Left                   
    FR.spin(fwd,  -Speed , pct);                               
    BR.spin(fwd,  -Speed , pct);                             
    //BRR.spin(fwd,  Speed , pct);                             
}

}
} else{
FL.stop(brakeType::hold);
BL.stop(brakeType::hold);
//BLL.stop(brakeType::hold);
FR.stop(brakeType::hold);
BR.stop(brakeType::hold);
//BRR.stop(brakeType::hold);

}
} CODE/ ]

Thanks all.

ps sorry for formatting, first time posting here.

1 Like
#2

Can you explain in more detail what exactly is not working and what is happening?

#3

Ah yes, “Odometry no worky”. A truly amazing name for a topic. Perfectly spot on to the problem

3 Likes
#4

You are asking a lot from this forum. The formatting makes it very difficult to read. You have no comments in your code. You do not identify what the problem is. Help us help you.

to make code formatting put three ` before and after (tilde key, no shift.)

4 Likes