V5 inertia sensor code

nygil · March 24, 2021, 5:00pm

void tug(int to,int deg,int spe){
/*
to = timeout 
deg = degres you want to turn + for right - for left
spe = speed
*/


//resets all sensors / integers
int deg2 = 0;
Brain.Timer.reset();
Inertial.calibrate();
Inertial.setHeading(0, degrees); 





if(deg < 0){


//for going left you need to find the distence from 360
deg2 = deg - 360;

  FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
  BackL.spin(directionType::fwd, spe,velocityUnits::pct);
  FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
  BackR.spin(directionType::fwd, spe,velocityUnits::pct);

  wait(90,msec);

//loop till the gyro states that is withen the deadzone
while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 4) || Inertial.heading(degrees) < (deg2 - 4))){

Controller1.Screen.clearScreen(); 
Controller1.Screen.print(Inertial.heading(degrees)); 
wait(60,msec);

// telling the robot to ether turn left or right to get to where it needs to go 
if(Inertial.heading(degrees) < (deg2 - 1)){

  //turn right
  FrontL.spin(directionType::rev, spe,velocityUnits::pct);
  BackL.spin(directionType::rev, spe,velocityUnits::pct);
  FrontR.spin(directionType::rev, spe,velocityUnits::pct);
  BackR.spin(directionType::rev, spe,velocityUnits::pct);
  

}else if(Inertial.heading(degrees) > (deg2 + 1)){

  //turn left    
  FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
  BackL.spin(directionType::fwd, spe,velocityUnits::pct);
  FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
  BackR.spin(directionType::fwd, spe,velocityUnits::pct);

}

wait(50,msec);



}
}else if(deg > 0){
while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 2) || Inertial.heading(degrees) < (deg2 - 2))){




if(Inertial.heading(degrees) < (deg - 1)){

  //turn right
  FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
  BackL.spin(directionType::fwd, spe,velocityUnits::pct);
  FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
  BackR.spin(directionType::fwd, spe,velocityUnits::pct);

}else if(Inertial.heading(degrees) > (deg + 1)){

  //turn right    
  FrontL.spin(directionType::rev, spe,velocityUnits::pct);
  BackL.spin(directionType::rev, spe,velocityUnits::pct);
  FrontR.spin(directionType::rev, spe,velocityUnits::pct);
  BackR.spin(directionType::rev, spe,velocityUnits::pct);

}



}



}

}

So recently I have had a struggle with being able to get my inertia sensor to work with turns with the code I currently am showing yo my robot does not move at all would anyone be able to help/work with me to figure out this problem so could use this at my next competition . Or if anyone thinks it would be easier to use the three wire encoders that I use for everything else

DrewWHOOP · March 24, 2021, 5:54pm

you should really consider at the very least using a P loop. What you currently have is going to massively overshoot the target when it does work.

nygil · March 24, 2021, 6:08pm

ok do you know where I could go to find a example of it?

invalidflaw · March 24, 2021, 7:26pm

Couple of comments,

First, don’t focus so much on what sensors are best as much as focusing on getting a single sensor to work. Both the encoders or the inertial sensor can work. For this discussion, I’m going to say to use the intertial sensor. This removes the need to calibrate for wheel size, rotation offset etc.

Second, your code is SLOW. in your while loop, you have 110 millisecods of waits between the wait after the controller print and after the ifelse statement. In addition, you are printing to the controller screen. I haven’t tested it recently, however a couple of years ago when I did, the controller screen took nearly 100 ms to draw anything. If that is the case, you are only checking your rotation rate 4-8 times a second, which is really slow. Try taking out one wait command and only wait for 10ms. That is all you really need to allow task processing. Also print to the brain screen, not the controller, its much faster.

Now that my obligitory nitpicks are out of the way lets get to the issue. I think the problem is here

