Hi, so I have been working on a PD loop for my autons and there is one main thing that is bringing me back. When I am trying to turn it seems like the forward command below it is connecting to each other which isn’t what I want. As far as I know, it isn’t a math problem from talking to a couple of different people but something else they couldn’t figure out. ChangeUp1.5.zip (32.4 KB)
#include "vex.h"
vex::competition Competition;
vex::motor LeftMotor = vex::motor( vex::PORT11,true );
vex::motor BackLeftMotor = vex::motor( vex::PORT12,true );
vex::motor_group LDrive = vex::motor_group(LeftMotor,BackLeftMotor);
vex::motor RightMotor = vex::motor( vex::PORT20,false );
vex::motor BackRightMotor = vex::motor( vex::PORT19,false );
vex::motor_group RDrive = vex::motor_group(RightMotor,BackRightMotor);
vex::motor_group Drive = vex::motor_group(LeftMotor,BackLeftMotor,RightMotor,BackRightMotor);
vex::motor Intake = vex::motor(vex::PORT13,true );
vex::motor Intake2 = vex::motor(vex::PORT17,false);
vex::motor_group Intakes = vex::motor_group(Intake,Intake2);
vex::motor Index = vex::motor(vex::PORT18,false );
vex::motor Index2 = vex::motor(vex::PORT14,false);
vex::motor_group Indexer = vex::motor_group(Index,Index2);
vex::controller Controller1 = vex::controller(vex::controllerType::primary);
vex::controller Controller2 = vex::controller(vex::controllerType::partner);
triport ThreeWirePort = vex::triport( vex::PORT22 );
vex::line LineA = vex::line(ThreeWirePort.A);
vex::line LineB = vex::line(ThreeWirePort.B);
void wait(float mSec){
vex::task::sleep(mSec);
}
int driveControlTask(){
while (Competition.isAutonomous()!=1){
LeftMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value(), vex::velocityUnits::pct);
RightMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() - Controller1.Axis1.value(), vex::velocityUnits::pct);
BackLeftMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value(), vex::velocityUnits::pct);
BackRightMotor.spin(vex::directionType::fwd, Controller1.Axis3.value() - Controller1.Axis1.value(), vex::velocityUnits::pct);
wait(10);
} return(0);
}
void Indexcontroll()
{
if(Controller1.ButtonL2.pressing() == true) //extaking
{
Indexer.spin(vex::directionType::fwd,-100,vex::velocityUnits::pct);
Intakes.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else if(Controller1.ButtonR2.pressing() == true) //intaking
{
Indexer.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
Intakes.spin(vex::directionType::fwd,-100,vex::velocityUnits::pct);
}
else if(Controller1.ButtonR1.pressing() == true) //auto index
{
Index.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
Index2.spin(vex::directionType::fwd,-100,vex::velocityUnits::pct);
Intakes.spin(vex::directionType::fwd,-100,vex::velocityUnits::pct);
}
else if (Controller1.ButtonL1.pressing() && Controller1.ButtonR2.pressing() == true) //Just scoring
{
Indexer.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else if (Controller1.ButtonL1.pressing() && Controller1.ButtonR1.pressing() ==true) //Just descoring
{
Intakes.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else if (Controller1.ButtonB.pressing() == true) //flipping out
{
Indexer.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
Intakes.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else if (Controller1.ButtonRight.pressing() == true) //flipping out
{
Intakes.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else if (Controller1.ButtonY.pressing() == true) //flipping out
{
Indexer.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else if (Controller1.ButtonY.pressing() == true) //flipping out
{
Indexer.spin(vex::directionType::fwd,100,vex::velocityUnits::pct);
}
else
{
Indexer.stop(vex::brakeType::brake);
Intakes.stop(vex::brakeType::brake);
}
vex::task::sleep(15);
}
//////////////////
// LCD Commands //
//////////////////
int sq1StartX = 50;
int sq1SizeX = 80;
int sq1EndX = sq1StartX + sq1SizeX;
int sq1StartY = 30;
int sq1SizeY = 80;
int sq1EndY = sq1StartY + sq1SizeY;
int sq2StartX = 150;
int sq2SizeX = 80;
int sq2EndX = sq2StartX + sq2SizeX;
int sq2StartY = 30;
int sq2SizeY = 80;
int sq2EndY = sq2StartY + sq2SizeY;
int sq3StartX = 250;
int sq3SizeX = 80;
int sq3EndX = sq3StartX + sq3SizeX;
int sq3StartY = 30;
int sq3SizeY = 80;
int sq3EndY = sq3StartY + sq3SizeY;
int sq4StartX = 350;
int sq4SizeX = 80;
int sq4EndX = sq4StartX + sq4SizeX;
int sq4StartY = 30;
int sq4SizeY = 80;
int sq4EndY = sq4StartY + sq4SizeY;
int sq5StartX = 50;
int sq5SizeX = 80;
int sq5EndX = sq5StartX + sq5SizeX;
int sq5StartY = 130;
int sq5SizeY = 80;
int sq5EndY = sq5StartY + sq5SizeY;
int sq6StartX = 150;
int sq6SizeX = 80;
int sq6EndX = sq6StartX + sq6SizeX;
int sq6StartY = 130;
int sq6SizeY = 80;
int sq6EndY = sq6StartY + sq6SizeY;
int sq7StartX = 250;
int sq7SizeX = 80;
int sq7EndX = sq7StartX + sq7SizeX;
int sq7StartY = 130;
int sq7SizeY = 80;
int sq7EndY = sq7StartY + sq7SizeY;
int sq8StartX = 350;
int sq8SizeX = 80;
int sq8EndX = sq8StartX + sq8SizeX;
int sq8StartY = 130;
int sq8SizeY = 80;
int sq8EndY = sq8StartY + sq8SizeY;
int selected = 0;
void colorChanger(bool selection){
if (selection == true){Brain.Screen.setFillColor(vex::color::black);}
else {Brain.Screen.setFillColor(vex::color::red);}
}
//Make sure lettering is to the correct square
//Make all autonomouses into voids and put them into the autonomos task
//
int LCD(){
Brain.Screen.setPenColor(vex::color::red);
Brain.Screen.setFillColor(vex::color::red);
while (true){
if (selected == 1){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq1StartX, sq1StartY, sq1SizeX, sq1SizeY);
if (selected == 2){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq2StartX, sq2StartY, sq2SizeX, sq2SizeY);
if (selected == 3){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq3StartX, sq3StartY, sq3SizeX, sq3SizeY);
if (selected == 4){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq4StartX, sq4StartY, sq4SizeX, sq4SizeY);
if (selected == 5){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq5StartX, sq5StartY, sq5SizeX, sq5SizeY);
if (selected == 6){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq6StartX, sq6StartY, sq6SizeX, sq6SizeY);
if (selected == 7){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq7StartX, sq7StartY, sq7SizeX, sq7SizeY);
if (selected == 8){colorChanger(true);} else {colorChanger(false);}
Brain.Screen.drawRectangle(sq8StartX, sq8StartY, sq8SizeX, sq8SizeY);
int xPressed = Brain.Screen.xPosition();
int yPressed = Brain.Screen.yPosition();
if (Brain.Screen.pressing()){
if (xPressed > sq1StartX && xPressed < sq1EndX && yPressed > sq1StartY && yPressed < sq1EndY){selected = 1;}
if (xPressed > sq2StartX && xPressed < sq2EndX && yPressed > sq2StartY && yPressed < sq2EndY){selected = 2;}
if (xPressed > sq3StartX && xPressed < sq3EndX && yPressed > sq3StartY && yPressed < sq3EndY){selected = 3;}
if (xPressed > sq4StartX && xPressed < sq4EndX && yPressed > sq4StartY && yPressed < sq4EndY){selected = 4;}
if (xPressed > sq5StartX && xPressed < sq5EndX && yPressed > sq5StartY && yPressed < sq5EndY){selected = 5;}
if (xPressed > sq6StartX && xPressed < sq6EndX && yPressed > sq6StartY && yPressed < sq6EndY){selected = 6;}
if (xPressed > sq7StartX && xPressed < sq7EndX && yPressed > sq7StartY && yPressed < sq7EndY){selected = 7;}
if (xPressed > sq8StartX && xPressed < sq8EndX && yPressed > sq8StartY && yPressed < sq8EndY){selected = 8;}
Brain.Screen.setFillColor(vex::color::transparent);
Brain.Screen.printAt(sq1StartX,sq1StartY+20,true,"BR/RR");
Brain.Screen.printAt(sq1StartX,sq1StartY+40,true,"Corner");
Brain.Screen.printAt(sq2StartX,sq2StartY+20,true,"BL/RL");
Brain.Screen.printAt(sq2StartX,sq2StartY+40,true,"Corner");
Brain.Screen.printAt(sq3StartX,sq3StartY+20,true,"BR/RR");
Brain.Screen.printAt(sq3StartX,sq3StartY+40,true,"2Goal");
Brain.Screen.printAt(sq4StartX,sq4StartY+20,true,"BL/RL");
Brain.Screen.printAt(sq4StartX,sq4StartY+40,true,"2Goal");
Brain.Screen.printAt(sq5StartX,sq5StartY+20,true,"BR/RL");
Brain.Screen.printAt(sq5StartX,sq5StartY+40,true,"3Goal");
Brain.Screen.printAt(sq6StartX,sq6StartY+20,true,"BR/RL");
Brain.Screen.printAt(sq6StartX,sq6StartY+40,true,"CenterGoal");
Brain.Screen.printAt(sq7StartX,sq7StartY+20,true,"Skills");
Brain.Screen.printAt(sq7StartX,sq7StartY+40,true,"");
Brain.Screen.printAt(sq8StartX,sq8StartY+20,true,"BL/RL");
Brain.Screen.printAt(sq8StartX,sq8StartY+40,true,"SortCorner");
Brain.Screen.render();
}
wait(10);
} return(0);
}
void shooting(){
while (true ){
if (LineA.value(analogUnits::pct) < 70 ){
Indexer.stop();
Intakes.stop();
break;
}
else {
Index.spin(forward,100,pct);
Index2.spin(forward,100,pct);
Intakes.setVelocity(100,pct);
Intakes.spinFor(-1,turns);
}
}
}
void stoping(){
wait(1000,msec);
}
void autosort(){
while (true ){
if (LineB.value(analogUnits::pct) < 69 ){
Indexer.stop();
Intakes.stop();
break;
}
else {
Index.spin(forward,100,pct);
Index2.spin(reverse,100,pct);
Intakes.spin(reverse,50,pct);
}
}
}
void pre_auton( void ) {
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
vex::task LCDL(LCD);
}
/////////////////////////////////////////
//Code for PD loop///////////////////////
/////////////////////////////////////////
//Settings
double kP = 0.025;
double kD = 0;
double turnkP = 0.08;
double turnkD = 0;
//Autonomous Settings
int desiredValue;
int TruneValue;
int error; //SensorValue - DesiredValue : Position
int prevError; //Position 20 miliseconds ago
int derivative; // error - prevError : Speed
int turnError; //SensorValue - DesiredValue : Position
int turnPrevError; //Position 20 miliseconds ago
int turnDerivative; // error - prevError : Speed
bool resetDriveSensors = false;
//Variables modified for use
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);
BackLeftMotor.setPosition(0,degrees);
BackRightMotor.setPosition(0,degrees);
}
//Get the position of both motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
int backleftMotorPosition = BackLeftMotor.position(degrees);
int backrightMotorPosition = BackRightMotor.position(degrees); //I changed this. it was originally BackLeftMotor.position(degrees);
///////////////////////////////////////////
//Lateral movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the four motors
int averagePosition = (leftMotorPosition + rightMotorPosition + backleftMotorPosition + backrightMotorPosition )/4;
//Potential
error = desiredValue - averagePosition;//I also changed this. It was flipped around.
//Derivative
derivative = error - prevError;
double lateralMotorPower = error * kP + derivative * kD;
/////////////////////////////////////////////////////////////////////
///////////////////////////////////////////
//Turning movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the four motors
int turnDifference = (leftMotorPosition + backleftMotorPosition) - (rightMotorPosition + backrightMotorPosition);
//Potential
turnError = TruneValue - turnDifference;
//Derivative
turnDerivative = turnError - turnPrevError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD;
/////////////////////////////////////////////////////////////////////
LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BackLeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
BackRightMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
return 1;
}
/////////////////////////////////////////
void BR_RR_Corner(){//Blue Left, Red Right, Sorts corner goal
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(1850, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);//Drives forward to turn
LDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
Drive.rotateFor(1950, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,false);//Goes to corner goal
Intakes.rotateFor(-5000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(-1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(-800, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);//Drives forward to turn
Index.rotateFor(-2000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(-2000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(2000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
LDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
}
void BL_RL_Corner(){//Blue Right, Red Left, sort corner goal
Index2.rotateFor(3000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(3000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(200, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,false);//Backs away from goal
Index.rotateFor(3000, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//score/descores
Index2.rotateFor(-3000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(-3000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
LDrive.rotateFor(-180, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles towards goal
RDrive.rotateFor(180, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
Drive.rotateFor(300, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Backs away from goal
Index.rotateFor(-1500, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//score/descores
Index2.rotateFor(-500, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//score/descores
Index.rotateFor(700, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//score/descores
Index2.rotateFor(3000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
Drive.rotateFor(-1500, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Drives forward to turn
LDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
}
void BR_RR_2G(){//Blue Left, Red Right, scores/sorts in corner and wall goal
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(1850, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Drives forward to turn
LDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
Drive.rotateFor(1950, vex::rotationUnits::raw, 40, vex::velocityUnits::pct,false);//Goes to corner goal
Intakes.rotateFor(-5000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(-1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(-800, vex::rotationUnits::raw, 40, vex::velocityUnits::pct,true);//Drives forward to turn
Index.rotateFor(-2000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(-2000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(2000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
LDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
}
void BL_RL_2G(){//Blue Right, Red Left, scores corner and wall goal
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(1850, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);//Drives forward to turn
LDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
Drive.rotateFor(1950, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,false);//Goes to corner goal
Intakes.rotateFor(-5000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(-1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(-800, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);//Drives forward to turn
Index.rotateFor(-2000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(-2000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(2000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
LDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
}
void BL_RL_3G(){
vex::task billWiTheScienceFi(drivePID);
resetDriveSensors = true;
Indexer.setVelocity(100,pct);
Intakes.setVelocity(100,pct);
Indexer.spinFor(1,turns,false);
Intakes.spinFor(1,turns,true);
wait(500, msec);
resetDriveSensors = true;
desiredValue = 830;//830
wait(2000, msec);
resetDriveSensors = true;
TruneValue = 830;//830
resetDriveSensors = true;
desiredValue = 660;
wait(500, msec);
Indexer.spinFor(3,turns,false);
Intakes.spinFor(-1,turns,true);
wait(500, msec);
resetDriveSensors = true;
desiredValue = -670;
wait(5000, msec);
resetDriveSensors = true;
desiredValue = 1600;
wait(5000, msec);
resetDriveSensors = true;
autosort();
desiredValue = 650;
wait(500, msec);
Indexer.spinFor(3,turns,false);
Intakes.spinFor(-1,turns,true);
//resetDriveSensors = true;
//desiredValue = 2000;
///autosort();
///wait(1000, msec);
///Indexer.setVelocity(100,pct);
///Intakes.setVelocity(100,pct);
///Indexer.spinFor(5,turns,false);
///Intakes.spinFor(5,turns,true);
//wait(1000, msec);
//resetDriveSensors = true;
//desiredValue = 830;//830
///wait(5000, msec);
///resetDriveSensors = true;
///desiredValue = 670;
///wait(2000, msec);
///resetDriveSensors = true;
///desiredValue = 2000;
}
void BL_RL_2GC(){
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
Drive.rotateFor(1850, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Drives forward to turn
LDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
Drive.rotateFor(1950, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,false);//Goes to corner goal
Intakes.rotateFor(-5000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(-1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(-1950, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Drives forward to turn
LDrive.rotateFor(700, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(-700, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
Drive.rotateFor(1000, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Drives forward to turn
}
void Skills(){//Blue Left, Red Right, scores in corner, wall goal and center
enableDrivePID = true;
vex::task billWiTheScienceFi(drivePID);
resetDriveSensors = true;
TruneValue = 2000;
}
void BL_RL_Sort(){//Blue Right, Red Left, scores in corner and back center goal.
Index2.rotateFor(2000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(2000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(200, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,false);//Backs away from goal
Index.rotateFor(3000, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//score/descores
Index2.rotateFor(-3000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(-3000, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
LDrive.rotateFor(-180, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,false);//Angles towards goal
RDrive.rotateFor(180, vex::rotationUnits::raw, 70, vex::velocityUnits::pct,true);
Drive.rotateFor(300, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Backs away from goal
Index.rotateFor(-1500, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//score/descores
Index2.rotateFor(-500, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,true);//score/descores
Index.rotateFor(700, vex::rotationUnits::raw, 50, vex::velocityUnits::pct,false);//score/descores
Index2.rotateFor(3000, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,true);
Index.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);//scores
Index2.rotateFor(1800, vex::rotationUnits::raw, 100 , vex::velocityUnits::pct,false);
Intakes.rotateFor(-1800, vex::rotationUnits::raw, 100, vex::velocityUnits::pct,true);
Drive.rotateFor(-1950, vex::rotationUnits::raw, 30, vex::velocityUnits::pct,true);//Drives forward to turn
LDrive.rotateFor(500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,false);//Angles toward goal
RDrive.rotateFor(-500, vex::rotationUnits::raw, 20, vex::velocityUnits::pct,true);
}
void autonomous( void ) {
if (selected == 1){BR_RR_Corner();}
if (selected == 2){BL_RL_Corner();}
if (selected == 3){BR_RR_2G();}
if (selected == 4){BL_RL_2G();}
if (selected == 5){BL_RL_3G();}
if (selected == 6){BL_RL_2GC();}
if (selected == 7){Skills();}
if (selected == 8){BL_RL_Sort();}
}
void usercontrol( void ) {
enableDrivePID = false;
vex::task driveControl(driveControlTask);
enableDrivePID = false; //turns off pd loop for driver controll
while (1) {
Indexcontroll();
// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.
// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................
vex::task::sleep(20); //Sleep the task for a short amount of time to prevent wasted resources.
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
//Run the pre-autonomous function.
pre_auton();
//Set up callbacks for autonomous and driver control periods.
Competition.autonomous( autonomous );
Competition.drivercontrol( usercontrol );
//Prevent main from exiting with an infinite loop.
while(1) {
vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
}
}
Here is the code. Oh and just so you know there is only one auton that i have tested with. Don’t worry about the other ones. It is called BL BR three goal.