Autonomus Code

Autonomus Code is an important program that is used to control the robot outside of live matches.

## Functions

```
// // "normal" pid
// void go(int distance, int speed) {
//   frontright.setPosition(0, degrees);
//   frontleft.setPosition(0, degrees);
//   frontright.setVelocity(speed, percent);
//   frontleft.setVelocity(speed, percent);
//   while (frontright.position(degrees) < distance) {
//     frontleft.spin(forward, 100, pct);
//     frontright.spin(forward, 100, pct);
//   }
//   frontright.stop();
//   frontleft.stop();
// }
// // float prevError;
// // float kP = 1;
// // float kD = 1;
// // float ki = 1;
// // float go(float error) {
// //   float p = error*kP;
// //   int i = 0;
// //   double kP = 0.6;
// //   float d = (error-prevError)*kD;
// //   float prevError = error;
// //   while (abs(error) > i) {
// //     error = error - frontleft.position(degrees);
// //     p = error * kP;
// //     Brain.Screen.printAt(15, 15, "%f", frontleft.position(degrees));
// //     frontleft.spin(forward, p, pct);
// //     frontright.spin(forward, p, pct);
// //   }
// //   frontleft.stop();
// //   frontright.stop();
// //   Brain.Screen.printAt(15, 15, "%f", frontleft.position(degrees));
// //   d=(error-prevError)*kD;
// //   return p*d;
// // }
// // PID::PID(float kp, float ki, float kd) {
// //   PID::_kp = kp;
// //   PID::_ki = ki;
// //   PID::_kd = kd;
// // }
// float calculate(float error) {
//  float kp = 1;
//  float ki = 0;
//  float kd = 0;
//  float dt = 0.2;
//  float prevError = 0.0;
//  float integral = 0.0;
//    float p = error * kp;
//    if (error < 60 && error > 10) {
//      integral += error * dt;
//    } else {
//      integral = 0;
//    }
//    float i = integral * ki;
//    float d = (error - prevError) * kd;
//    d /= dt;
//    wait(dt, msec);
//    return p;
//    return i;
//    return d;
//  }
// void move(float setpoint) {
//   float error = setpoint - frontleft.position(degrees);
//   float power;
//   float i = 0;
//   double kP = 0.61;
//   while (abs(error) > i) {
//     error = setpoint - frontleft.position(degrees);
//     power = error * kP;
//     Brain.Screen.printAt(15, 15, "%f", frontleft.position(degrees));
//     Brain.Screen.printAt(30, 30, "%f", frontright.position(degrees));
//     frontleft.spin(forward, power, pct);
//     frontright.spin(forward, power, pct);
//   }
//   frontleft.stop();
//   frontright.stop();
//   Brain.Screen.clearScreen();
//   Brain.Screen.printAt(15, 15, "%f", frontleft.position(degrees));
//   Brain.Screen.printAt(30, 30, "%f", frontright.position(degrees));
//   }
// void l(float given) {
//   float error = given - frontleft.position(degrees);
//   float turn;
//   float i = 0;
//   double kP = 0.6;
//   while (abs(error) > i) {
//     error = given - frontleft.position(degrees);
//     turn = error * kP;
//     Brain.Screen.printAt(15, 15, "%f", frontleft.position(degrees));
//     Brain.Screen.printAt(30, 30, "%f", frontright.position(degrees));
//     frontleft.spin(forward, turn, pct);
//     frontright.spin(reverse, turn, pct);
//   }
//     frontleft.stop();
//     frontright.stop();
//     Brain.Screen.clearScreen();
//     Brain.Screen.printAt(15, 15, "%f", frontleft.position(degrees));
//     Brain.Screen.printAt(30, 30, "%f", frontright.position(degrees));
// }
// // float PID::calculate(float error) {
// // float kp = 1;
// // float ki = 0;
// // float kd = 0;
// // float dt = 0.2;
// // float prevError = 0.0;
// // float integral = 0.0;
// //   float p = error * kp;
// //   if (error < 60 && error > 10) {
// //     integral += error * dt;
// //   } else {
// //     integral = 0;
// //   }
// //   float i = integral * ki;
// //   float d = (error - prevError) * kd;
// //   d /= dt;
// //   wait(dt, msec);
// //   return p;
// //   return i;
// //   return d;
// //   return 0;
// // }
// // void zoomZ() {
// //   while(true) {
// //     Brain.Screen.setCursor(1,1);
// //   Brain.Screen.print("Odom value: %d", odom.position(degrees));
// //     wait(15, msec);
// //     Brain.Screen.clearScreen();
// //   }
// // }
// //   l(200);
// //   thread seventeen(zoomZ);
// // if (odom.position(degrees) > name) {
// //   Brain.Screen.print("positive");
// //   wait(.5,msec);
// //   Brain.Screen.clearScreen();
// // }
// //   while (true) {
// //     frontleft.setVelocity(odom + k, pct);
// //     frontleft.spin(forward, k + odom, pct);
// //     frontleft.spin(forward,10,pct);
// //     frontright.spin(forward,10,pct);
// //     Brain.Screen.printAt(15, 15, "Odom value: %d", odom.position(degrees));
// //     Brain.Screen.printAt(50, 50, "%f", frontleft.position(degrees));
// //     wait(15,msec);
// //   }
// //     frontleft.stop();
// //     frontright.stop();
// //     Brain.Screen.clearScreen();
// //obj: make motor speed inc/dec based on direction.
// //Itention will be for left spin decrease left side to stop drifting.
// //move to position
// //input x, input y, into main, then robot turns x, moves y
// void mtp(float x, float y) {
//   float a=90-atan2(x,y);
//   l(a);
//   move ((x+y));
// }
// //pt=position tracking
// float x;
// float y;
// // void pt() {
// //   while (true){
// //     x+=odom(degrees);
// //     y+=odom(degrees);
// //   }
// // }
// //theta is the output of your inertial/bots rotation (iner.rotation etc etc)
// void pt() {
//   while (true){
//   float rad = (iner.rotation(degrees) * 3.1415 / 180);
//   Brain.Screen.printAt(60, 60, "%f", odomy.position(degrees));
//   Brain.Screen.printAt(80, 80, "%f", odomx.position(degrees));
//   x+=(sin(rad) * odomy.position(degrees)) + (cos(rad) * odomx.position(degrees));
//   y+=(cos(rad) * odomy.position(degrees)) + (cos(rad) * odomx.position(degrees));
//   // odomy.setPosition(0,degrees);
//   // odomx.setPosition(0,degrees);
//   wait(20,msec);
//   Brain.Screen.clearScreen();
//   }
// }
// void intake(float setpoint) {
//   float error = setpoint - frontleft.position(degrees);
//   float power;
//   float i = 0;
//   double kP = 0.61;
//   while (abs(error) > i) {
//     error = setpoint - frontleft.position(degrees);
//     power = error * kP;
//     Brain.Screen.printAt(15, 15, "%f", inttemp.position(degrees));
//     inttemp.spin(forward, power, pct);
//   }
//   inttemp.stop();
//   Brain.Screen.printAt(15, 15, "%f", inttemp.position(degrees));
//   inttemp.setPosition(0,degrees);
// }
// float k=0.60999;
// int name=0;
// //  thread t (pt);
// //   mtp(-100,500);
// int main() {
//   // Initializing Robot Configuration. DO NOT REMOVE!
//   vexcodeInit();
//   frontleft.setStopping(hold);
//   frontright.setStopping(hold);
//    frontleft.setPosition(0,degrees);
//    frontright.setPosition(0,degrees);
//    l(200);
//     frontleft.setPosition(0,degrees);
//     frontright.setPosition(0,degrees);
//     wait (.1, seconds);
//     move(1300);
//       frontleft.setPosition(0,degrees);
//       frontright.setPosition(0,degrees);
//       wait (.1, seconds);
//       move(-800);
//         frontleft.setPosition(0,degrees);
//         frontright.setPosition(0,degrees);
//         wait (.1, seconds);
//         l(-200);
//           frontleft.setPosition(0,degrees);
//           frontright.setPosition(0,degrees);
//           wait (.1, seconds);
//           move(2100);
//             frontleft.setPosition(0,degrees);
//             frontright.setPosition(0,degrees);
//             wait (.1, seconds);
//             move(2100);
// }

//-----------------------------------------------------------------------------------------------------------------//

// MOVE TEST
void goo(int distance, int speed) {
backleft.setPosition(0, degrees);
backright.setPosition(0, degrees);
backright.setVelocity(speed, percent);
backleft.setVelocity(speed, percent);
while (backright.position(degrees) < distance) {
backleft.spin(forward, 100, pct);
backright.spin(forward, 100, pct);
}
}

// MOVE
void movefatty(float setpoint) {
float error = setpoint - backleft.position(degrees);
float power;
float i = 0;
double kP = 0.62;
while (abs(error) > i) {
error = setpoint - backleft.position(degrees);
power = error * kP;
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
Brain.Screen.printAt(30, 30, "%f", backright.position(degrees));
// frontleft.spin(forward, power, pct);
// frontright.spin(forward, power, pct);
backleft.spin(forward, power, pct);
backright.spin(forward, power, pct);
}
backleft.stop();
backright.stop();
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
Brain.Screen.printAt(30, 30, "%f", backright.position(degrees));
}

// MOVE 2
void bingus(float setpoint) {
float error = setpoint - backleft.position(degrees);
float power;
float i = 0;
double kP = 0.62;
while (abs(error) > i) {
error = setpoint - backleft.position(degrees);
power = error * kP;
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
backleft.spin(forward, power, pct);
backright.spin(forward, power, pct);
}
Brain.Screen.clearScreen();
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
backleft.resetPosition();
backright.resetPosition();
backleft.stop();
backright.stop();
}

// TURN
void turnfatty(float setpoint) {
float error = setpoint - backleft.position(degrees);
float power;
float i = 0;
double kP = 0.5;
while (abs(error) > i) {
error = setpoint - backleft.position(degrees);
power = error * kP;
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
backleft.spin(forward, power, pct);
backright.spin(reverse, power, pct);
}
Brain.Screen.clearScreen();
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
backleft.resetPosition();
backright.resetPosition();
backleft.stop();
backright.stop();
}

// TURN 3 ( JANKY / DISFUNCTIONAL )
void t3(float setpoint) {
float error = setpoint - enertial.heading(degrees);
float power;
float i = 0;
double kP = 0.5;
while (error > i) {
error = setpoint - enertial.heading(degrees);
power = error * kP;
Brain.Screen.printAt(15, 15, "%f", enertial.heading(degrees));
backleft.spin(forward, power, pct);
backright.spin(forward, power, pct);
}
Brain.Screen.clearScreen();
Brain.Screen.printAt(15, 15, "%f", enertial.heading(degrees));
backleft.resetPosition();
backright.resetPosition();
enertial.resetRotation();
backleft.stop();
backright.stop();
}

// TURN 2 (UNCHANGED)
void turn2(float setpoint) {
float error = setpoint - backleft.position(degrees);
float power;
float i = 0;
double kP = 0.619;
while (abs(error) > i) {
error = setpoint - backleft.position(degrees);
power = error * kP;
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
Brain.Screen.printAt(30, 30, "%f", backright.position(degrees));
// frontleft.spin(forward, power, pct);
// frontright.spin(reverse, power, pct);
backleft.spin(forward, power, pct);
backright.spin(reverse, power, pct);
}
// frontleft.stop();
// frontright.stop();
backleft.stop();
backright.stop();
Brain.Screen.printAt(15, 15, "%f", backleft.position(degrees));
Brain.Screen.printAt(30, 30, "%f", backright.position(degrees));
}

// DRIVE CODE
void tank() {
float deadzone1 = -10;
float deadzone3 = 10;
while (true) {
Controller1.Axis1.position() < deadzone1)) {
Brain.Screen.printAt(15, 15, "%d", Controller1.Axis2.position(percent));
Brain.Screen.printAt(30, 30, "%d", Controller1.Axis3.position(percent));
// frontleft.spin(forward, Controller1.Axis3.position(), percent);
// frontright.spin(forward, Controller1.Axis2.position(), percent);
backleft.spin(forward, Controller1.Axis3.position(), percent);
backright.spin(forward, Controller1.Axis2.position(), percent);
} else {
// frontleft.stop();
// frontright.stop();
backleft.stop();
backright.stop();
}
}
}

```

## Variants

### Match

* Offensive
* Defensive

```
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
enertial.calibrate();
frontleft.setStopping(brake);
frontright.setStopping(brake);
// ------------------------------------------------------
frontleft.resetPosition();
frontright.resetPosition();
// ------------------------------------------------------
// Turn right
turnfatty(235);
wait(.25, seconds);
// Score team acorn
bingus(-1500);
wait(.25, seconds);
// Back out of net
bingus(650);
wait(.25, seconds);
// Turn left
turnfatty(-225);
wait(.25, seconds);
// Collect acorn against centre bar
bingus(-2200);
wait(.25, seconds);
// Turn right
turnfatty(525);
wait(.25, seconds);
// Score acorn
bingus(-1500);
wait(.25, seconds);
// Back out of net
bingus(900);
wait(.25, seconds);
}

```