while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 4) || Inertial.heading(degrees) < (deg2 - 4))){

Here I believe you are saying that the timer is not triggered and you are not within a certain goal angle. However your logic doesn’t reflect that. You accidently lumped one of the angle checks with tiem timer check. To solve this, try

while(to > Brain.Timer.value() && ((Inertial.heading(degrees) > (deg2 + 4) || Inertial.heading(degrees) < (deg2 - 4)))){

The key change is that the timer condition and the robot rotation conditions are now isolated by a parentheses.

nygil · March 24, 2021, 7:53pm

I was mainly just curios because I currently have both sensors on the robot I was just wondering witch one would be easier/more consistent I took out the wait times in the code and I went to test it but the robot made no attempt to move at all it just stayed stationary.

invalidflaw · March 24, 2021, 8:46pm

So lets follow what the code is doing. Here is your code properly tabbed

void tug(int to,int deg,int spe){
/*
to = timeout 
deg = degres you want to turn + for right - for left
spe = speed
*/


//resets all sensors / integers
  int deg2 = 0;
  Brain.Timer.reset();
  Inertial.calibrate();
  Inertial.setHeading(0, degrees); 

  if(deg < 0){
    //for going left you need to find the distence from 360
    deg2 = deg - 360;

    FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
    BackL.spin(directionType::fwd, spe,velocityUnits::pct);
    FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
    BackR.spin(directionType::fwd, spe,velocityUnits::pct);

    wait(90,msec);

    //loop till the gyro states that is withen the deadzone
    while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 4) || Inertial.heading(degrees) < (deg2 - 4))){

      Controller1.Screen.clearScreen(); 
      Controller1.Screen.print(Inertial.heading(degrees)); 
      wait(60,msec);

      // telling the robot to ether turn left or right to get to where it needs to go 
      if(Inertial.heading(degrees) < (deg2 - 1)){

        //turn right
        FrontL.spin(directionType::rev, spe,velocityUnits::pct);
        BackL.spin(directionType::rev, spe,velocityUnits::pct);
        FrontR.spin(directionType::rev, spe,velocityUnits::pct);
        BackR.spin(directionType::rev, spe,velocityUnits::pct);
  

      }else if(Inertial.heading(degrees) > (deg2 + 1)){

        //turn left    
        FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
        BackL.spin(directionType::fwd, spe,velocityUnits::pct);
        FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
        BackR.spin(directionType::fwd, spe,velocityUnits::pct);
      }
      wait(50,msec);
    }
  }else if(deg > 0){
    while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 2) || Inertial.heading(degrees) < (deg2 - 2))){

      if(Inertial.heading(degrees) < (deg - 1)){

        //turn right
        FrontL.spin(directionType::fwd, spe,velocityUnits::pct);
        BackL.spin(directionType::fwd, spe,velocityUnits::pct);
        FrontR.spin(directionType::fwd, spe,velocityUnits::pct);
        BackR.spin(directionType::fwd, spe,velocityUnits::pct);

      }else if(Inertial.heading(degrees) > (deg + 1)){

        //turn right    
        FrontL.spin(directionType::rev, spe,velocityUnits::pct);
        BackL.spin(directionType::rev, spe,velocityUnits::pct);
        FrontR.spin(directionType::rev, spe,velocityUnits::pct);
        BackR.spin(directionType::rev, spe,velocityUnits::pct);
      } // end if
    } // end while
  } // end if
} // end FUNC

side note, always tab in code for if, else, while, or functions. It makes complex code much more readable.

From your description, the motors did not move. If that is the case, either you set the motor velocity to 0 or you never told the motors to move in the function sequence. I’m guessing you set the speed input to be greater than 0 so you must have a logic case that the code can flow through that never commands motors to spin.

A couple of comments. Don’t calibrate the inertial sensor on every move. Calibration takes several seconds and is only needed on the program start. Second, What are the inputs you are using to test this function? Its not possible to debug without knowing the input commands. I suspect that you are plugging in a positive “deg” angle.

nygil · March 24, 2021, 10:14pm

yes so when I state the function in the auton section I state it as

tug(3,180,30);

so i am giving the motors a speed to follow and i am also giving a positive deg angle for it to follow

invalidflaw · March 24, 2021, 10:22pm

Well I think I see problem number one. With tug(3,180,30); you gave yourself only 3 milliseconds to do this move. That obviously won’t work. I’m guessing you want tug(3000,180,30);

nygil · March 24, 2021, 10:46pm

since this code is written the same as my auton drive code and my ball counter code I know that the brain timer is supposed to count in seconds. or at least it does in the other functions

invalidflaw · March 24, 2021, 10:54pm

are you 100% sure about that? I honestly don’t use vexcode. I use pros for code. From the API reference here, the value is returned in ms without the seconds time unit defined. Does anyone else know what the time() command returns?

nygil · March 24, 2021, 10:56pm

yes here is a piece of my regular auton drive code

