I spent quite a while trying to implement your tips into my code. Here is my full code. If there are any obvious issues please let me know. I appreciate all your help so far. Full Code Upload (indentation is messed up by copy paste):
//Vex Start
#include "vex.h"
#include "cmath"
using namespace vex;
//Define Devices
competition Competition;
controller MainController = controller();
controller SecondaryController = controller();
motor DriveRFront = motor(PORT8, ratio18_1, true);
motor DriveRBack = motor(PORT7, ratio18_1, true);
motor DriveLFront = motor(PORT9, ratio18_1, false);
motor DriveLBack = motor(PORT6, ratio18_1, false);
motor MainLift = motor(PORT18, ratio18_1, false);
motor GoalLift = motor(PORT14, ratio36_1, true);
motor ConveyerMotor = motor(PORT17, ratio18_1, false);
motor ClampMotor = motor(PORT15, ratio36_1, true);
//Pre Auton Code (Configuration)
void pre_auton(void) {
vexcodeInit();
//General Motor Config
//Main Lift Config
MainLift.setMaxTorque (100, percent);
MainLift.setVelocity(100, percent);
MainLift.setStopping (hold);
//Goal Lift Config
GoalLift.setMaxTorque (100, percent);
GoalLift.setVelocity (100, percent);
GoalLift.setStopping (brake);
//Conveyer Config (ADJUST VELOCITY)
ConveyerMotor.setMaxTorque (100, percent);
ConveyerMotor.setVelocity (90, percent);
ConveyerMotor.setStopping (brake);
//Goal Clamp Config
ClampMotor.setMaxTorque (100, percent);
ClampMotor.setVelocity (100, percent);
ClampMotor.setStopping (hold);
//Drive Base Config
//Torque Config
DriveRFront.setMaxTorque (100, percent);
DriveRBack.setMaxTorque (100, percent);
DriveLFront.setMaxTorque (100, percent);
DriveLBack.setMaxTorque (100, percent);
//Velocity Config
DriveRFront.setVelocity (100, percent);
DriveRBack.setVelocity (100, percent);
DriveLFront.setVelocity (100, percent);
DriveLBack.setVelocity (100, percent);
//Stopping Mode Config
DriveRFront.setStopping (brake);
DriveRBack.setStopping (brake);
DriveLFront.setStopping (brake);
DriveLBack.setStopping (brake);
}
//Autonomous Function Define
//Drivebase Forward
void drivefwd (int autonspeed, int duration) {
DriveRFront.spin(directionType::fwd, autonspeed, velocityUnits::pct);
DriveRBack.spin(directionType::fwd, autonspeed, velocityUnits::pct);
DriveLFront.spin(directionType::fwd, autonspeed, velocityUnits::pct);
DriveLBack.spin(directionType::fwd, autonspeed, velocityUnits::pct);
task::sleep(duration);
DriveRFront.stop();
DriveRBack.stop();
DriveLFront.stop();
DriveLBack.stop();
}
//Drivebase Reverse
void driverev (int autonspeed, int duration) {
DriveRFront.spin(directionType::rev, autonspeed, velocityUnits::pct);
DriveRBack.spin(directionType::rev, autonspeed, velocityUnits::pct);
DriveLFront.spin(directionType::rev, autonspeed, velocityUnits::pct);
DriveLBack.spin(directionType::rev, autonspeed, velocityUnits::pct);
task::sleep(duration);
DriveRFront.stop();
DriveRBack.stop();
DriveLFront.stop();
DriveLBack.stop();
}
const float TurnMultiplier = 0.35;
//Drivebase Turn Left
void driveleft (int Velocity, int turnangle) {
DriveRFront.resetPosition();
while (std::abs (DriveRFront.rotation(rotationUnits::deg) * TurnMultiplier) < turnangle) {
DriveRFront.spin(directionType::fwd, Velocity, velocityUnits::pct);
DriveRBack.spin(directionType::fwd, Velocity, velocityUnits::pct);
DriveLFront.spin(directionType::fwd, -Velocity, velocityUnits::pct);
DriveLBack.spin(directionType::fwd, -Velocity, velocityUnits::pct);
}
DriveRFront.stop();
DriveRBack.stop();
DriveLFront.stop();
DriveLBack.stop();
}
//Drivebase Turn Right
void driveright (int Velocity, int turnangle) {
DriveRFront.resetPosition();
while (std::abs (DriveRFront.rotation(rotationUnits::deg) * TurnMultiplier) < turnangle) {
DriveRFront.spin(directionType::fwd, -Velocity, velocityUnits::pct);
DriveRBack.spin(directionType::fwd, -Velocity, velocityUnits::pct);
DriveLFront.spin(directionType::fwd, Velocity, velocityUnits::pct);
DriveLBack.spin(directionType::fwd, Velocity, velocityUnits::pct);
}
DriveRFront.stop();
DriveRBack.stop();
DriveLFront.stop();
DriveLBack.stop();
}
//Drivebase Stop
void drivestop () {
DriveRFront.stop ();
DriveRBack.stop ();
DriveLFront.stop ();
DriveLBack.stop ();
}
//Drivebase Set Stop Hold
void drivehold () {
DriveRFront.setStopping(hold);
DriveRBack.setStopping(hold);
DriveLFront.setStopping(hold);
DriveLBack.setStopping(hold);
}
//Drivebase Set Stop Brake
void drivebrake () {
DriveRFront.setStopping(brake);
DriveRBack.setStopping(brake);
DriveLBack.setStopping(brake);
DriveLFront.setStopping(brake);
}
//Drivebase Set Stop Brake
void drivecoastduration (int duration) {
DriveRFront.stop();
DriveRBack.stop();
DriveLFront.stop();
DriveLBack.stop();
DriveRFront.setStopping(coast);
DriveRBack.setStopping(coast);
DriveLFront.setStopping(coast);
DriveLBack.setStopping(coast);
task::sleep(duration);
DriveRFront.stop();
DriveRBack.stop();
DriveLFront.stop();
DriveLBack.stop();
}
void drivecoast () {
DriveRFront.stop();
DriveRBack.stop();
DriveLFront.stop();
DriveLBack.stop();
DriveRFront.setStopping(coast);
DriveRBack.setStopping(coast);
DriveLFront.setStopping(coast);
DriveLBack.setStopping(coast);
}
//Main Lift Up
void mainliftup (int autonspeed, int duration) {
MainLift.spin(directionType::fwd, autonspeed, velocityUnits::pct);
task::sleep(duration);
MainLift.stop();
}
//Main Lift Down
void mainliftdown (int autonspeed, int duration) {
MainLift.spin(directionType::rev, autonspeed, velocityUnits::pct);
task::sleep(duration);
MainLift.stop();
}
//Main Lift Stop
void mainliftstop () {
MainLift.stop();
}
//Min Lift Set Stop Hold
void mainlifthold () {
MainLift.setStopping(hold);
}
//Main Lift Set Stop Brake
void mainliftbrake () {
MainLift.setStopping(brake);
}
//Main Lift Set Stop Coast
void mainliftcoast () {
MainLift.setStopping(coast);
}
void clampdown () {
ClampMotor.spin(directionType::fwd);
}
void clampup () {
ClampMotor.spinTo (double (100), deg);
}
//Goal Lift Up
void goalliftup (int autonspeed, int duration) {
GoalLift.spin(directionType::fwd, autonspeed, velocityUnits::pct);
task::sleep(duration);
GoalLift.stop();
}
//Goal Lift Down
void goalliftdown (int autonspeed, int duration) {
GoalLift.spin(directionType::rev, autonspeed, velocityUnits::pct);
task::sleep(duration);
GoalLift.stop();
}
//Goal Lift Stop
void goalliftstop () {
GoalLift.stop();
}
//Goal Lift Set Stop Hold
void goallifthold () {
GoalLift.setStopping(hold);
}
//Goal Lift Set Stop Brake
void goalliftbrake () {
GoalLift.setStopping(brake);
}
//Goal Lift Set Stop Coast
void goalliftcoast () {
GoalLift.setStopping(coast);
}
//Spins Conveyer Belt Forwards
void conveyerfwd () {
ConveyerMotor.spin(directionType::fwd);
}
//Spins Conveyer Belt Backwards
void conveyerrev () {
ConveyerMotor.spin(directionType::rev);
}
//Auton Code
void autonomous(void) {
drivebrake();
goallifthold();
mainlifthold();
goalliftdown (100,2000);
drivefwd (30, 700);
goalliftup (100, 2500);
driverev (30, 700);
driveleft (50, 90);
mainliftup (100, 500);
drivefwd (60, 700);
driveleft (50, 90);
drivefwd (30, 2000);
goalliftdown (100, 2000);
}
int toggleConveyer () {
bool ConveyerState = false;
bool ConveyerLast = false;
while (true) {
if (MainController.ButtonDown.pressing () == true && !ConveyerLast) {
ConveyerState = !ConveyerState;
ConveyerLast = true;
}
else if (!MainController.ButtonDown.pressing () == true) {
ConveyerLast = false;
}
if (ConveyerState) {
ConveyerMotor.spin (directionType::fwd, 100, velocityUnits::pct);
}
else {
ConveyerMotor.stop ();
}
task::sleep (50);
}
return 1;
}
int toggleClamp () {
bool GoalClampState = false;
bool GoalClampLast = false;
while (true) {
if (MainController.ButtonB.pressing () == true && !GoalClampLast) {
GoalClampState = !GoalClampState;
GoalClampLast = true;
}
else if (!MainController.ButtonB.pressing () == true) {
GoalClampLast = false;
}
if (GoalClampState) {
ClampMotor.spin (directionType::fwd, 100, velocityUnits::pct);
}
else {
ConveyerMotor.spinTo (double (90), rotationUnits::deg);
}
task::sleep (50);
}
return 1;
}
int runMainLift () {
//Main Lift Up
if (MainController.ButtonR1.pressing () == true) {
MainLift.spin (directionType::fwd, 100, velocityUnits::pct);
}
//Main Lift Down
else if (MainController.ButtonR2.pressing () == true) {
MainLift.spin (directionType::rev, 100, velocityUnits::pct);
}
//Stop Main Lift Motors
else {
MainLift.stop();
MainLift.setStopping(hold);
}
return 1;
}
int runGoalLift () {
//Goal Lift Code
//Goal Lift Up
if (MainController.ButtonL1.pressing () == true) {
GoalLift.spin (directionType::fwd, 100, velocityUnits::pct);
}
//Goal Lift Down
else if (MainController.ButtonL2.pressing () == true) {
GoalLift.spin (directionType::rev, 100, velocityUnits::pct);
}
//Stop Goal Lift Motor
else {
GoalLift.stop();
GoalLift.setStopping(hold);
}
return 1;
}
int runMainDrive () {
//Drivetrain Axis Config
int axis3original = MainController.Axis3.position(percentUnits::pct);
int axis3mapped = 0.00008 * pow(axis3original, 3) + 0.2 * axis3original;
int axis4original = MainController.Axis4.position(percentUnits::pct);
int axis4mapped = 0.00008 * pow(axis4original, 3) + 0.2 * axis4original;;
//Drivetrain Motor Control
DriveRFront.spin(directionType::fwd, axis3mapped - axis4mapped, velocityUnits::pct);
DriveRBack.spin(directionType::fwd, axis3mapped - axis4mapped, velocityUnits::pct);
DriveLFront.spin(directionType::fwd, axis3mapped + axis4mapped, velocityUnits::pct);
DriveLBack.spin(directionType::fwd, axis3mapped + axis4mapped, velocityUnits::pct);
return 1;
}
//Driver Control Code
void usercontrol(void) {
while (1) {
task runMainDriveTask (runMainDrive);
task runMainLiftTask (runMainLift);
task runGoalLiftTask (runGoalLift);
task toggleConveyerTask (toggleConveyer);
task toggleClampTask (toggleClamp);
//Vex Wait
wait (20, msec);
}
}
//Vex Loop
int main() {
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);
pre_auton();
while (true) {
wait(100, msec);
}
}
edit by mods: code tags added, please try and remember to use them.