Code issues with our robot

Hi there, my 27301 A team is having issues with their robot spinning all over the place, not sure if its the inertial sensor acting up, but it drives forward no issues, but turning it just starts spinning. Could someone just see if I missed anything? I cant tell what else is wrong:

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       VEX                                                       */
/*    Created:      Thu Sep 26 2019                                           */
/*    Description:  Competition Template                                      */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// leftFront            motor         8               
// leftBack             motor         9               
// rightFront           motor         2               
// rightBack            motor         1               
// Inertial16           inertial      16              
// Controller1          controller                    
// armLeft              motor         7               
// armRight             motor         4               
// Piston_1             digital_out   A               
// Drivetrain           drivetrain    3, 5            
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"

using namespace vex;

// A global instance of competition
competition Competition;

motor_group LeftSide(leftFront, leftBack);
motor_group RightSide(rightFront,rightBack);
motor_group ArmsTog(armLeft, armRight);
smartdrive robotDrive(LeftSide, RightSide, Inertial16, 12.56, 16, 16, distanceUnits::in);
int armsSpeed = 90;  //Arms speed 
// define your global instances of motors and other devices here

// define your global instances of motors and other devices here

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the V5 has been powered on and        */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton(void) {
  Inertial16.calibrate();
  robotDrive.setDriveVelocity(100,pct);
  robotDrive.setTurnVelocity(100,pct);
  robotDrive.setStopping(coast);
  robotDrive.setTurnConstant(1.0);
  robotDrive.setTurnThreshold(1.0);
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {
  robotDrive.setDriveVelocity(55,percent);
  robotDrive.driveFor(reverse,64,inches);
  Piston_1.set(true);
  wait(.5, seconds);
  robotDrive.driveFor(fwd,60,inches);
  robotDrive.turnFor(right, 90, degrees);
  robotDrive.stop(); 

  // ..........................................................................
  // Insert autonomous user code here.
  // ..........................................................................
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void usercontrol(void) {
  robotDrive.setDriveVelocity(100,percent);
  // User control code here, inside the loop
  while (1) {

// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.
LeftSide.spin(vex::directionType::fwd, (- Controller1.Axis1.value()) - Controller1.Axis3.value(), vex::velocityUnits::pct);
RightSide.spin(vex::directionType::fwd, (Controller1.Axis1.value()) - Controller1.Axis3.value(), vex::velocityUnits::pct);
// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................
if(Controller1.ButtonR2.pressing()){
ArmsTog.spin(fwd, armsSpeed, vex::velocityUnits::pct);
  }
  else if (Controller1.ButtonR1.pressing()){

  ArmsTog.spin(vex::directionType::rev, armsSpeed, vex::velocityUnits::pct);
  }
else {
  ArmsTog.stop(hold);
}
if (Controller1.ButtonL1.pressing()){

  Piston_1.set(true);
}
else {
  Piston_1.set(false);
wait(20, msec); // Sleep the task for a short amount of time to
                // prevent wasted resources.
  }
}
}

//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
wait(100, msec);
  }
}

edit: mods fixed the code tags

The calibration of the inertial sensor takes about 2 seconds. If you begin moving before it calibrates then it will not function properly. It’s hard to tell if you are manually accounting for this delay or not, but if not then that may be your problem.

2 Likes

Should we be putting a wait command before everything? This is our 15 sec auto and 1 min skills run.

What wheels are you using?

4” Omni wheels.

They moved the inertial sensor to the bottom of the robot hoping that would fix it

As @Grillied stated, the inertial takes time to calibrate. Normally this would be done before auton (we would put a very complex and unnecessary code under driver, and we would run it before plugging the controller in, but I believe this can also be done in initialization, someone fact check that). That is probably your problem if the robot is spinning during auton.

I agree with what @Grillied and @soar said. Try what they said and see if it fixed.

Thanks everyone, kinda came down to we think something is wrong with the inertial sensor, need to try and replace, I hope have one still in the bag

It might be a good idea to have the team perform diagnostics on the sensor.

Use this link: https://kb.vex.com/hc/en-us/articles/360037382272-Using-the-V5-Inertial-Sensor
See the section called Reading Inertial Sensor Values but the whole article is helpful.

You can also add programming to display the values of the sensor during the programmed run. This may provide insight as to what the sensor is reading.

2 Likes