Ok, here is my functions and auton.
/--------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: ProggrammingKid /
/ Created: Sun Sep 08 2019 /
/ Description: V5 project /
/ /
/----------------------------------------------------------------------------*/
#include “vex.h”
/* ---- START VEXCODE CONFIGURED DEVICES ----
Robot Configuration:
[Name] [Type] [Port(s)]
Vision17 vision 17
FL motor 10
FR motor 2
BL motor 4
BR motor 3
---- END VEXCODE CONFIGURED DEVICES ----*/
using namespace vex;
// A global instance of vex::brain used for printing to the V5 brain screen
// A global instance of vex::competition
vex::competition Competition;
//Function for going forward
void forward1(float distance, int speed){
FL.rotateTo(distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct,false);
FR.rotateTo(distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct,false);
BL.rotateTo(distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct,false);
BR.rotateTo(distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct);
}
//Function to go backwards
//Check motor positions for - on the distance and for setting reversed motors
void backwards1(float distance, int speed){
FL.rotateTo(-distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct,false);
FR.rotateTo(-distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct,false);
BL.rotateTo(-distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct,false);
BR.rotateTo(-distance,vex::rotationUnits::rev,speed, vex::velocityUnits::pct);
}
//function for going left
void left1(float distance, int speed){
FL.rotateTo(-distance,rotationUnits::rev,speed, velocityUnits::pct,false);
FR.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct,false);
BL.rotateTo(-distance,rotationUnits::rev,speed, velocityUnits::pct,false);
BR.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct);
}
void left2(float distance, int speed){
FR.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct,false);
BR.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct);
}
//funtion for right
void right1(float distance, int speed){
FL.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct,false);
FR.rotateTo(-distance,rotationUnits::rev,speed, velocityUnits::pct,false);
BL.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct,false);
BR.rotateTo(-distance,rotationUnits::rev,speed, velocityUnits::pct);
}
void right2(float distance, int speed){
FL.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct,false);
BL.rotateTo(distance,rotationUnits::rev,speed, velocityUnits::pct);
}
//Intake in function
void intIn( float distance, int speed){
IntL.rotateTo(distance, rotationUnits::rev,speed, velocityUnits::pct,false);
IntR.rotateTo(-distance, rotationUnits::rev,speed, velocityUnits::pct);
}
//Intake out function
void intOut( float distance, int speed){
IntL.rotateTo(-distance, rotationUnits::rev,speed, velocityUnits::pct,false);
IntR.rotateTo(distance, rotationUnits::rev,speed, velocityUnits::pct);
}
//center intake in function
void centerIn( float distance, int speed){
centerInt.rotateTo(-distance, rotationUnits::rev,speed, velocityUnits::pct);
}
void doBoth (float distance, int speed){
IntL.rotateTo(distance, rotationUnits::rev,speed, velocityUnits::pct,false);
IntR.rotateTo(-distance, rotationUnits::rev,speed, velocityUnits::pct,false);
centerInt.rotateTo(-distance, rotationUnits::rev,speed, velocityUnits::pct);
}
//reseting encoders function
void resetEncoders(){
BR.resetRotation();
FR.resetRotation();
BL.resetRotation();
FL.resetRotation();
}
//Sets stopping type
void setStop(){
BR.setStopping(brakeType::brake);
FR.setStopping(brakeType::brake);
BL.setStopping(brakeType::brake);
FL.setStopping(brakeType::brake);
}
//test 90
//a function to easly sleep
void bedtime(double ms){
task::sleep(ms);
}
/*----------------------------------------------------------------------
Pre-Autonomous Functions
---------------------------------------------------------------------------*/
void pre_auton( void ) {
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …
resetEncoders();
setStop();
int speedy_boi;
speedy_boi = 100;
double power;
power = 100;
int twenteee;
twenteee = 25;
int fiftyee;
fiftyee = 50;
FL.setReversed(false);
FR.setReversed(true);
BL.setReversed(false);
BR.setReversed(true);
//Sets max tourque for motors
FL.setMaxTorque(power,percentUnits::pct);
FR.setMaxTorque(power,percentUnits::pct);
BR.setMaxTorque(power,percentUnits::pct);
BL.setMaxTorque(power,percentUnits::pct);
IntL.setMaxTorque(power,percentUnits::pct);
IntR.setMaxTorque(power,percentUnits::pct);
centerInt.setMaxTorque(power,percentUnits::pct);
IntL.setVelocity(speedy_boi, percentUnits::pct);
IntR.setVelocity(speedy_boi, percentUnits::pct);
centerInt.setVelocity(speedy_boi, percentUnits::pct);
}
/*---------------------------------------------------------------------------
Autonomous Task
---------------------------------------------------------------------------*/
void autonomous( void ) {
forward1(1.27, 60);
resetEncoders();
left2(2.8, 60);
resetEncoders();
forward1(1.21, 40);
resetEncoders();
intIn(2, 100);
resetEncoders();
doBoth(4.3, 100);
resetEncoders();
backwards1(1.42, 50);
resetEncoders();
left2(-1.08, 65);
resetEncoders();
backwards1(3.12,60);
resetEncoders();
left1(1.1,65);
resetEncoders();
forward1(0.5,50);
resetEncoders();
doBoth(8,100);
resetEncoders();
bedtime(90);
backwards1(1,100);
}