Untitled
Elephanf
c_cpp
2 years ago
30 kB
44
No Index
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: Navin Jayanthi, Prabhas Banerjee */
/* Created: Sep 17 2023 */
/* Description: Competition Template */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Controller1 controller
// Catapult motor 9
// LF motor 10
// RM motor 17
// RB motor 13
// RF motor 12
// LM motor 14
// LB motor 5
// Wing1 digital_out A
// Wing2 digital_out B
// Hangmech digital_out C
// Intake motor 20
// Inertial inertial 18
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
#include <algorithm>
#include <cmath>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <string>
using namespace std;
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the csompetition 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. */
/*---------------------------------------------------------------------------*/
string intToString(int toString) {
char endString[256];
sprintf(endString, "%d", toString);
return endString;
}
string doubleToString(double toString) {
char endString[256];
sprintf(endString, "%f", toString);
return endString;
}
string boolToString(bool toString) { return toString ? "true" : "false"; }
string append(string s, int i) {
string endString = (s + intToString(i));
return endString;
}
string append(string s, double d) {
string endString = (s + doubleToString(d));
return endString;
}
string append(string s, bool b) {
string endString = (s + boolToString(b));
return endString;
}
bool wingState1 = false;
bool wingState2 = false;
bool hangmechState = false;
bool expoDrive = true;
bool autonSelected = false;
int currentPrints = 0;
int Col = 1;
int autonSelect = 1;
const int Default_cataVelo = 80;
int cataVelo = 80;
const int intakeVelo = 100;
enum turnDir { LEFT, RIGHT };
enum stopType { COAST, HOLD };
enum debugType {
DRIVE,
WING,
STICK,
CATA,
INTAKE,
AUTON,
LOOP_TEST,
INERTIAL
};
enum driveType { EXPO, NORMAL };
enum driveDirection { FORWARD, REVERSE };
enum activationState { TOGGLE, HOLD_BUTTON };
enum wingTrigger { SINGLE, DOUBLE };
enum autoVelocityType { CONSTANT, SCALAR };
enum unitType { DEGS, REVS, INCHES };
enum expoDriveType {QUADRATIC, CUBIC, GEOMETRIC};
const expoDriveType default_DriveScalar = QUADRATIC;
enum autoDriveType {ARC, LOG, OCTIC, QUARTIC};
const autoDriveType default_AutoScalar = QUARTIC;
const autoDriveType default_TurnScalar = LOG;
class CDebug { // fair enough
private:
int driveLine;
int wingLine;
int stickLine;
int cataLine;
int intkLine;
int autonLine;
int loopLine;
int inertialLine;
public:
void processColumn() {
if (1+currentPrints >= 32) {
Col = 15;
}
}
void printsOn() {
Brain.Screen.setCursor(30, 30);
Brain.Screen.print(currentPrints);
processColumn();
}
void Print(debugType d) { // for default
processColumn();
switch (d) {
case DRIVE:
Brain.Screen.setCursor(1 + currentPrints, Col);
Brain.Screen.print(LF.velocity(percent));
Brain.Screen.setCursor(2 + currentPrints, Col);
Brain.Screen.print(LM.velocity(percent));
Brain.Screen.setCursor(3 + currentPrints, Col);
Brain.Screen.print(LB.velocity(percent));
Brain.Screen.setCursor(1 + currentPrints, Col+10);
Brain.Screen.print(RF.velocity(percent));
Brain.Screen.setCursor(2 + currentPrints, Col+10);
Brain.Screen.print(RM.velocity(percent));
Brain.Screen.setCursor(3 + currentPrints, Col+10);
Brain.Screen.print(RB.velocity(percent));
driveLine = 1+currentPrints;
currentPrints += 3;
break;
case WING:
Brain.Screen.setCursor(1+currentPrints, Col);
Brain.Screen.print(boolToString(wingState1).c_str());
Brain.Screen.setCursor(1+currentPrints, Col+10);
Brain.Screen.print(boolToString(wingState2).c_str());
wingLine = 1+currentPrints;
currentPrints +=1 ;
break;
case STICK:
Brain.Screen.setCursor(1 + currentPrints, Col);
Brain.Screen.print(Controller1.Axis3.position(percent));
Brain.Screen.setCursor(1 + currentPrints, Col+10);
Brain.Screen.print(Controller1.Axis4.position(percent));
stickLine = 1+currentPrints;
currentPrints += 1;
break;
case CATA:
Brain.Screen.setCursor(1 + currentPrints, Col);
Brain.Screen.print(append("Cata velocity: ", Catapult.velocity(rpm)).c_str());
cataLine = 1+currentPrints;
currentPrints += 1;
break;
case INTAKE:
Brain.Screen.setCursor(1 + currentPrints, Col);
Brain.Screen.print(append("Intake Velocity: ", Intake.velocity(rpm)).c_str());
intkLine = 1+currentPrints;
currentPrints += 1;
break;
case AUTON:
Brain.Screen.setCursor(1 + currentPrints, Col);
// Brain.Screen.print(append("Auton Actve: ", autonActive).c_str());
// autonLine = 1+currentPrints;
// currentPrints += 1;
break;
case LOOP_TEST:
Brain.Screen.setCursor(1 + currentPrints, Col);
Brain.Screen.print("In loop");
loopLine = 1+currentPrints;
currentPrints += 1;
break;
case INERTIAL:
Brain.Screen.setCursor(1+currentPrints, Col);
Brain.Screen.print(Inertial.heading(deg));
inertialLine = 1+currentPrints;
currentPrints += 1;
}
}
void debugAll() {
Print(DRIVE);
Print(WING);
Print(STICK);
Print(CATA);
Print(INTAKE);
Print(AUTON);
Print(LOOP_TEST);
Print(INERTIAL);
}
void Clear(debugType d) {
switch(d){
case DRIVE:
Brain.Screen.clearLine(driveLine);
Brain.Screen.clearLine(driveLine+1);
Brain.Screen.clearLine(driveLine+2);
currentPrints -= 2;
break;
case WING:
Brain.Screen.clearLine(wingLine);
break;
case STICK:
Brain.Screen.clearLine(stickLine);
break;
case CATA:
Brain.Screen.clearLine(cataLine);
break;
case INTAKE:
Brain.Screen.clearLine(intkLine);
break;
case AUTON:
Brain.Screen.clearLine(autonLine);
break;
case LOOP_TEST:
Brain.Screen.clearLine(loopLine);
break;
case INERTIAL:
Brain.Screen.clearLine(inertialLine);
break;
}
currentPrints -= 1;
processColumn();
}
void clearAll() {
Clear(DRIVE);
Clear(WING);
Clear(STICK);
Clear(CATA);
Clear(INTAKE);
Clear(AUTON);
Clear(LOOP_TEST);
Clear(INERTIAL);
processColumn();
}
};
CDebug Debug;
void clearAll() {
Brain.Screen.clearScreen();
currentPrints = 0;
Debug.processColumn();
}
void clearMostRecent(void) {
Brain.Screen.clearLine(currentPrints);
currentPrints -= 1;
}
void Print(string s) {
Debug.processColumn();
Brain.Screen.setCursor(1+currentPrints, Col);
Brain.Screen.print(s.c_str());
currentPrints +=1;
}
void Print(int i) {
Debug.processColumn();
Brain.Screen.setCursor(1+currentPrints, Col);
Brain.Screen.print(intToString(i).c_str());
currentPrints +=1;
}
void Print(double d) {
Debug.processColumn();
Brain.Screen.setCursor(1+currentPrints, Col);
Brain.Screen.print(doubleToString(d).c_str());
currentPrints +=1;
}
void Print(bool b) {
Debug.processColumn();
Brain.Screen.setCursor(1+currentPrints, Col);
Brain.Screen.print(boolToString(b).c_str());
currentPrints +=1;
}
class CPP_Drivetrain { // congratulations, you made driving more complicated
private:
const float Error = 0.1;
double inchesToRev(double x) { return x / (3.25/M_PI); }
double degsToRev(double x) {return x / 360;}
double gearRatio(double x) { return (72 / 36) * x; }
// double drive_QuarticScalar(double x) {
// if (x < 0) {
// return -1 * abs(pow(x, 4)/1000000);
// } else {
// return abs(pow(x, 4)/1000000);
// }
// }
double quadraticScalar(double x) {
if (x < 0) {
return -1 * abs(pow(x, 2)/100);
} else {
return abs(pow(x, 2)/100);
}
}
double cubicScalar(double x) { return 0.0001*pow(x, 3); }
double geometricScalar(double x) {
if (x >= 0) {
return 1.2*pow(1.043, x) - 1.2 + (0.2*x);
}
else {
x = -x;
x = 1.2*pow(1.043, x) - 1.2 + (0.2*x);
x = -x;
return x;
}
}
double arcScalar(double x) { return sqrt(1 - pow(1.7 * (x) - 1.7 + 1, 2)); }
double logScalar(double x) { return (log10(x+0.03) / log10(35)) + 0.99; }
double octicScalar(double x) {return -1 * (256 * pow(x - 0.5, 8) - 1); }
double quarticScalar(double x) { return -1 * (16*pow(x-0.5, 4) - 1); }
double startDistance = gearRatio((LF.position(rev) + RF.position(rev)) / 2);
double distDriven = gearRatio(((LF.position(rev) + RF.position(rev)) / 2) - startDistance);
double startTurnDistance = Inertial.heading();
double distTurned = Inertial.heading() - startTurnDistance;
void forwardDrive(void) {
LF.spin(vex::forward);
LM.spin(vex::forward);
LB.spin(vex::forward);
RF.spin(vex::forward);
RM.spin(vex::forward);
RB.spin(vex::forward);
}
void reverseDrive(void) {
LF.spin(vex::reverse);
LM.spin(vex::reverse);
LB.spin(vex::reverse);
RF.spin(vex::reverse);
RM.spin(vex::reverse);
RB.spin(vex::reverse);
}
void fullDrive(driveDirection d) {
switch(d){
case FORWARD:
forwardDrive();
break;
case REVERSE:
reverseDrive();
break;
}
}
void fullLeftTurn(void) {
LF.spin(vex::reverse);
LM.spin(vex::reverse);
LB.spin(vex::reverse);
RF.spin(vex::forward);
RM.spin(vex::forward);
RB.spin(vex::forward);
}
void fullRightTurn(void) {
LF.spin(vex::forward);
LM.spin(vex::forward);
LB.spin(vex::forward);
RF.spin(vex::reverse);
RM.spin(vex::reverse);
RB.spin(vex::reverse);
}
void fullTurn(turnDir t) {
switch (t) {
case LEFT:
fullLeftTurn();
break;
case RIGHT:
fullRightTurn();
break;
}
}
public:
double scalarSelect(double x, autoDriveType a) {
switch(a){
case ARC:
return arcScalar(x);
break;
case LOG:
return logScalar(x);
break;
case QUARTIC:
return quarticScalar(x);
break;
case OCTIC:
return octicScalar(x);
break;
}
}
double autoDriveScalar(double x) {
return scalarSelect(x, default_AutoScalar);
}
double autoTurnScalar(double x) {
return scalarSelect(x, default_TurnScalar);
}
double scalarSelect(double x, expoDriveType e) {
double y;
switch(e) {
case QUADRATIC:
y = quadraticScalar(x);
break;
case CUBIC:
y = cubicScalar(x);
break;
case GEOMETRIC:
y = geometricScalar(x);
break;
}
return y;
}
double expoDriveScalar(double x) {
return scalarSelect(x, default_DriveScalar);
}
void Stop(stopType stopType) {
switch (stopType) {
case HOLD:
LF.stop(hold);
LM.stop(hold);
LB.stop(hold);
RF.stop(hold);
RM.stop(hold);
RB.stop(hold);
break;
case COAST:
LF.stop(coast);
LM.stop(coast);
LB.stop(coast);
RF.stop(coast);
RM.stop(coast);
RB.stop(coast);
break;
}
}
void Velocity(double veloPer) {
LF.setVelocity(veloPer, percent);
LM.setVelocity(veloPer, percent);
LB.setVelocity(veloPer, percent);
RF.setVelocity(veloPer, percent);
RM.setVelocity(veloPer, percent);
RB.setVelocity(veloPer, percent);
}
void Turn(turnDir t, double turnDeg, autoVelocityType a) {
switch (a) {
case CONSTANT:
switch (t) {
case LEFT:
LF.spinFor(vex::reverse, gearRatio(turnDeg), degrees, false);
LM.spinFor(vex::reverse, gearRatio(turnDeg), degrees, false);
LB.spinFor(vex::reverse, gearRatio(turnDeg), degrees, false);
RF.spinFor(vex::forward, gearRatio(turnDeg), degrees, false);
RM.spinFor(vex::forward, gearRatio(turnDeg), degrees, false);
RB.spinFor(vex::forward, gearRatio(turnDeg), degrees, false);
Stop(HOLD);
break;
case RIGHT:
LF.spinFor(vex::reverse, gearRatio(turnDeg), degrees, false);
LM.spinFor(vex::reverse, gearRatio(turnDeg), degrees, false);
LB.spinFor(vex::reverse, gearRatio(turnDeg), degrees, false);
RF.spinFor(vex::forward, gearRatio(turnDeg), degrees, false);
RM.spinFor(vex::forward, gearRatio(turnDeg), degrees, false);
RB.spinFor(vex::forward, gearRatio(turnDeg), degrees, false);
Stop(HOLD);
break;
}
break;
case SCALAR:
startTurnDistance = Inertial.heading();
while (distTurned < turnDeg - Error) {
Debug.Print(LOOP_TEST);
Velocity(autoTurnScalar(distTurned / turnDeg) * 100);
fullTurn(t);
distTurned = Inertial.heading() - startTurnDistance;
}
Debug.Clear(LOOP_TEST);
Stop(HOLD);
break;
}
}
void DriveFor(driveDirection d, double driveRev, unitType u, autoVelocityType a) {
if (u == INCHES) {
driveRev = inchesToRev(driveRev);
}
else if (u == DEGS) {
driveRev = degsToRev(driveRev);
}
switch (a) {
case CONSTANT:
if (d == FORWARD) {
LF.spinFor(vex::forward, gearRatio(driveRev), rev, false);
LM.spinFor(vex::forward, gearRatio(driveRev), rev, false);
LB.spinFor(vex::forward, gearRatio(driveRev), rev, false);
RF.spinFor(vex::forward, gearRatio(driveRev), rev, false);
RM.spinFor(vex::forward, gearRatio(driveRev), rev, false);
RB.spinFor(vex::forward, gearRatio(driveRev), rev);
} else if (d == REVERSE) {
LF.spinFor(vex::reverse, gearRatio(driveRev), rev, false);
LM.spinFor(vex::reverse, gearRatio(driveRev), rev, false);
LB.spinFor(vex::reverse, gearRatio(driveRev), rev, false);
RF.spinFor(vex::reverse, gearRatio(driveRev), rev, false);
RM.spinFor(vex::reverse, gearRatio(driveRev), rev, false);
RB.spinFor(vex::reverse, gearRatio(driveRev), rev);
}
break;
case SCALAR:
startDistance = ((LF.position(rev) + RF.position(rev)) / 2);
if (d == REVERSE) {
driveRev *= -1;
distDriven = -1 * (((LF.position(rev) + RF.position(rev)) / 2) - startDistance);
}
while (distDriven < driveRev - Error) {
Velocity(gearRatio(autoDriveScalar(distDriven / driveRev)) * 100);
fullDrive(d);
if (d == FORWARD) {
distDriven =
((LF.position(rev) + RF.position(rev)) / 2) - startDistance;
}
else if (d == REVERSE) {
distDriven =
-1 * (((LF.position(rev) + RF.position(rev)) / 2) - startDistance);
}
}
Stop(HOLD);
}
}
void Drive(driveType d) {
int DTV = -1 * Controller1.Axis3.position(percent);
int DTH = Controller1.Axis4.position(percent);
int RightDrive = DTV + DTH;
int LeftDrive = DTV - DTH;
switch (d) {
case NORMAL:
LF.setVelocity(LeftDrive, percent);
LM.setVelocity(LeftDrive, percent);
LB.setVelocity(LeftDrive, percent);
RF.setVelocity(RightDrive, percent);
RM.setVelocity(RightDrive, percent);
RB.setVelocity(RightDrive, percent);
fullDrive(FORWARD);
break;
case EXPO:
LF.setVelocity(expoDriveScalar(LeftDrive), percent);
LM.setVelocity(expoDriveScalar(LeftDrive), percent);
LB.setVelocity(expoDriveScalar(LeftDrive), percent);
RF.setVelocity(expoDriveScalar(RightDrive), percent);
RM.setVelocity(expoDriveScalar(RightDrive), percent);
RB.setVelocity(expoDriveScalar(RightDrive), percent);
fullDrive(FORWARD);
break;
}
}
};
CPP_Drivetrain Drivetrain;
void wingFunc(wingTrigger t, activationState a) {
switch (t) {
case SINGLE:
switch (a) {
case TOGGLE:
Controller1.ButtonY.pressed([]() -> void{
wingState1 = !wingState1;
wingState2 = !wingState2;
Wing1.set(wingState1);
Wing2.set(wingState2);
});
break;
case HOLD_BUTTON:
if (Controller1.ButtonY.pressing()) {
Wing1.set(true);
Wing2.set(true);
} else {
Wing1.set(false);
Wing2.set(false);
}
break;
}
break;
case DOUBLE:
switch (a) {
case TOGGLE:
Controller1.ButtonL1.pressed([]() -> void {
wingState1 = !wingState1;
Wing1.set(wingState1);
});
Controller1.ButtonR1.pressed([]() -> void {
wingState2 = !wingState2;
Wing2.set(wingState2);
});
break;
case HOLD_BUTTON:
if (Controller1.ButtonL1.pressing()) {
Wing1.set(true);
} else {
Wing1.set(false);
}
if (Controller1.ButtonR1.pressing()) {
Wing2.set(true);
} else {
Wing2.set(false);
}
break;
}
break;
}
}
void setCataVelo(double velo) {
cataVelo = velo;
Catapult.setVelocity(cataVelo, percent);
}
void autonDisplay(void) {
Controller1.Screen.clearLine(3);
Controller1.Screen.setCursor(3, 0);
switch (autonSelect) {
case 1:
Controller1.Screen.print("No Auto");
break;
case 2:
Controller1.Screen.print("Far Auto (AWP)");
break;
case 3:
Controller1.Screen.print("Close Auto (AWP)");
break;
case 4:
Controller1.Screen.print("Far Auto (Elims)");
break;
case 5:
Controller1.Screen.print("Close Auto (Elims)");
break;
case 6:
Controller1.Screen.print("Skills");
break;
}
}
void autonChanger(void) {
if (!autonSelected){
if (autonSelect < 6) {
autonSelect+= 1;
} else if (autonSelect >= 6) {
autonSelect = 1;
}
}
}
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
Inertial.calibrate();
while (Inertial.isCalibrating()) {
wait(100, msec);
}
autonDisplay();
Wing1.set(wingState1);
Wing2.set(wingState2);
Hangmech.set(hangmechState);
setCataVelo(Default_cataVelo);
// 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) {
//TEST AUTONOMOUS
autonSelected = true;
// preAutonActive = false;
// autonActive = true;
Controller1.Screen.clearLine(3);
clearAll();
switch (autonSelect) {
case 1:
// no auto
Print("No Auto");
wait(20, msec);
break;
case 2:
// far auto AWP
Print("Far Auto AWP");
Drivetrain.DriveFor(FORWARD, 25, INCHES, SCALAR);
wait(0.5, sec);
Drivetrain.Turn(RIGHT, 90, SCALAR);
Intake.spinFor(vex::reverse, 5, rev);
Drivetrain.DriveFor(FORWARD, 5, INCHES, SCALAR);
wait(0.5, sec);
Drivetrain.Turn(RIGHT, 180, SCALAR);
Drivetrain.DriveFor(FORWARD, 5, INCHES, SCALAR);
Intake.spinFor(vex::forward, 3, rev);
wait(0.5, sec);
Drivetrain.Turn(RIGHT, 180, SCALAR);
Intake.spinFor(vex::reverse, 5, rev);
Drivetrain.DriveFor(FORWARD, 5, INCHES, SCALAR);
wait(0.5, sec);
Drivetrain.Turn(RIGHT, 180, SCALAR);
Drivetrain.DriveFor(FORWARD, 10, INCHES, SCALAR);
Intake.spinFor(vex::forward, 3, rev);
wait(0.5, sec);
Drivetrain.Turn(RIGHT, 180, SCALAR);
Intake.spinFor(vex::reverse, 5, rev);
Drivetrain.DriveFor(FORWARD, 10, INCHES, SCALAR);
wait(0.1, sec);
Drivetrain.DriveFor(REVERSE, 15, INCHES, SCALAR);
Drivetrain.Turn(RIGHT, 90, SCALAR);
wait(0.5, sec);
wingState1 = true;
wingState2 = true;
Wing1.set(wingState1);
Wing2.set(wingState2);
Drivetrain.DriveFor(FORWARD, 20, INCHES, SCALAR);
break;
case 3:
// close auto AWP
Print("Close Auto AWP");
wingState2 = true;
Wing2.set(wingState2);
Drivetrain.DriveFor(REVERSE, 3, INCHES, SCALAR);
wait(0.1, sec);
Drivetrain.DriveFor(FORWARD, 6, INCHES, SCALAR);
wingState2 = false;
Wing2.set(wingState2);
wait(0.5, sec);
Drivetrain.Turn(LEFT, 180, SCALAR);
Drivetrain.DriveFor(FORWARD, 9, INCHES, SCALAR);
Drivetrain.Turn(RIGHT, 45, SCALAR);
Drivetrain.DriveFor(FORWARD, 3, INCHES, SCALAR);
Intake.spinFor(vex::reverse, 10, rev);
Drivetrain.DriveFor(REVERSE, 2, INCHES, SCALAR);
Drivetrain.DriveFor(FORWARD, 5, INCHES, SCALAR);
wait(0.5, sec);
Drivetrain.Turn(RIGHT, 135, SCALAR);
Drivetrain.DriveFor(FORWARD, 10, INCHES, SCALAR);
wait(0.3, sec);
Drivetrain.Turn(LEFT, 45, SCALAR);
Drivetrain.DriveFor(FORWARD, 5, INCHES, SCALAR);
break;
// Far Auto ELIMS
Print("Far Auto Elims (do not use)");
break;
case 5:
// close auto ELIMS
Print("Close Auto Elims (do not use)");
break;
case 6:
//skills
Print("Skills");
Drivetrain.DriveFor(FORWARD, 3, INCHES, SCALAR);
wait(0.25, sec);
Drivetrain.Turn(RIGHT, 40, SCALAR);
wait(0.25, sec);
Drivetrain.DriveFor(FORWARD, 14, INCHES, SCALAR);
wait(0.25, sec);
Drivetrain.Turn(LEFT, 45, SCALAR);
wait(0.25, sec);
Drivetrain.DriveFor(FORWARD, 6, INCHES, SCALAR);
wait(0.25, sec);
Catapult.spinFor(vex::forward, 50, rev);
wait(0.25, sec);
Drivetrain.Turn(LEFT, 45, SCALAR);
Drivetrain.DriveFor(FORWARD, 78, INCHES, SCALAR);
wait(0.25, sec);
Drivetrain.Turn(RIGHT, 90, SCALAR);
wait(0.25, sec);
wingState1 = true;
wingState2 = true;
Wing1.set(wingState1);
Wing2.set(wingState2);
Drivetrain.DriveFor(FORWARD, 32.5, INCHES, SCALAR);
wait(0.25, sec);
wingState1 = false;
wingState2 = false;
Wing1.set(wingState1);
Wing1.set(wingState2);
Drivetrain.DriveFor(REVERSE, 9, INCHES, SCALAR);
wait(0.5, sec);
Drivetrain.Turn(RIGHT, 90, SCALAR);
Drivetrain.DriveFor(FORWARD, 9, INCHES, SCALAR);
wait(0.5, sec);
Drivetrain.Turn(LEFT, 135, SCALAR);
wait(0.5, sec);
break;
}
clearMostRecent();
// autonActive = false;
// ..........................................................................
// 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) {
while(!autonSelected){
Controller1.ButtonLeft.pressed(autonChanger);
Controller1.ButtonA.pressed([]() -> void {
autonSelected = true;
Controller1.ButtonLeft.pressed([]() -> void {});
Controller1.ButtonA.pressed([]() -> void {});
});
autonDisplay();
wait(100, msec);
}
Controller1.Screen.clearScreen();
Controller1.Screen.setCursor(1, 0);
Controller1.Screen.print(append("Cata velocity %: ", floor(cataVelo)).c_str());
Controller1.Screen.setCursor(2, 0);
Controller1.Screen.print(append("EX Drive: ", expoDrive).c_str());
Debug.Print(WING);
Print("two");
Debug.Print(DRIVE);
Print(2);
Debug.Print(STICK);
Print(2.0);
Print(false);
Debug.Print(CATA);
Debug.Print(INERTIAL);
// User control code here, inside the loop
while (1) {
Catapult.setMaxTorque(100, percent);
// This is the main execution loop for the user control program.
// Each time throughs the loop your program should update motor + servo
// values based on feedback from the joysticks.
bool cataVeloUp = Controller1.ButtonUp.pressing();
if (cataVeloUp && cataVelo < 100) {
cataVelo += 10;
Controller1.Screen.clearLine(1);
Controller1.Screen.setCursor(1, 0);
Controller1.Screen.print(append("Cata velocity %: ", (cataVelo)).c_str());
wait(100, msec);
}
bool cataVeloDown = Controller1.ButtonDown.pressing();
if (cataVeloDown && cataVelo > 10) {
cataVelo -= 10;
Controller1.Screen.clearLine(1);
Controller1.Screen.setCursor(1, 0);
Controller1.Screen.print(append("Cata velocity %: ", (cataVelo)).c_str());
wait(100, msec);
}
// setting velocities
Catapult.setVelocity(cataVelo, percent);
Intake.setVelocity(intakeVelo, percent);
// drive
if (expoDrive == true) {
Drivetrain.Drive(EXPO);
} else if (expoDrive == false) {
Drivetrain.Drive(NORMAL);
}
Controller1.ButtonRight.pressed([]() -> void {
Controller1.Screen.clearLine(2);
expoDrive = !expoDrive;
Controller1.Screen.setCursor(2, 0);
Controller1.Screen.print(append("EX Drive: ", expoDrive).c_str());
wait(500, msec);
});
// catapult
if (Controller1.ButtonR1.pressing()) {
Catapult.spin(vex::forward);
} else {
Catapult.stop(hold);
}
// wings
wingFunc(SINGLE, TOGGLE);
// intake
if (Controller1.ButtonA.pressing()) {
Intake.spin(vex::forward);
} else if (Controller1.ButtonB.pressing()) {
Intake.spin(vex::reverse);
} else {
Intake.stop(hold);
}
// endgame
Controller1.ButtonX.pressed([]() -> void {
hangmechState = !hangmechState;
Hangmech.set(hangmechState);
});
// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................
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);
}
}
Editor is loading...