No output from Odometry code

Programming Support
1

The problem is that it doesn’t output anything. At the bottom of the OdomCode.h file it tries to print what the angle of the robot should be. I’ve also tried copy and pasting all the code into main.cpp but it still had the same problem so I think it’s a logic issue.

OdomCode.h File-

#include "vex.h"

using namespace vex;

int odom() {//Using int since something needs to be returned but doesn't need to do anything
 
//Variables for encoders
 //Change of encoder since last reset.  A reset is a known location
 double ALr;//Left
 double ARr;//Right
 //Encoder Change since last cycle
 double AL;//Left
 double AR;//Right
 double AS;//Back
 //Previous encoder position
 double pL = 0;//Left
 double pR = 0;//Right
 double pS = 0;//Back
 
 //Positional stuff
 double O1;//New absolute orientation
 double O0=0;//previous Global orientation
 double Or=0;//Global orientation at last reset.
 double AO;//Change in angle
 double Om; //Average orientation
 
 double d0 [] = {0,0};//Previous global position.  1st term is x 2nd term is y
 double d1 [] = {};//Current global position.
 double Ad1 [] = {0,0};//Current local offset.  1st term is x 2nd term is y
 double Ad [] = {0,0};//Current Global offset but rotated by -Om
 double Adp [] = {};//A place holder for Ad when doing the polar part.  1st term is r  2nd term is θ
 
 //Distances from tracking point to wheel
 int Sl=3; //Left wheel to tracking point
 int Sr=-3; //Right wheel to tracking point
 int Ss=3; //Back wheel to tracking point
 
 //Defining a switch statement
 int Global_Position=0;
 
 while (1) {
   //TO DO: Add system for updating reset point
 
   //Updating ARr and ALr
   ALr = EncoderL.position(degrees);
   ARr = EncoderR.position(degrees);
   //Calculating change in encoder position
   AL = 360 /(EncoderL.position(degrees) - pL) * M_PI * 2.75;//Percent of rotation since last cycle*π*2.75
   AR = 360 /(EncoderR.position(degrees) - pR) * M_PI * 2.75;//Percent of rotation since last cycle*π*2.75
   AS = 360 /(EncoderS.position(degrees) - pS) * M_PI * 2.75;//Percent of rotation since last cycle*π*2.75
 
   //Main block of code
 
   //Calculate global orientation
   O1 = Or + (ALr-ARr) / (Sl+Sr);
   //Calculate change in angle
   AO = O1 - O0;
 
   //Calculate global position
   if (AO == 0){
     Global_Position = 1;
   }
   switch (Global_Position) {
     case 0: //Standard equation
     Ad1 [0] = 2 * sin(AO/2) * (AS/AO + Ss);
     Ad1 [1] = 2 * sin(AO/2) * (AR/AO + Sr);
     break;
     case 1: //Makes sure system doesn't break from dividing by 0
     Ad1 [0] = AS;
     Ad1 [1] = AR;
     break;
   }
 
   //Calculate Global position/offset
   //Calculating average orientation
   Om = O0 + (AO/2); 
   //Converting from cartesian to polar
   Adp [0] = sqrt(Ad1 [0] * Ad1 [0] + Ad1 [1] * Ad1 [1]); 
   Adp [1] = atan(Ad1[1] / Ad1[0]); 
   //Doing math to modify something polar cords.  Yea idk
   Adp [1] = Adp [1]* -Om;
   //Making polar into cartesian
   Ad [0] = Adp [0] * cos(Adp [1]);
   Ad [1] = Adp [0] * sin(Adp [1]);
  
   //Calculate new absolute position
   d1 [0] = d0 [0] + Ad [0];
   d1 [1] = d0 [1] + Ad [1];
 
   //Updating previous Values
   //Previous Encoders
   pL = EncoderL.position(degrees);
   pR = EncoderR.position(degrees);
   pS = EncoderS.position(degrees);
   //Updating Previous angle
   O0 = AO;
   //Updating Previous position
   d0 [0] = d1 [0];
   d0 [1] = d1 [1];
   //Resetting switch statement thing
   Global_Position = 0;
 
   Brain.Screen.printAt(1,95, "Anle the bot should be at: %f", O1);
 
   vex::task::sleep(10); // Sleep the task for a short amount of time 
 }
 return 0;
}

main.cpp File-

#include "vex.h"
#include "OdomCode.h"

vex::task odomTask (odom); //MultiThreading part
//Nothing else is relevant
2

3 wires weren’t flush with the ports so it wasn’t getting any inputs from the encoders.

2 Likes