here is the drive code
#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.
controller Controller1 = controller(primary);
controller Controller2 = controller(partner);
motor leftMotorA = motor(PORT13, ratio6_1, true);
motor leftMotorB = motor(PORT12, ratio6_1, true);
motor leftMotorC = motor(PORT11, ratio6_1, false);
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB, leftMotorC);
motor rightMotorA = motor(PORT18, ratio6_1, false);
motor rightMotorB = motor(PORT17, ratio6_1, false);
motor rightMotorC = motor(PORT16, ratio6_1, true);
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB, rightMotorC);
inertial DrivetrainInertial = inertial(PORT14);
smartdrive Drivetrain = smartdrive(LeftDriveSmart, RightDriveSmart, DrivetrainInertial, 299.24, 320, 40, mm, 0.6);
motor intakeMotorA = motor(PORT6, ratio18_1, true);
motor intakeMotorB = motor(PORT5, ratio18_1, true);
motor_group intake = motor_group(intakeMotorA, intakeMotorB);
motor winch = motor(PORT20, ratio36_1, false);
digital_out mogo = digital_out(Brain.ThreeWirePort.A);
digital_out wall_n_climb = digital_out(Brain.ThreeWirePort.H);
optical Optical10 = optical(PORT10);
// generating and setting random seed
void initializeRandomSeed(){
int systemTime = Brain.Timer.systemHighResolution();
double batteryCurrent = Brain.Battery.current();
double batteryVoltage = Brain.Battery.voltage(voltageUnits::mV);
// Combine these values into a single integer
int seed = int(batteryVoltage + batteryCurrent * 100) + systemTime;
// Set the seed
srand(seed);
}
bool vexcode_initial_drivetrain_calibration_completed = false;
void calibrateDrivetrain() {
wait(200, msec);
Brain.Screen.print("Calibrating");
Brain.Screen.newLine();
Brain.Screen.print("Inertial");
DrivetrainInertial.calibrate();
while (DrivetrainInertial.isCalibrating()) {
wait(25, msec);
}
vexcode_initial_drivetrain_calibration_completed = true;
// Clears the screen and returns the cursor to row 1, column 1.
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
}
void vexcodeInit() {
// Calibrate the Drivetrain
calibrateDrivetrain();
//Initializing random seed.
initializeRandomSeed();
}
// 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);
}
// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
// define variables used for controlling motors based on controller inputs
bool Controller1RightShoulderControlMotorsStopped = true;
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_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
while(true) {
if(RemoteControlCodeEnabled) {
// stop the motors if the brain is calibrating
if (DrivetrainInertial.isCalibrating()) {
LeftDriveSmart.stop();
RightDriveSmart.stop();
while (DrivetrainInertial.isCalibrating()) {
wait(25, 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 value is inside of the deadband range
if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
// check if the left motor has already been stopped
if (DrivetrainLNeedsToBeStopped_Controller1) {
// stop the left drive motor
LeftDriveSmart.stop();
// tell the code that the left motor has been stopped
DrivetrainLNeedsToBeStopped_Controller1 = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
DrivetrainLNeedsToBeStopped_Controller1 = true;
}
// check if the value is inside of the deadband range
if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
// check if the right motor has already been stopped
if (DrivetrainRNeedsToBeStopped_Controller1) {
// stop the right drive motor
RightDriveSmart.stop();
// tell the code that the right motor has been stopped
DrivetrainRNeedsToBeStopped_Controller1 = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
DrivetrainRNeedsToBeStopped_Controller1 = true;
}
// only tell the left drive motor to spin if the values are not in the deadband range
if (DrivetrainLNeedsToBeStopped_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 (DrivetrainRNeedsToBeStopped_Controller1) {
RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
RightDriveSmart.spin(forward);
}
// check the ButtonR1/ButtonR2 status to control intake
if (Controller1.ButtonR1.pressing()) {
intake.spin(forward);
Controller1RightShoulderControlMotorsStopped = false;
} else if (Controller1.ButtonR2.pressing()) {
intake.spin(reverse);
Controller1RightShoulderControlMotorsStopped = false;
} else if (!Controller1RightShoulderControlMotorsStopped) {
intake.stop();
// set the toggle so that we don't constantly tell the motor to stop when the buttons are released
Controller1RightShoulderControlMotorsStopped = true;
}
}
// wait before repeating the process
wait(20, msec);
}
return 0;
}
// define variables used for controlling motors based on controller inputs
bool Controller2LeftShoulderControlMotorsStopped = true;
bool Controller2RightShoulderControlMotorsStopped = true;
bool DrivetrainNeedsToBeStopped_Controller2 = true;
// define a task that will handle monitoring inputs from Controller2
int rc_auto_loop_function_Controller2() {
// process the controller input every 20 milliseconds
// update the motors based on the input values
while(true) {
if(RemoteControlCodeEnabled) {
// stop the motors if the brain is calibrating
if (DrivetrainInertial.isCalibrating()) {
LeftDriveSmart.stop();
RightDriveSmart.stop();
while (DrivetrainInertial.isCalibrating()) {
wait(25, msec);
}
}
// calculate the drivetrain motor velocities from the controller joystick axies
// left = Axis3 + Axis4
// right = Axis3 - Axis4
int drivetrainLeftSideSpeed = Controller2.Axis3.position() + Controller2.Axis4.position();
int drivetrainRightSideSpeed = Controller2.Axis3.position() - Controller2.Axis4.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_Controller2) {
// stop the drive motors
LeftDriveSmart.stop();
RightDriveSmart.stop();
// tell the code that the motors have been stopped
DrivetrainNeedsToBeStopped_Controller2 = 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_Controller2 = true;
}
// only tell the left drive motor to spin if the values are not in the deadband range
if (DrivetrainNeedsToBeStopped_Controller2) {
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_Controller2) {
RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
RightDriveSmart.spin(forward);
}
// check the ButtonL1/ButtonL2 status to control winch
if (Controller2.ButtonL1.pressing()) {
winch.spin(forward);
Controller2LeftShoulderControlMotorsStopped = false;
} else if (Controller2.ButtonL2.pressing()) {
winch.spin(reverse);
Controller2LeftShoulderControlMotorsStopped = false;
} else if (!Controller2LeftShoulderControlMotorsStopped) {
winch.stop();
// set the toggle so that we don't constantly tell the motor to stop when the buttons are released
Controller2LeftShoulderControlMotorsStopped = true;
}
// check the ButtonR1/ButtonR2 status to control intake
if (Controller2.ButtonR1.pressing()) {
intake.spin(forward);
Controller2RightShoulderControlMotorsStopped = false;
} else if (Controller2.ButtonR2.pressing()) {
intake.spin(reverse);
Controller2RightShoulderControlMotorsStopped = false;
} else if (!Controller2RightShoulderControlMotorsStopped) {
intake.stop();
// set the toggle so that we don't constantly tell the motor to stop when the buttons are released
Controller2RightShoulderControlMotorsStopped = true;
}
}
// wait before repeating the process
wait(20, msec);
}
return 0;
}
task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);
task rc_auto_loop_task_Controller2(rc_auto_loop_function_Controller2);
#pragma endregion VEXcode Generated Robot Configuration
competition Competition;