VEX V5 odometry

Programming Support
1

I wrote a full-field positioning program and found that when I output the values of x0,y0 on the Controller, both values are normal, but when I output the values of gx,gy on the Controller, the Controller always outputs zero (meaning that the value of gx,gy is also always 0). What’s the problem?

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       Flasd                                                     */
/*    Created:      1/11/2025, 8:48:53 AM                                     */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/
#include "vex.h"

using namespace vex;

// A global instance of vex::brain used for printing to the V5 brain screen
vex::brain       Brain;
motor left1 = motor(PORT14,ratio6_1,true);
motor left2 = motor(PORT15,ratio6_1,true);
motor left3 = motor(PORT11,ratio6_1,true);
motor right1 = motor(PORT10,ratio6_1,false);
motor right2 = motor(PORT17,ratio6_1,false);
motor right3 = motor(PORT16,ratio6_1,false);
motor xiring_ = motor(PORT19,ratio18_1,true);
motor lady_brown = motor(PORT8,ratio18_1,true);
inertial Inertial = inertial(PORT7);
rotation lady_b = rotation(PORT4);
rotation remove_x = rotation(PORT3);
digital_out take_mobile = digital_out(Brain.ThreeWirePort.D);
digital_out up_down_eat = digital_out(Brain.ThreeWirePort.H);
digital_out throww_corner = digital_out(Brain.ThreeWirePort.C);
controller Controller1 = controller(primary);
// define your global instances of motors and other devices here
int autonum = 1;
double gx = 0;
double gy = 0;
double gx_new = 0;
double gy_new = 0;
double ga = 0;
double ga_new = 0;
double ga_now = 0;
void remove_p() {
    int leftMotorSpeed =
        Controller1.Axis3.position()*1.0 + (Controller1.Axis1.position()*0.35);
    int rightMotorSpeed = 
        Controller1.Axis3.position()*1.0 - (Controller1.Axis1.position()*0.35);
    /*int rightMotorSpeed =
        Controller1.Axis3.position()*1.0 - (Controller1.Axis1.position()*0.3);*/
    if(Controller1.Axis3.position() != 0 && Controller1.Axis1.position() != 0){
      leftMotorSpeed = Controller1.Axis3.position()*1.0 + (Controller1.Axis1.position()*0.8);
      rightMotorSpeed = Controller1.Axis3.position()*1.0 - (Controller1.Axis1.position()*0.8);
    }
    else{
      leftMotorSpeed = Controller1.Axis3.position()*1.0 + (Controller1.Axis1.position()*0.35);
      rightMotorSpeed = Controller1.Axis3.position()*1.0 - (Controller1.Axis1.position()*0.35);
    }
    if (abs(leftMotorSpeed) < 5) {
      left1.setVelocity(0, percent);
      left2.setVelocity(0, percent);
      left3.setVelocity(0, percent);
    } else {
      left1.setVelocity(leftMotorSpeed, percent);
      left2.setVelocity(leftMotorSpeed, percent);
      left3.setVelocity(leftMotorSpeed, percent);
    }    

    if (abs(rightMotorSpeed) < 5) {
      right1.setVelocity(0, percent);
      right2.setVelocity(0, percent);
      right3.setVelocity(0, percent);
    } else {
      right1.setVelocity(rightMotorSpeed, percent);
      right2.setVelocity(rightMotorSpeed, percent);
      right3.setVelocity(rightMotorSpeed, percent);
    }
    left1.spin(forward);
    left2.spin(forward);
    left3.spin(forward);
    right1.spin(forward);
    right2.spin(forward);
    right3.spin(forward);
    wait(25, msec);
}
void allreset() {
    if (Controller1.ButtonUp.pressing()) {
    xiring_.resetPosition();
    lady_b.resetPosition();
    Inertial.resetRotation();
    lady_b.setPosition(0,degrees);
    remove_x.resetPosition();
  }
}
int main() {
    Controller1.Screen.clearScreen(); //清屏
  //double p = 1.182;
    double dtheta;
    double dex,dey;
    double xc,yc;
    double x0,y0;
   
while(1) {
    remove_p();
    allreset();
    Controller1.Screen.setCursor(1, 1);
    Controller1.Screen.print("gyt:%4.2f     PROG : %d ",Inertial.rotation(), autonum);
    Controller1.Screen.newLine();
    Controller1.Screen.print("len:%8.2f      ", fabs(fabs(left1.position(degrees)) + 
                                                    fabs(left2.position(degrees)) + 
                                                    fabs(right1.position(degrees)) + 
                                                    fabs(right2.position(degrees))) / 4);
    Controller1.Screen.newLine();
    Controller1.Screen.print("take:%d      ", !take_mobile.operator bool());
    Controller1.Screen.newLine();
    Controller1.Screen.print("axis:%8.2f    %8.2f", gx,gy);
    left3.resetPosition();
    right3.resetPosition();
    remove_x.resetPosition();
    wait(30,msec);
    ga_now = Inertial.rotation();
    dtheta = ga_now - ga;
    dey = left3.position(degrees) * 260;
    dex = remove_x.position(degrees) * 220;
    xc = dey/dtheta;
    yc = -dex/dtheta;
    x0 = xc - xc*cos(dtheta) - yc*sin(dtheta);
    y0 = yc - yc*cos(dtheta) + xc*sin(dtheta);
    gx_new = gx + x0*cos(ga) + y0*sin(ga);
    gy_new = gy + y0*cos(ga) - x0*sin(ga);
    ga_new = ga_now;
    gx = gx_new;
    gy = gy_new;
    ga = ga_new;
        // Allow other tasks to run
        
}
}
2