void drive( int ti,int dist) {
//RESET
Brain.Timer.reset();
double ipd = 35.2595494613;
int dist2 = dist * ipd;

Enleft.setPosition(0, degrees); 
Enright.setPosition(0, degrees); 

while(ti > Brain.Timer.value() && ((Enleft.position(degrees) + Enright.position(degrees))/2)  < dist2){

int slowdown = (dist2/4) * 3;
int speedup = (dist2/4) / 8 ;

if(((Enleft.position(degrees) + Enright.position(degrees))/2)  < slowdown && ((Enleft.position(degrees) + Enright.position(degrees))/2) > speedup){

  FrontL.spin(directionType::fwd, 57,velocityUnits::pct);
  BackL.spin(directionType::fwd, 57,velocityUnits::pct);
  FrontR.spin(directionType::rev, 57,velocityUnits::pct);
  BackR.spin(directionType::rev, 57,velocityUnits::pct);

}
if(((Enleft.position(degrees) + Enright.position(degrees))/2) > slowdown || ((Enleft.position(degrees) + Enright.position(degrees))/2) < speedup){

  FrontL.spin(directionType::fwd, 30,velocityUnits::pct);
  BackL.spin(directionType::fwd, 30,velocityUnits::pct);
  FrontR.spin(directionType::rev, 30,velocityUnits::pct);
  BackR.spin(directionType::rev, 30,velocityUnits::pct);

}


}

if (Enleft.position(degrees) + Enright.position(degrees)/2 || dist - 20) {

  FrontL.stop();
  FrontR.stop();
  BackL.stop();
  BackR.stop();

}
}

and this code I state in the same manor minus the speed part. I also know that the sensor is good because I used the example vex gave and even though it did overshoot by a considerable amount it did stop

invalidflaw · March 24, 2021, 11:04pm

Never mind (facepalms). Who made the vexcode API? I was looking at the wrong command. time() and value() are differennt, who knew!?!!

With that out of the way, the next issue is to look at why the motors aren’t turning. In that case the command is flowing in such a way that now motor.spin commands are being executed. Early in the code we hit
if(deg < 0){
We are positive, move on,

}else if(deg > 0){
Thats true, so this while loop is next
while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 2) || Inertial.heading(degrees)
Since the motors are not moving, something in this while loop condition is false. Either the brain timer is past “to” or the intertial heading comparison is not right. Before the first if/else statement, print the brain timer value and the inertial heading. That may help diagnose what is happening.

nygil · March 24, 2021, 11:17pm

else if(deg > 0){
    while(to > Brain.Timer.value() && (Inertial.heading(degrees) > (deg2 + 2) || Inertial.heading(degrees) < (deg2 - 2)))

ok so from what I can grasp the ‘to’ is the amount of time I allow it to complete the task and I printed the timer and inertial heading and nothing showed up on my brain screen

squiddy_slapper · March 24, 2021, 11:45pm

I use this and it works with 100% accuracy. It’s super simple, yet effective. I would highly suggest this.

FL.spin(fwd, 20, pct);
BL.spin(fwd, 20, pct);
FR.spin(reverse, 20, pct);
BR.spin(reverse, 20, pct);

waitUntil(inertial.heading(degrees)>40 and inertial.heading(degrees) < 43);

FL.stop();
BL.stop();
FR.stop();
BR.stop();

nygil · March 24, 2021, 11:52pm

@squiddy_slapper
so just so I do understand this I would just state that in auton? Also where would I calibrate the sensor. i copied that into the auton and there is a weird error so I had to change the wait until to

waitUntil(true);(Inertial.heading(degrees) >44 &&  Inertial.heading(degrees) < 46);

and no mater what I seem to change the and to it does not want to accept it

invalidflaw · March 25, 2021, 12:24am

Always calibrate the sensor in the initialize function of vexcode. You don’t want to calibrate more than once. calibrating more than once will simply confuse the sensor

nygil · March 25, 2021, 12:27am

so I have the sensor calibration in the pre auton and then I have @squiddy_slapper example code in the auton but the robot still does not move

invalidflaw · March 25, 2021, 12:29am

So stupid question. How are you triggering your auton routine? I can think of no reason why the auton shouldn’t start.

nygil · March 25, 2021, 12:31am

I am using a competition switch to toggle my auton on and off

waitUntil(true);(Inertial.heading(degrees) >44 || Inertial.heading(degrees) < 46);

this is the code except it does not like the || it says expression result unused

invalidflaw · March 25, 2021, 12:42am

Yep,

waitUntil(true);(Inertial.heading(degrees) >44 || Inertial.heading(degrees) < 46);

will exit immediately. the (true) before the conditional dominates the other one. Try,

waitUntil(Inertial.heading(degrees) >44 || Inertial.heading(degrees) < 46);