Hello again,
I am reposting this from “PID Loop Help Requested” as I figured it might not have been looked at since it was marked as solved. Since the responses to that, we were able to implement our turning PID and achieve a consistent three-ring negative side autonomous! We’ve also tried to implement it into our positive side autonomous program, but have not had as much success. It still works, but seems to be less consistent than the negative side. We hypothesize that this has something to do with the positive auto requiring the robot to cross over the zero heading more than the negative auto, but are unsure of what exactly could be causing inconsistency. I have included our current code below, with about lines 100-160 being our turning PID code. If anyone has ideas on what may be causing this inconsistency or how to improve the code, help would be appreciated. Thanks!
#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include "vex.h"
using namespace vex;
// Brain should be defined by default
brain Brain;
// START V5 MACROS
#define waitUntil(condition) \
do { \
wait(5, msec); \
} while (!(condition))
#define repeat(iterations) \
for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS
// Robot configuration code.
// generating and setting random seed
void initializeRandomSeed(){
int systemTime = Brain.Timer.systemHighResolution();
double batteryCurrent = Brain.Battery.current();
double batteryVoltage = Brain.Battery.voltage(voltageUnits::mV);
// Combine these values into a single integer
int seed = int(batteryVoltage + batteryCurrent * 100) + systemTime;
// Set the seed
srand(seed);
}
void vexcodeInit() {
//Initializing random seed.
initializeRandomSeed();
}
// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
printf("VEXPlaySound:%s\n", soundName);
wait(5, msec);
}
#pragma endregion VEXcode Generated Robot Configuration
competition Competition;
controller Controller1 = controller(primary);
bumper swingarm = bumper(Brain.ThreeWirePort.B);
bumper MoGoDetection = bumper(Brain.ThreeWirePort.E);
digital_out CornerClearCylinder = digital_out(Brain.ThreeWirePort.F);
digital_out PtoCylinders = digital_out(Brain.ThreeWirePort.G);
digital_out MoGoClamp = digital_out(Brain.ThreeWirePort.H);
optical RingSensor = optical(PORT5);
inertial InertialL = inertial(PORT13, right);
motor FirstStageIntake = motor(PORT11, ratio6_1, false);
motor SecondStageIntake = motor(PORT12, ratio6_1, true);
motor leftMotorA = motor(PORT1, ratio6_1, true);
motor leftMotorB = motor(PORT3, ratio6_1, true);
motor_group leftDrive = motor_group(leftMotorA, leftMotorB);
motor PtoMotorLeft = motor(PORT2, ratio6_1, true);
motor rightMotorA = motor(PORT10, ratio6_1, false);
motor rightMotorB = motor(PORT7, ratio6_1, false);
motor_group rightDrive = motor_group(rightMotorA, rightMotorB);
motor PtoMotorRight = motor(PORT20, ratio6_1, false);
int Brain_precision = 0, Console_precision = 0, Controller1_precision = 0, Wheelencoder = 0;
float Pid = 0,Pidmuilt=0.5,piD,piDmuilt=0.021,pId,pIdmuilt =0.00,PrevPid;
double New_PID_Turning_Degrees;
bool RemoteControlCodeEnabled = true;
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;
bool Right = true, Left = false;
bool isPtoOnBase = true, isRedAlliance, isPos, isFirstStageOn = false, CornerClearer = false, HaltSecIntakeAtTop, doNotRunFirstStageB = false, PidOverride, XButton=true, AButton=true, ReverseIntakes=true, isMoGoClampUP=false;
//PIDTurning(Target,direction,the speed at which it moves before the PID,the time it moves before PID,amount of error that's allowed)
//For the third paramiter which is the speed it moves before PID, you should use 100 as long as it's not close to (within 5 degrees) of the destenation.
//For the fourth parmeter which is the time it moves before PID, use 0.1, unless the direction is importaint and it's not too close (within 10 degrees) to the target 0.2 maybe 0.3.
//Amount of error that's allowed in degrees 1 degree is fine but if more accuacy is needed it could go down to 0.5 maybe less but that's untested.
void PIDTurning(double PID_Turning_Degrees, bool Clockwise, int InitialSpeed, float InitailTime, float Bandwidth){
leftDrive.setVelocity(InitialSpeed, percent);
rightDrive.setVelocity(InitialSpeed, percent);
if((PID_Turning_Degrees>355||PID_Turning_Degrees<5)&& Clockwise){
PidOverride=true;
New_PID_Turning_Degrees=350;
}else if((PID_Turning_Degrees>355||PID_Turning_Degrees<5)&& !Clockwise){
PidOverride=true;
New_PID_Turning_Degrees=10;
}else{
New_PID_Turning_Degrees=PID_Turning_Degrees;
}
if(Clockwise){
leftDrive.spin(forward);
rightDrive.spin(reverse);
}else{
leftDrive.spin(reverse);
rightDrive.spin(forward);
}
wait(InitailTime,seconds);
while((InertialL.heading(degrees)<(PID_Turning_Degrees-(Bandwidth/2))) || (InertialL.heading(degrees)>(PID_Turning_Degrees+(Bandwidth/2)))){
while((InertialL.heading(degrees)<(PID_Turning_Degrees-(Bandwidth/2))) || (InertialL.heading(degrees)>(PID_Turning_Degrees+(Bandwidth/2)))){
//
if(PidOverride&&((InertialL.heading(degrees)<11)||(InertialL.heading(degrees)>349))){
leftDrive.setStopping(brake);
rightDrive.setStopping(brake);
leftDrive.setVelocity(20, rpm);
rightDrive.setVelocity(20, rpm);
if(Clockwise){
leftDrive.spin(forward);
rightDrive.spin(reverse);
}else{
leftDrive.spin(reverse);
rightDrive.spin(forward);
}
}else{
//
Pid = (New_PID_Turning_Degrees-InertialL.heading(degrees));
pId =pId+Pid;
piD=Pid-PrevPid;
float Turnspeed=((Pid*Pidmuilt)+(pId*pIdmuilt)+(piD*piDmuilt));
leftDrive.setVelocity(Turnspeed, percent);
rightDrive.setVelocity(Turnspeed, percent);
leftDrive.spin(forward);
rightDrive.spin(reverse);
PrevPid = Pid;
wait(10,msec);
}
}
if(PidOverride){
leftDrive.stop();
rightDrive.stop();
}else{
wait(20,msec);
}
}
}
void ButtonUp_pressed() {
isPtoOnBase = false;
PtoCylinders.set(true);
PtoMotorLeft.setStopping(coast);
PtoMotorRight.setStopping(coast);
PtoMotorLeft.setVelocity(100.0, percent);
PtoMotorRight.setVelocity(100.0, percent);
PtoMotorLeft.spin(forward);
PtoMotorRight.spin(forward);
while(Controller1.ButtonUp.pressing()){this_thread::sleep_for(10);}
PtoMotorLeft.stop();
PtoMotorRight.stop();
}
void ButtonDown_pressed() {
if (!isPtoOnBase) {
PtoMotorLeft.setVelocity(90.0, percent);
PtoMotorRight.setVelocity(90.0, percent);
PtoMotorLeft.spin(reverse);
PtoMotorRight.spin(reverse);
while(!swingarm.pressing()){this_thread::sleep_for(10);}
PtoMotorLeft.stop();
PtoMotorRight.stop();
isPtoOnBase = true;
PtoCylinders.set(false);
}
}
//go to loading position pls
void ButtonLeft_released() {
isPtoOnBase = false;
PtoCylinders.set(true);
PtoMotorLeft.setStopping(brake);
PtoMotorRight.setStopping(brake);
PtoMotorLeft.setVelocity(25.0, percent);
PtoMotorRight.setVelocity(25.0, percent);
PtoMotorLeft.spinFor(forward, .8, turns, false);
PtoMotorRight.spinFor(forward, .8, turns);
PtoMotorLeft.setVelocity(100.0, percent);
PtoMotorRight.setVelocity(100.0, percent);
}
void ButtonRight_pressed() {
SecondStageIntake.spin(forward, 90, percent);
}
void ButtonRight_released() {
SecondStageIntake.stop();
SecondStageIntake.setVelocity(50, percent);
SecondStageIntake.spinFor(reverse, 45, degrees);
SecondStageIntake.setVelocity(90, percent);
}
void ButtonR1_pressed() {
if (isFirstStageOn) {
FirstStageIntake.setVelocity(100,percent);
FirstStageIntake.spin(forward);
isFirstStageOn = false;
}
else {
FirstStageIntake.stop();
isFirstStageOn = true;
}
}
void ButtonR2_pressed() {
if (doNotRunFirstStageB) {
SecondStageIntake.setVelocity(90, percent);
SecondStageIntake.spin(forward);
doNotRunFirstStageB = false;
}
else {
SecondStageIntake.stop();
doNotRunFirstStageB = true;
}
}
// "when Controller1 ButtonL1 pressed" hat block
void ButtonL1_pressed() {
if (isMoGoClampUP) {
MoGoClamp.set(true);
isMoGoClampUP = false;
}
else {
MoGoClamp.set(false);
isMoGoClampUP = true;
}
}
// "when Controller1 ButtonL2 pressed" hat block
void ButtonL2_pressed() {
FirstStageIntake.spin(reverse);
SecondStageIntake.spin(reverse);
ReverseIntakes=false;
while(Controller1.ButtonL2.pressing()){this_thread::sleep_for(20);}
FirstStageIntake.stop();
SecondStageIntake.stop();
ReverseIntakes=true;
}
void ButtonB_pressed() {
if (!isFirstStageOn && !doNotRunFirstStageB) {
FirstStageIntake.stop();
SecondStageIntake.stop();
isFirstStageOn = true;
doNotRunFirstStageB = true;
}
else {
SecondStageIntake.setVelocity(100,percent);
FirstStageIntake.setVelocity(100,percent);
FirstStageIntake.spin(forward);
SecondStageIntake.spin(forward);
isFirstStageOn = false;
doNotRunFirstStageB = false;
}
}
void ButtonA_pressed() {
// Reverse the second stage of the intake
AButton=false;
SecondStageIntake.spin(reverse, 10.0, volt);
while(Controller1.ButtonA.pressing()){this_thread::sleep_for(20);}
AButton=true;
SecondStageIntake.stop();
}
// "when Controller1 ButtonX pressed" hat block
void ButtonX_pressed() {
// Reverse the first stage of the intake
XButton=false;
isFirstStageOn = false;
doNotRunFirstStageB = true;
FirstStageIntake.spin(reverse, 10.0, volt);
while(Controller1.ButtonX.pressing()){this_thread::sleep_for(20);}
XButton=true;
FirstStageIntake.stop();
doNotRunFirstStageB = false;
}
// "when Controller1 ButtonY pressed" hat block
void ButtonY_pressed() {
if (CornerClearer) {
CornerClearer = false;
CornerClearCylinder.set(false);
}
else {
CornerClearer = true;
CornerClearCylinder.set(true);
}
}
// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
while(true) {
if(RemoteControlCodeEnabled) {
// stop the motors if the brain is calibrating
if (InertialL.isCalibrating()) {
rightDrive.stop();
PtoMotorRight.stop();
leftDrive.stop();
PtoMotorLeft.stop();
while (InertialL.isCalibrating()) {
this_thread::sleep_for(25);
}
}
// calculate the drivetrain motor velocities from the controller joystick axies
// left = Axis3 + Axis1
// right = Axis3 - Axis1
int drivetrainLeftSideSpeed = Controller1.Axis3.position() + Controller1.Axis1.position();
int drivetrainRightSideSpeed = Controller1.Axis3.position() - Controller1.Axis1.position();
// check if the value is inside of the deadband range
if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
// check if the left motor has already been stopped
if (DrivetrainLNeedsToBeStopped_Controller1) {
// stop the left drive motor
leftDrive.stop();
if (isPtoOnBase){
PtoMotorLeft.stop();
}
// tell the code that the left motor has been stopped
DrivetrainLNeedsToBeStopped_Controller1 = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
DrivetrainLNeedsToBeStopped_Controller1 = true;
}
// check if the value is inside of the deadband range
if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
// check if the right motor has already been stopped
if (DrivetrainRNeedsToBeStopped_Controller1) {
// stop the right drive motor
rightDrive.stop();
if (isPtoOnBase){
PtoMotorRight.stop();
}
// tell the code that the right motor has been stopped
DrivetrainRNeedsToBeStopped_Controller1 = false;
}
} else {
// reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
DrivetrainRNeedsToBeStopped_Controller1 = true;
}
// only tell the left drive motor to spin if the values are not in the deadband range
if (DrivetrainLNeedsToBeStopped_Controller1) {
leftDrive.setVelocity(drivetrainLeftSideSpeed, percent);
leftDrive.spin(forward);
if (isPtoOnBase){
PtoMotorLeft.setVelocity(drivetrainLeftSideSpeed, percent);
PtoMotorLeft.spin(forward);
}
}
// only tell the right drive motor to spin if the values are not in the deadband range
if (DrivetrainRNeedsToBeStopped_Controller1) {
rightDrive.setVelocity(drivetrainRightSideSpeed, percent);
rightDrive.spin(forward);
if (isPtoOnBase){
PtoMotorRight.setVelocity(drivetrainRightSideSpeed, percent);
PtoMotorRight.spin(forward);
}
}
}
// wait before repeating the process
wait(20, msec);
}
return 0;
}
task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);
event IntakeReset = event();
event message1 = event();
// "when started" hat block
int whenStarted1() {
// PTO motor initializations
PtoMotorLeft.setMaxTorque(100.0, percent);
PtoMotorLeft.setStopping(coast);
PtoMotorRight.setMaxTorque(100.0, percent);
PtoMotorRight.setStopping(coast);
// Intake motor initializations
FirstStageIntake.setMaxTorque(100.0, percent);
FirstStageIntake.setStopping(coast);
FirstStageIntake.setVelocity(100.0, percent);
SecondStageIntake.setMaxTorque(100.0, percent);
SecondStageIntake.setStopping(coast);
SecondStageIntake.setVelocity(100.0, percent);
// Determines whether we are competing as Red or Blue alliance
Brain.Screen.setPenWidth(20);
Brain.Screen.setFont(prop30);
for(int i=0;i<30;i++){
Brain.Screen.setPenColor(white);
if (Brain.Screen.xPosition() < 240.0) {
Brain.Screen.setPenColor(green);
isRedAlliance = true;
}
Brain.Screen.setFillColor(red);
Brain.Screen.drawRectangle(30, 30, 170, 170);
Brain.Screen.setCursor(3, 4);
Brain.Screen.print("RED");
Brain.Screen.setPenColor(white);
if (Brain.Screen.xPosition() > 240.0) {
Brain.Screen.setPenColor(green);
isRedAlliance = false;
}
Brain.Screen.setFillColor(blue);
Brain.Screen.drawRectangle(260, 30, 170, 170);
Brain.Screen.setCursor(3, 20);
Brain.Screen.print("BLUE");
wait(0.25, seconds);
wait(5, msec);
}
Brain.Screen.clearScreen();
Brain.Screen.setFont(prop60);
for(int i=0;i<30;i++){
Brain.Screen.setPenColor(white);
if (Brain.Screen.xPosition() < 240.0) {
Brain.Screen.setPenColor(green);
isPos = true;
}
if (isRedAlliance) {
Brain.Screen.setFillColor(red);
}
else {
Brain.Screen.setFillColor(blue);
}
Brain.Screen.drawRectangle(30, 30, 170, 170);
Brain.Screen.setCursor(2, 4);
Brain.Screen.print("+");
Brain.Screen.setPenColor(white);
if (Brain.Screen.xPosition() > 240.0) {
Brain.Screen.setPenColor(green);
isPos = false;
}
Brain.Screen.drawRectangle(260, 30, 170, 170);
Brain.Screen.setCursor(2, 12);
Brain.Screen.print("_");
wait(0.25, seconds);
wait(5, msec);
}
Brain.Screen.clearScreen();
Brain.Screen.setFont(prop40);
Brain.Screen.setCursor(1, 10);
Brain.Screen.setFillColor(transparent);
Brain.Screen.print("OK\?");
if (isRedAlliance) {
Brain.Screen.setFillColor(red);
}
else {
Brain.Screen.setFillColor(blue);
}
Brain.Screen.setPenWidth(0);
Brain.Screen.drawRectangle(160, 105, 160, 30);
if (isPos) {
Brain.Screen.drawRectangle(220, 40, 30, 160);
}
while(!Brain.Screen.pressing()){this_thread::sleep_for(5);}
Brain.Screen.clearScreen();
wait(1.0, seconds);
Brain.Screen.print("calibrating");
InertialL.calibrate();
wait(6,seconds);
Brain.Screen.clearScreen();
if (isRedAlliance) {
Brain.Screen.setFillColor(red);
}
else {
Brain.Screen.setFillColor(blue);
}
Brain.Screen.drawRectangle(160, 105, 160, 30);
if (isPos) {
Brain.Screen.drawRectangle(220, 40, 30, 160);
}
return 0;
}
// "when driver control" hat block
int ondriver_drivercontrol_0() {
PtoCylinders.set(false);
leftMotorA.setStopping(coast);
leftMotorB.setStopping(coast);
PtoMotorLeft.setStopping(coast);
rightMotorA.setStopping(coast);
rightMotorB.setStopping(coast);
PtoMotorRight.setStopping(coast);
return 0;
}
void VEXcode_driver_task() {
// Start the driver control tasks....
vex::task drive0(ondriver_drivercontrol_0);
while(Competition.isDriverControl() && Competition.isEnabled()){this_thread::sleep_for(10);}
drive0.stop();
return;
}
int onauton_autonomous_0() {
leftDrive.setStopping(brake);
rightDrive.setStopping(brake);
if (isRedAlliance) {
if (!isPos) {
PtoCylinders.set(true);
MoGoClamp.set(false);
PtoMotorLeft.setVelocity(100.0, percent);
PtoMotorRight.setVelocity(100.0, percent);
leftDrive.setVelocity(20.0, percent);
rightDrive.setVelocity(20.0, percent);
leftDrive.spinFor(forward, 1.3, turns, false);
rightDrive.spinFor(forward, 1.3, turns, true);
PIDTurning(57, true, 100, 0.1,1);
leftDrive.setVelocity(20.0, percent);
rightDrive.setVelocity(20.0, percent);
leftDrive.spinFor(forward, 0.15, turns, false);
rightDrive.spinFor(forward, 0.15, turns, false);
PtoMotorLeft.spin(forward);
PtoMotorRight.spin(forward);
wait(0.5,seconds);
PtoMotorLeft.spin(reverse);
PtoMotorRight.spin(reverse);
while(!swingarm.pressing()){wait(20,msec);}
PtoMotorLeft.stop();
PtoMotorRight.stop();
leftDrive.setVelocity(60.0, percent);
rightDrive.setVelocity(60.0, percent);
leftDrive.spinFor(reverse, 5.8, turns, false);
rightDrive.spinFor(reverse, 5.8, turns, false);
wait(1, seconds);
MoGoClamp.set(true);
wait(0.2, seconds);
PIDTurning(165, false, 100, 0.2,2);
FirstStageIntake.spin(forward, 10.0, volt);
SecondStageIntake.spin(forward);
leftDrive.setVelocity(70.0, percent);
rightDrive.setVelocity(70.0, percent);
leftDrive.spinFor(forward, 3, turns, false);
rightDrive.spinFor(forward, 3, turns, true);
leftDrive.spinFor(reverse, 2.3, turns, false);
rightDrive.spinFor(reverse, 2.3, turns, true);
PIDTurning(125, false, 100, 0.2,1);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spin(forward);
rightDrive.spin(forward);
wait(1.7, seconds);
PIDTurning(140, true, 100, 0.1,1);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spinFor(reverse, 7.5, turns, false);
rightDrive.spinFor(reverse, 7.5, turns, false);
wait(1,seconds);
FirstStageIntake.stop();
SecondStageIntake.stop();
}
}
if (!isRedAlliance) {
if (!isPos) {
PtoCylinders.set(true);
MoGoClamp.set(false);
PtoMotorLeft.setVelocity(100.0, percent);
PtoMotorRight.setVelocity(100.0, percent);
leftDrive.setVelocity(20.0, percent);
rightDrive.setVelocity(20.0, percent);
leftDrive.spinFor(forward, 1.4, turns, false);
rightDrive.spinFor(forward, 1.4, turns, true);
PIDTurning(305, false, 100, 0.1,1);
leftDrive.setVelocity(20.0, percent);
rightDrive.setVelocity(20.0, percent);
leftDrive.spinFor(forward, 0.15, turns, false);
rightDrive.spinFor(forward, 0.15, turns, false);
PtoMotorLeft.spin(forward);
PtoMotorRight.spin(forward);
wait(0.5,seconds);
PtoMotorLeft.spin(reverse);
PtoMotorRight.spin(reverse);
while(!swingarm.pressing()){wait(20,msec);}
PtoMotorLeft.stop();
PtoMotorRight.stop();
leftDrive.setVelocity(60.0, percent);
rightDrive.setVelocity(60.0, percent);
leftDrive.spinFor(reverse, 5.8, turns, false);
rightDrive.spinFor(reverse, 5.8, turns, false);
wait(1, seconds);
MoGoClamp.set(true);
wait(0.2, seconds);
PIDTurning(195, true, 100, 0.2,2);
FirstStageIntake.spin(forward, 10.0, volt);
SecondStageIntake.spin(forward);
leftDrive.setVelocity(70.0, percent);
rightDrive.setVelocity(70.0, percent);
leftDrive.spinFor(forward, 3, turns, false);
rightDrive.spinFor(forward, 3, turns, true);
leftDrive.spinFor(reverse, 2, turns, false);
rightDrive.spinFor(reverse, 2, turns, true);
PIDTurning(235, true, 100, 0.2,1);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spin(forward);
rightDrive.spin(forward);
wait(1.7, seconds);
PIDTurning(225, false, 100, 0.1,1);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spinFor(reverse, 7.5, turns, false);
rightDrive.spinFor(reverse, 7.5, turns, false);
wait(1,seconds);
FirstStageIntake.stop();
SecondStageIntake.stop();
}
if (isPos) {
PtoCylinders.set(true);
MoGoClamp.set(false);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spinFor(reverse, 4.1, turns, false);
rightDrive.spinFor(reverse, 4.1, turns, true);
PIDTurning(32.5, true, 100, 0.3, 1);
leftDrive.setVelocity(30, percent);
rightDrive.setVelocity(30, percent);
//this is the closest to the line v
leftDrive.spinFor(reverse, 1.85, turns, false);
rightDrive.spinFor(reverse, 1.85, turns, true);
//this is the closest to the line ^
MoGoClamp.set(true);
wait(0.1, seconds);
leftDrive.spinFor(reverse, 0.48, turns, false);
rightDrive.spinFor(forward, 0.48, turns, true);
SecondStageIntake.spin(forward, 12.7, volt);
FirstStageIntake.spin(forward, 12.7, volt);
wait(0.1, seconds);
leftDrive.setVelocity(60, percent);
rightDrive.setVelocity(60, percent);
leftDrive.spinFor(forward, 5.4, turns, false);
rightDrive.spinFor(forward, 5.4, turns, false);
wait(2,seconds);
MoGoClamp.set(false);
wait(0.5,seconds);
PIDTurning(46, false, 100, 0.3, 0.75);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spin(reverse);
rightDrive.spin(reverse);
wait(0.54, seconds);
MoGoClamp.set(true);
wait(0.1, seconds);
leftDrive.stop();
rightDrive.stop();
PIDTurning(45, true, 100, 0.1, 1);
FirstStageIntake.spin(forward, 12.7, volt);
SecondStageIntake.spin(forward, 12.7, volt);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spin(forward);
rightDrive.spin(forward);
wait(2,seconds);
leftDrive.spinFor(reverse, 1, turns, false);
rightDrive.spinFor(reverse, 1, turns, true);
PIDTurning(45, false, 100, 0.1, 1);
leftDrive.setStopping(coast);
rightDrive.setStopping(coast);
leftDrive.setVelocity(100, percent);
rightDrive.setVelocity(100, percent);
leftDrive.spinFor(reverse, 6.2, turns, false);
rightDrive.spinFor(reverse, 6.2, turns, false);
}
}
return 0;
}
void VEXcode_auton_task() {
// Start the auton control tasks....
vex::task auto0(onauton_autonomous_0);
while(Competition.isAutonomous() && Competition.isEnabled()){
// this_thread::sleep_for(10);
// while(InertialL.gyroRate(yaxis,dps) != 0){
// //AS Stands for Angular Speed in degrees per millisecond
// AS += InertialL.gyroRate(yaxis,dps)*200;
// this_thread::sleep_for(5);
// }
// while(APS != 0){
// //AP Stands for Angular Postion in degrees
// AP += APS*1000;
// this_thread::sleep_for(5);
// }
// InertialL.acceleration(xaxis)/9.81;
}
auto0.stop();
return;
}
int main() {
vex::competition::bStopTasksBetweenModes = false;
Competition.autonomous(VEXcode_auton_task);
Competition.drivercontrol(VEXcode_driver_task);
Controller1.ButtonUp.pressed(ButtonUp_pressed);
Controller1.ButtonDown.pressed(ButtonDown_pressed);
Controller1.ButtonLeft.released(ButtonLeft_released);
Controller1.ButtonRight.pressed(ButtonRight_pressed);
Controller1.ButtonRight.released(ButtonRight_released);
Controller1.ButtonR1.pressed(ButtonR1_pressed);
Controller1.ButtonR2.pressed(ButtonR2_pressed);
Controller1.ButtonL1.pressed(ButtonL1_pressed);
//Controller1.ButtonL1.released(ButtonL1_released);
Controller1.ButtonL2.pressed(ButtonL2_pressed);
Controller1.ButtonB.pressed(ButtonB_pressed);
Controller1.ButtonA.pressed(ButtonA_pressed);
Controller1.ButtonX.pressed(ButtonX_pressed);
Controller1.ButtonY.pressed(ButtonY_pressed);
wait(30, msec);
whenStarted1();
}