We are trying to do a simple auton like go forward 10 inches. but the robot doesn’t stop moving for a long time. It all the way across the field and then still goes on for a few seconds If we put in 10 mm, then it stops at about 7 inches. Can anyone help. Here is the code to setup the smartdrive and our code for auton.
CODE ] Drivetrain.driveFor(10, distanceUnits::in); /CODE ]
CODE ] #include “vex.h”
#include “math.h”
using namespace vex;
// A global instance of brain used for printing to the V5 brain screen
brain Brain;
//define left motors
motor leftMotorA = motor(PORT7, ratio6_1, true);
motor leftMotorB = motor(PORT6, ratio6_1, false);
motor leftMotorC = motor(PORT5, ratio6_1, true);
//make motors combined into left side
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB, leftMotorC);
//define right motors
motor rightMotorA = motor(PORT9, ratio6_1, false);
motor rightMotorB = motor(PORT8, ratio6_1, true);
motor rightMotorC = motor(PORT10, ratio6_1, false);
//define intake motors
//motor intake1 = motor(PORT8, ratio6_1, true);
motor intake = motor(PORT3, ratio6_1, true);
//define lift motor
motor lift = motor(PORT4, ratio18_1, true);
//make motors combined into right side
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB, rightMotorC);
//define the inertial sensor
inertial DrivetrainInertial = inertial(PORT2);
//define the variables used in the drivetrain
distanceUnits units = distanceUnits::in; //Imperial measurements - inches.
double gearRatio = 36.0 / 48.0;
double wheelTravel = 3.25 * M_PI; //wheel diameter times PI
double trackWidth = 12;
//define mogo goal mechanism
digital_out mogoGoal = digital_out(Brain.ThreeWirePort.B);
digital_out doinker = digital_out(Brain.ThreeWirePort.A);
//define the rotation sensor
rotation rotationSensor = rotation(PORT11, false);
//create smartdrive by combining the two sides and measurements of the robot
smartdrive Drivetrain = smartdrive(LeftDriveSmart, RightDriveSmart, DrivetrainInertial, wheelTravel,trackWidth, gearRatio);
//define the controllerpOL|
controller Controller1 = controller(primary);
void vexcodeInit(void) {
Brain.Screen.print(“Device initialization…”);
Brain.Screen.setCursor(2, 1);
// Calibrate the drivetrain gyro
wait(200, msec);
DrivetrainInertial.startCalibration(1);
Brain.Screen.print(“Calibrating Gyro for Drivetrain”);
// Wait for the gyro calibration process to finish
while (DrivetrainInertial.isCalibrating()) {
Brain.Screen.setCursor(3, 1);
Brain.Screen.print(“Calibrating…”);
wait(100, msec); // Check periodically
}
// Display inertial sensor readings after calibration
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print(“Gyro Calibration Complete!”);
wait(1, sec); // Give time to observe this message
// Display inertial sensor readings
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print(“Heading: %f”, DrivetrainInertial.heading());
Brain.Screen.setCursor(2, 1);
Brain.Screen.print(“Rotation: %f”, DrivetrainInertial.rotation());
Brain.Screen.setCursor(3, 1);
Brain.Screen.print(“Pitch: %f”, DrivetrainInertial.pitch());
Brain.Screen.setCursor(4, 1);
Brain.Screen.print(“Roll: %f”, DrivetrainInertial.roll());
wait(2, seconds); // Wait to observe the inertial sensor values
// Reset motor encoders
leftMotorA.resetPosition();
leftMotorB.resetPosition();
leftMotorC.resetPosition();
rightMotorA.resetPosition();
rightMotorB.resetPosition();
rightMotorC.resetPosition();
// Display motor encoder values
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print(“Left A: %f”, leftMotorA.position(rotationUnits::deg));
Brain.Screen.setCursor(2, 1);
Brain.Screen.print(“Left B: %f”, leftMotorB.position(rotationUnits::deg));
Brain.Screen.setCursor(3, 1);
Brain.Screen.print(“Left C: %f”, leftMotorC.position(rotationUnits::deg));
Brain.Screen.setCursor(4, 1);
Brain.Screen.print(“Right A: %f”, rightMotorA.position(rotationUnits::deg));
Brain.Screen.setCursor(5, 1);
Brain.Screen.print(“Right B: %f”, rightMotorB.position(rotationUnits::deg));
Brain.Screen.setCursor(6, 1);
Brain.Screen.print(“Right C: %f”, rightMotorC.position(rotationUnits::deg));
wait(2, seconds); // Wait to observe motor encoder values
Brain.Screen.clearScreen();
// Reset the rotation sensor
rotationSensor.resetPosition(); /CODE ]