It’s really long. Sorry!
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: 23ReiT */
/* Created: Wed Oct 30 2019 */
/* Description: V5 project */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// CrabMotor motor 12
// Controller1 controller
// LeftArm motor 18
// RightArm motor 19
// LeftRoller motor 5
// RightRoller motor 6
// CubeBase motor 3
// LeftMotor motor 20
// RightMotor motor 9
// CubeBaseSwitch limit B
// GyroSensor gyro A
// RightBumper bumper H
// LeftBumper bumper G
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
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 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 LeftGyroTurn(double degree, int veloc)// Auton Call for Turning via Left side with degrees and velocity as variables
{
//Prints the DegreeAmount for debugging puroses to ensure that it is going for the right degree amount
/*LeftMotor.setVelocity(veloc,vex::velocityUnits::pct);
RightMotor.setVelocity(veloc,vex::velocityUnits::pct);
Controller1.Screen.clearScreen();
Controller1.Screen.print(GyroSensor.value(rotationUnits::deg));
//While loop to do the spin
while (GyroSensor.value(rotationUnits::deg) <= degree)
{
LeftMotor.spin(directionType::rev); // Assuming this is the polarity needed for a clockwise turn
RightMotor.spin(directionType::fwd);
this_thread::sleep_for(10);
}
*/
double targetGyroValue; // earlier in the code
targetGyroValue = GyroSensor.value(rotationUnits::deg) + degree;
while(GyroSensor.value(rotationUnits::deg) < targetGyroValue)
{
LeftMotor.spin(directionType::rev, 50, velocityUnits::pct);
RightMotor.spin(directionType::fwd, 50, velocityUnits::pct);
}
//Stop motors after reached degree turn
LeftMotor.stop();
RightMotor.stop();
Controller1.Screen.clearScreen();
Controller1.Screen.print("Gyro Turn Finished");
}
void RightGyroTurn(double degree, int veloc)
{
//Prints the DegreeAmount for debugging puroses to ensure that it is going for the right degree amount
LeftMotor.setVelocity(veloc,vex::velocityUnits::pct);
RightMotor.setVelocity(veloc,vex::velocityUnits::pct);
Controller1.Screen.clearScreen();
Controller1.Screen.print(degree);
//While loop to do the spin
while (GyroSensor.value(rotationUnits::deg) <= degree)
{
LeftMotor.spin(directionType::fwd); // Assuming this is the polarity needed for a clockwise turn
RightMotor.spin(directionType::rev);
this_thread::sleep_for(10);
}
//Stop motors after reached degree turn
LeftMotor.stop();
RightMotor.stop();
Controller1.Screen.clearScreen();
Controller1.Screen.print("Gyro Turn Finished");
}
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
task::sleep(2000);
}
/*---------------------------------------------------------------------------*/
/* */
/* 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)
{
while(1)
{
LeftMotor.startRotateFor(vex::directionType::fwd, 1300, vex::rotationUnits::deg, 30, vex::velocityUnits::pct);
RightMotor.startRotateFor(vex::directionType::fwd, 1300, vex::rotationUnits::deg, 30, vex::velocityUnits::pct);
LeftRoller.startRotateFor(vex::directionType::fwd,10, vex::rotationUnits::rev,100, vex::velocityUnits::pct);
RightRoller.startRotateFor(vex::directionType::fwd,10, vex::rotationUnits::rev,100, vex::velocityUnits::pct);
task::sleep(500);
LeftRoller.startRotateFor(vex::directionType::fwd,10, vex::rotationUnits::rev,100, vex::velocityUnits::pct);
RightRoller.startRotateFor(vex::directionType::fwd,10, vex::rotationUnits::rev,100, vex::velocityUnits::pct);
task::sleep(5000);
LeftMotor.startRotateFor(vex::directionType::fwd, -1300, vex::rotationUnits::deg, 50, vex::velocityUnits::pct);
RightMotor.rotateFor(vex::directionType::fwd, -1300, vex::rotationUnits::deg, 50, vex::velocityUnits::pct);
task::sleep(100);
if(LeftBumper.pressing())//Makes sure that the robot is perpendicular to the wall (Step 1)
{
if(RightBumper.pressing())//Makes sure that the robot is perpendicular to the wall (Step 2)
{
RightGyroTurn(90, 50);
LeftMotor.startRotateFor(vex::directionType::fwd, 500, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
RightMotor.rotateFor(vex::directionType::fwd, 500, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
task::sleep(500);
RightMotor.startRotateFor(vex::directionType::fwd,1, rotationUnits::rev, 70, velocityUnits::pct);
LeftMotor.startRotateFor(vex::directionType::fwd,1, rotationUnits::rev, 70, velocityUnits::pct);
CubeBase.startRotateFor(vex::directionType::fwd, 240, vex::rotationUnits::deg,50, vex::velocityUnits::pct);
LeftRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
RightRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
wait(2000, msec);
LeftMotor.startRotateFor(vex::directionType::fwd, -3, vex::rotationUnits::rev, 100, vex::velocityUnits::pct);
RightMotor.rotateFor(vex::directionType::fwd, -3, vex::rotationUnits::rev, 100, vex::velocityUnits::pct);
CubeBase.startRotateFor(vex::directionType::fwd, -240, vex::rotationUnits::deg,50, vex::velocityUnits::pct);
}
else
{
RightMotor.spin(vex::directionType::rev);
}
}
else if(RightBumper.pressing())//Makes sure that the robot is perpendicular to the wall (Step 1)
{
if(LeftBumper.pressing())//Makes sure that the robot is perpendicular to the wall (Step 2)
{
RightGyroTurn(90,50);
LeftMotor.startRotateFor(vex::directionType::fwd, 500, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
RightMotor.rotateFor(vex::directionType::fwd, 500, vex::rotationUnits::deg, 70, vex::velocityUnits::pct);
task::sleep(500);
RightMotor.startRotateFor(vex::directionType::fwd,1, rotationUnits::rev, 70, velocityUnits::pct);
LeftMotor.startRotateFor(vex::directionType::fwd,1, rotationUnits::rev, 70, velocityUnits::pct);
CubeBase.startRotateFor(vex::directionType::fwd, 240, vex::rotationUnits::deg,50, vex::velocityUnits::pct);
LeftRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
RightRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
wait(2000, msec);
LeftMotor.startRotateFor(vex::directionType::fwd, -3, vex::rotationUnits::rev, 100, vex::velocityUnits::pct);
RightMotor.rotateFor(vex::directionType::fwd, -3, vex::rotationUnits::rev, 100, vex::velocityUnits::pct);
CubeBase.startRotateFor(vex::directionType::fwd, -240, vex::rotationUnits::deg,50, vex::velocityUnits::pct);
}
else
{
LeftMotor.spin(vex::directionType::rev);
}
}
LeftMotor.stop();
RightMotor.stop();
LeftRoller.stop();
RightRoller.stop();
task::sleep(100000000);
}
}
/*---------------------------------------------------------------------------*/
/* */
/* 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)
{
// User control code here, inside the loop
while (1)
{
Brain.Screen.print(GyroSensor.value(rotationUnits::deg));
//Drivetrain
// Drivetrain.arcade(100,100, percentUnits (percentUnits::pct));
LeftMotor.spin(directionType::fwd,Controller1.Axis3.position(percentUnits::pct)/2,velocityUnits::pct);//Left on Axis 3 but halved
RightMotor.spin(directionType::fwd,Controller1.Axis2.position(percentUnits::pct)/2,velocityUnits::pct);//Right on Axis 2 but halved
//CrabMotor
if(Controller1.ButtonLeft.pressing())
{
CrabMotor.spin(directionType::fwd,50,vex::velocityUnits::pct);
LeftMotor.stop(brakeType::hold);
RightMotor.stop(brakeType::hold);
}
else if(Controller1.ButtonRight.pressing())
{
CrabMotor.spin(directionType::rev,50,vex::velocityUnits::pct);
LeftMotor.stop(brakeType::hold);
RightMotor.stop(brakeType::hold);
}
else
{
CrabMotor.stop();
LeftMotor.setStopping(coast);
RightMotor.setStopping(coast);
}
//gyro test
if(Controller1.ButtonB.pressing())
{
LeftGyroTurn(9,50);
}
//Rollers
if(Controller1.ButtonR1.pressing())
{
LeftRoller.spin(vex::directionType::fwd,50, vex::velocityUnits::pct);
RightRoller.spin(vex::directionType::fwd,50, vex::velocityUnits::pct);
}
else if(Controller1.ButtonR2.pressing())
{
LeftRoller.spin(vex::directionType::fwd,-50, vex::velocityUnits::pct);
RightRoller.spin(vex::directionType::fwd,-50, vex::velocityUnits::pct);
}
else
{
LeftRoller.stop();
RightRoller.stop();
}
//Arms
if(Controller1.ButtonL2.pressing())
{
LeftArm.spin(vex::directionType::fwd, 50, vex::velocityUnits::pct);
RightArm.spin(vex::directionType::fwd, 50, vex::velocityUnits::pct);
}
else if(Controller1.ButtonL1.pressing())
{
LeftArm.spin(vex::directionType::fwd, -50, vex::velocityUnits::pct);
RightArm.spin(vex::directionType::fwd, -50, vex::velocityUnits::pct);
}
else
{
LeftArm.stop();
RightArm.stop();
}
//CubeBase
if(Controller1.ButtonUp.pressing())
{
LeftRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
RightRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
CubeBase.rotateFor(200,vex::rotationUnits::deg, 30, velocityUnits::pct);
CubeBase.stop(brakeType::hold);
task::sleep(500);
}
else if(Controller1.ButtonDown.pressing())
{
CubeBase.rotateFor(-200,vex::rotationUnits::deg, 70, velocityUnits::pct);
CubeBase.stop();
}
else if(CubeBaseSwitch.pressing())
{
CubeBase.stop();
}
else
{
CubeBase.stop();
}
//Automation
if(Controller1.ButtonA.pressing())
{
LeftRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
RightRoller.spin(vex::directionType::fwd,-10, vex::velocityUnits::pct);
CubeBase.rotateFor(200,vex::rotationUnits::deg, 30, velocityUnits::pct);
CubeBase.stop(brakeType::hold);
RightMotor.startRotateFor(vex::directionType::rev,1, rotationUnits::rev, 20, velocityUnits::pct);
LeftMotor.startRotateFor(vex::directionType::rev,1, rotationUnits::rev, 0, velocityUnits::pct);
CubeBase.rotateFor(-200,vex::rotationUnits::deg, 70, velocityUnits::pct);
CubeBase.stop();
}
printf("Gyro: %.2f", GyroSensor.value(rotationUnits::deg));
}
wait(20, msec);
}
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);
}
}