Inertial Not Working! (Oscillating)

[yusuftalhablkbs]

I have changed everything, the sensor itself, cabling, brain, all measurements, gear ratios, the whole code, position of the sensor, connecting it with rubber links and more. It just keeps Oscillating and doesn’t turn to any degree. It does go straight that works. I checked the heading degrees they’re fine. I don’t know what to do at this point. I’m trying to learn PID hoping that will solve the problem but it’s really complicated.

Behavior;

If I don’t set any speed, It goes pretty slow and doesn’t reach any desired distance or turn.

When I set turn velocity it starts oscillating and doesn’t stop.

Here’s my code if anybody need’s it;

#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.




// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
  printf("VEXPlaySound:%s\n", soundName);
  wait(5, msec);
}

#pragma endregion VEXcode Generated Robot Configuration



// Brain should be defined by default


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.
controller Controller1 = controller(primary);
motor LfrontM = motor(PORT17, ratio6_1, true);
motor LmiddleM = motor(PORT15, ratio6_1, true);
motor LbackM = motor(PORT13, ratio6_1, true);
motor_group LeftDriveSmart = motor_group(LfrontM, LmiddleM, LbackM);
motor RfrontM = motor(PORT16, ratio6_1, false);
motor RmiddleM = motor(PORT14, ratio6_1, false);
motor RbackM = motor(PORT12, ratio6_1, false);
motor_group RightDriveSmart = motor_group(RfrontM, RmiddleM, RbackM);

motor intakeM = motor(PORT10,ratio18_1, true);
motor launcherM = motor(PORT18,ratio6_1, true);

digital_out wingFront = digital_out(Brain.ThreeWirePort.B);
digital_out wingBack = digital_out(Brain.ThreeWirePort.C);
digital_out lift = digital_out(Brain.ThreeWirePort.A);

inertial DrivetrainInertial = inertial(PORT9);
smartdrive Drivetrain = smartdrive(LeftDriveSmart, RightDriveSmart, DrivetrainInertial, 11.78, 10.75, 12.28, inches,.53);

bool FrontWing = false;
bool BackWing = false;

void calibrateDrivetrain() {
  Brain.Screen.print("Calibrating");
  Brain.Screen.newLine();
  Brain.Screen.print("Inertial");
  DrivetrainInertial.calibrate();
  while (DrivetrainInertial.isCalibrating()) {
    wait(100,msec);
  }
  // Clears the screen and returns the cursor to row 1, column 1.
  Brain.Screen.clearScreen();
  Brain.Screen.setCursor(1, 1);
}

// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.



// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
// define variables used for controlling motors based on controller inputs
bool DrivetrainNeedsToBeStopped_Controller1 = true;

// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
  // process the controller input every 20 milliseconds
  // update the motors based on the input values
  calibrateDrivetrain();
  while(true) {
    if(RemoteControlCodeEnabled) {
      // stop the motors if the brain is calibrating
      if (DrivetrainInertial.isCalibrating()) {
        LeftDriveSmart.stop();
        RightDriveSmart.stop();
        while (DrivetrainInertial.isCalibrating()) {
          wait(100, msec);
        }
      }
      
      // calculate the drivetrain motor velocities from the controller joystick axies
      // left = Axis3 + Axis1
      // right = Axis3 - Axis1
      int drivetrainLeftSideSpeed  = Controller1.Axis3.position() + Controller1.Axis1.position();
      int drivetrainRightSideSpeed = Controller1.Axis3.position() - Controller1.Axis1.position();
      
      // check if the values are inside of the deadband range
      if (abs(drivetrainLeftSideSpeed) < 5 && abs(drivetrainRightSideSpeed) < 5) {
        // check if the motors have already been stopped
        if (DrivetrainNeedsToBeStopped_Controller1) {
          // stop the drive motors
          LeftDriveSmart.stop();
          RightDriveSmart.stop();
          // tell the code that the motors have been stopped
          DrivetrainNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the motors next time the input is in the deadband range
        DrivetrainNeedsToBeStopped_Controller1 = true;
      }
      
      // only tell the left drive motor to spin if the values are not in the deadband range
      if (DrivetrainNeedsToBeStopped_Controller1) {
        LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
        LeftDriveSmart.spin(forward);
      }
      // only tell the right drive motor to spin if the values are not in the deadband range
      if (DrivetrainNeedsToBeStopped_Controller1) {
        RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
        RightDriveSmart.spin(forward);
      }
    }
    // wait before repeating the process
    wait(20, msec);
  }
  return 0;
}

task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);

// Include the V5 Library
#include "vex.h"
  
// Allows for easier use of the VEX Library
using namespace vex;

competition Competition;

float myVariable;

// "when started" hat block
int whenStarted1() {
  return 0;
}


void intakeOUT(){
  intakeM.spin(forward,100,percent);
}

void intakeIN(){
  intakeM.spin(reverse,100,percent);
}

void intakeOFF(){
  intakeM.stop();
}

void liftUp(){
    lift.set(true);
}

void liftDown(){
    lift.set(false);
}

void ShooterSpin(){
  launcherM.setVelocity(100,percent);
  launcherM.spin(forward);
}

void ShooterStop(){
  launcherM.stop();
}

void FrontWingIO(){
  if(FrontWing == false){
    FrontWing = true;
    wingFront.set(true);
    Brain.Screen.print("FrontWing: ");
    Brain.Screen.print(FrontWing);
  }
  else if(FrontWing == true){
    FrontWing = false;
    wingFront.set(false);
    Brain.Screen.print("FrontWing: ");
    Brain.Screen.print(FrontWing);
    
  }
}