What do gx, gy, x0, y0, etc represent?

I would try logging some of your intermediate values (such as xc and dtheta) to see what might be causing the issue. Once you find something that is causing your other values to be wrong, you can troubleshoot it from there. It is also nice to create a history of your logs (I don’t know if vexcode has a terminal but if it does then use something like that), so you can see if, for example, your gx and gy values ever changed.

3

When the robot performs the superposition motion of rotation and displacement, it is actually equivalent to pure rotation motion around a special point. This rotation center is called the instantaneous center, and the coordinates of the instantaneous center are recorded by variables xc and yc. The calculation formula is the change of the robot in x axis and y axis divided by the change of Angle respectively. When the robot moves from one point to another after a minute time of 30ms, the initial point is called the starting position, then x0 and y0 represent the coordinate value under the robot coordinate system of the starting position, and the calculation formula needs to use the assistance of xc and yc variables. Finally, after x0 and y0 are calculated, the origin of the robot at the end point can be calculated in the global coordinate system, that is, gx,gy.

2 Likes
4

bro the trigonometric functions (sin, cos) in VEX C++ expect radians, but Inertial.rotation() returns degrees

you at least should try converting the angles:

// Convert degrees to radians for trig functions
double dtheta_rad = dtheta * M_PI / 180.0;
double ga_rad = ga * M_PI / 180.0;

// Use these converted values in your calculations
x0 = xc - xc*cos(dtheta_rad) - yc*sin(dtheta_rad);
y0 = yc - yc*cos(dtheta_rad) + xc*sin(dtheta_rad);
gx_new = gx + x0*cos(ga_rad) + y0*sin(ga_rad);
gy_new = gy + y0*cos(ga_rad) - x0*sin(ga_rad);

this should resolve the zero values in gx and gy.

1 Like
5

I think you are right, but after my tests, I found that the problem still exists. This is my modified program.

double gx = 1;
double gy = 1;
double gx_new;
double gy_new;
double ga = 0;
double ga_new = 0;
double ga_now = 0;
int autoswitch(){ 
  Controller1.Screen.clearScreen(); //清屏
  //double p = 1.182;
    double dtheta;
    double dex,dey;
    double xc,yc;
    double x0,y0;
  while(1){
    Controller1.Screen.setCursor(1, 1);
    Controller1.Screen.print("gyt:%4.2f     PROG : %d ",Inertial.rotation(), autonum);
    Controller1.Screen.newLine();
    Controller1.Screen.print("len:%8.2f      ", fabs(fabs(left1.position(degrees)) + 
                                                    fabs(left2.position(degrees)) + 
                                                    fabs(right1.position(degrees)) + 
                                                    fabs(right2.position(degrees))) / 4);
    Controller1.Screen.newLine();
    Controller1.Screen.print("take:%d      ", !take_mobile.operator bool());
    Controller1.Screen.newLine();
    Controller1.Screen.print("axis:%8.4f;%8.4f",gx,gy);
    left3.resetPosition();
    right3.resetPosition();
    remove_x.resetPosition();
    w(10);
    ga_now = Inertial.rotation();
    dtheta = ga_now - ga;
    dey = left3.position(degrees) * 260;
    dex = remove_x.position(degrees) * 220;
    double dtheta_rad = dtheta * M_PI / 180;
    double ga_rad = ga * M_PI / 180;
    xc = dey/dtheta_rad;
    yc = -dex/dtheta_rad;
    x0 = xc - xc*cos(dtheta_rad) - yc*sin(dtheta_rad);
    y0 = yc - yc*cos(dtheta_rad) + xc*sin(dtheta_rad);
    gx_new = gx + x0*cos(ga_rad) + y0*sin(ga_rad);
    gy_new = gy + y0*cos(ga_rad) - x0*sin(ga_rad);
    ga_new = ga_now;
    gx = gx_new;
    gy = gy_new;
    ga = ga_new;
  }
}