PD Loop Help

Programming Support VEXcode Pro V5 Text Tech Support
1

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.

1 Like
2

You may want to post your code in a post, I don’t think people are going to download a zip file.

5 Likes
3

I would like to but don’t have internet net access at school to do it. I was rushed in the morning to post this so I’m going to have to figure something out.

4

Do you know how to add “code” to a post?

I mostly use images but they are a bit inconvenient for other people.
Any help will be appreciated.

5 
I think if you just add 4 spaces to the start of the line, it will automatically format it like this for long strings.

I’m not sure about the other way.

1 Like
6

Thank you for your help.

7

Include the [code] tag before your code, and the [/code] tag afterwards (each on their own line).

Like this:

[code]
Your code goes here!
More code!
Cooooooode!
[/code]
5 Likes
8

Thanks, I just got asked to change my screenshot for an actual code… This will help me a lot.

9 
\```
or you can use triple back ticks, or graves if you want to call them that
\```
3 Likes
11

Well, one thing I noticed so far is you can change those parts that say “potential” into proportional, because those are the proportional error calculations. Also, if you want to edit your post, removing that opcontrol looking code will help cut down the extra stuff we have to look through.

1 Like
12

Oh yah my bad. I will delete that and put it on the op.

13

Oh and as far as what’s actually wrong, it looks like you are setting the powers all in just a few lines, adding in the “forward” component at the same time as you add the “turn” component. You may need to simply add a clause to disable the straight line one when the tick values are different, since right now it is trying to run it as if it is a straight-line movement all the time. This should be where the interference is coming from.

1 Like
14

Look at Anomaly’s post from TP worlds called “Very Simple, Very Accurate Chassis Control” and you will see a good example that you can pull ideas from.

1 Like
15

What the ticks. As I said in the op the only auton that is using the pd is the 3 goal one. It looks different from all the others.
E: or am I mess enturping what your saying

16

The encoder measures rotation in ticks. Vex quad encoders have 360 ticks per revolution.

1 Like
17

Oh I understand what you mean now and how to fix it. Thank you. I might give an update on the new code to make sure I didn’t make any other stupid mistakes.

18

Ok, sounds good. I’m curious to know if that completely fixes it.

19

Are there ways to create those in blocks?

20

Not that I know of. Learning C/C++ will be useful both for Robotics and IRL though, so definitely consider the switch! :slight_smile:

21

Anything that can be done in text coding can be done in blocks, but that doesn’t mean it should be done in blocks. imo blocks is actually harder to work with then text code when you start getting to more complicated stuff, so I would highly recommend switching to a text-based programming language once you understand the fundamentals.

Also this thread is 3 years old, try not to revive old threads next time and make a new one instead.