void BackWingIO(){
  if(BackWing == false){
    BackWing = true;
    wingBack.set(true);
    Brain.Screen.print("BackWing: ");
    Brain.Screen.print(BackWing);
  }
  else if(BackWing == true){
    BackWing = false;
    wingBack.set(false);
    Brain.Screen.print("BackWing: ");
    Brain.Screen.print(BackWing);
    
  }
}
void KickAndTurn(){
  wingFront.set(true);
  Drivetrain.turnFor(right, 13, degrees);
  wingFront.set(false);
}
void GetMiddle(){
  intakeM.spin(reverse);
  Drivetrain.driveFor(forward, 64, inches);
  wait(1.2, seconds);
  intakeM.stop();
  Drivetrain.driveFor(reverse, 60, inches);
}
void PushColor(){
  wingBack.set(true);
  Drivetrain.turnFor(right, 140, degrees);
  Drivetrain.driveFor(reverse, 26, inches);
}
void GetCorner(){
  Drivetrain.driveFor(forward, 22, inches);
  Drivetrain.turnFor(left, 60, degrees);
  wait(0.2, seconds);
  intakeM.spin(reverse);
  wingBack.set(false);
  Drivetrain.driveFor(forward, 45, inches);
  wait(0.1, seconds);
  Drivetrain.driveFor(reverse, 5, inches); 
}
// "when autonomous" hat block
int onauton_autonomous_0() {
  Drivetrain.setDriveVelocity(50, percent);
  Drivetrain.setTurnVelocity(10, percent);

  Drivetrain.turnFor(right, 30, degrees);
  
  
  return 0;
}


// "when driver control" hat block
int ondriver_drivercontrol_0() {
  while (true) {
  wait(5, msec);

 
  Controller1.ButtonL2.pressed(BackWingIO);
  Controller1.ButtonL1.pressed(FrontWingIO);

  Controller1.ButtonR1.pressed(intakeIN);
  Controller1.ButtonR2.pressed(intakeOUT);
  Controller1.ButtonR1.released(intakeOFF);
  Controller1.ButtonR2.released(intakeOFF);
  

  Controller1.ButtonUp.pressed(liftUp);
  Controller1.ButtonDown.pressed(liftDown);

  Controller1.ButtonRight.pressed(ShooterSpin);
  Controller1.ButtonLeft.pressed(ShooterStop);

  
  return 0;
}
}

void VEXcode_driver_task() {
  // Start the driver control tasks....
  Drivetrain.setDriveVelocity(100,percent);
  vex::task drive0(ondriver_drivercontrol_0);
  while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  drive0.stop();
  return;

}

void VEXcode_auton_task() {
  // Start the auton control tasks....
  vex::task auto0(onauton_autonomous_0);
  while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}

  auto0.stop();
  return;
}



int main() {
  vex::competition::bStopTasksBetweenModes = false;
  Competition.autonomous(VEXcode_auton_task);
  Competition.drivercontrol(VEXcode_driver_task);

  whenStarted1();
}

edit: code tags added by mods, please remember to use them.

[isaac7862]

Oscillation happens when your robot is stuck in a loop of overcorrecting itself when turning. Your code looks good, with the calibration of the drivetrain and the inertial sensor. Everything else looks solid. The best way to fix this would probably be to implement a PID controller into your code. I am also in the process of learning PID, and I have found one very helpful article by George Gillard titled “An Introduction to PID Controllers”. I hope this is helpful, and if I missed something, feel free to correct me or add it to this thread. Here is the paper, attached below.
introduction_to_pid_controllers_ed2.pdf (400.2 KB)

[isaac7862]

Additionally, if there is somebody you know that has used PID’s before, or even just P&D, I’m sure they would be glad to sit down and explain it. I have also done this in the past month, and found it helpful and informative.

[yusuftalhablkbs]

I’ll try to reach out a neighbor school. Thanks for replying.

[yusuftalhablkbs]

I understand that the overcorrecting causes the oscillation. But I don’t understand why it overcorrects in the first place. And what it has to do with Drivetrain.setTurnVelocity code.

[dfergemann]

This can happen with heavy competition robots with fast drivetrains. The default Drivetrain code uses proportional-only turning control that isn’t tuned for the robots like this. You may be able to get slightly better results by adjusting the turn constant:

Drivetrain.setTurnConstant(kP)

Try different values for kP, which is 1 by default. The “Proportional” section of the PID paper other people posted here may help you understand what that number is doing and whether you should increase or decrease it, but it’s mostly trial and error. There’s no guarantee that this will solve the problem, and if it doesn’t, you’ll need to write some PID code.

[5024C]

We had the same problem, but we found that if you go to devices after you have started the code click the inertial sensor and set it to raw and then calibrate the seemed to fix a lot of the problems.

[spaghetti_code]

Looks like you’re using the SmartMotor Drive functionality to do your turns. A quick search for James Pearman’s explanation about using the smart motor drive for turns you’ll see that it uses a simple P loop, which if you set the turns to a higher velocity the robot will oscillate since it tends to overshoot.

V5 Inertial Sensor Drifting

Is this using drivetrain class ? Are you using turnTo or turnFor blocks ? turnFor will allow any error to accumulate. There’s no self correction as such in the code used in the drivetrain class, it uses a simple P loop that terminates when the turn is completed within 2 degrees.

Drive train PID's resources help

This is a great resource for a PID. If you have any questions about the tutorial a forum thread can be found here.

You’ll find posts where others are having the exact same challenge, You may want to write your own PID or a simple bang-bang function to solve the problem.

[yusuftalhablkbs]

Thank you so much! I didn’t know that it was possible to change that constant. First thing tomorrow morning I’ll try that.