wddcat / vex-robot-control Goto Github PK

LeftMotor2.resetRotation();
while(wait && LeftMotor2.rotation(vex::rotationUnits::deg) > -dis1) {
move(-power, -power);
}

if(left){
  RightMotor2.resetRotation();
  while(wait && RightMotor2.rotation(vex::rotationUnits::deg) > -dis2) {
    move(-power * k, -power);
  }
}
else{
  LeftMotor2.resetRotation();
  while(wait && LeftMotor2.rotation(vex::rotationUnits::deg) > -dis2) {
    move(-power, -power *k);
  }
}

LeftMotor2.resetRotation();
while(wait && LeftMotor2.rotation(vex::rotationUnits::deg) > -dis3) {
  move(-power, -power);
}

if(left){
  LeftMotor2.resetRotation();
  while(wait && LeftMotor2.rotation(vex::rotationUnits::deg) > -dis4) {
    move(-power, -power * k);
  }
}
else{
  RightMotor2.resetRotation();
  while(wait && RightMotor2.rotation(vex::rotationUnits::deg) > -dis4) {
    move(-power * k, -power);
  }
}

move(-power, -power);
sleep(time);
stopch();

void auto2(){
spread();
Intake(0);
if(!rushForward(50, 630, 2000)) goto STOP;
turnRightWithGyroL(40, 30.2, 1000, true, 0.6, 0.0, 1);//32.5
Intake(35);
Lift(-5);
if(!rushForward(20, 375, 3300)) goto STOP;
Intake(100);
if(!rushForward(40, 300, 2500)) goto STOP;
Stop(hold);
// sleep(50);
// Intake(100);
turnRightWithGyro(70, 235.5, 1800, true, 0.33, 0.00001, 1.25);
Lift(0);
if(!rushForward(65, 700, 2500)) goto STOP;
turnLeftWithGyro(40, 117, 1400, true, 0.34, 0.0001, 1.7);
rushForward(60, 670, 1900);
turnRightWithGyro(65, 167.3, 1200, false);//171.3
Intake(0);
TrayMotor.startRotateTo(240, deg);
Move(40, 40);
sleep(400);
Move(-10, -10);
sleep(50);
Move(9, 9);
sleep(500);
Stop();
Tray(100, coast, 600);
Tray(80, coast, 800);
Tray(65, coast, 1100);
Tray(50);
sleep(100);
Tray(0);
Move(30, 30);
sleep(200);
Tray(0, coast);
Move(-100, -100);
sleep(700);
goto STOP;
STOP:
Intake(0);
Stop(coast);
Tray(0, coast);
return;
}

bool turnRightWithGyroL(int power, float target, float timeLimit, bool fullTime, float P, float I, float D){
ResetMotor();
float cur = GyroGetAngle();
float err = target - cur, last_err = 0, total_err = 0, delta_err = 0, OUT;
Brain.resetTimer();
while (Brain.timer(msec) < timeLimit) {
cur = GyroGetAngle();
last_err = err;
err = target - cur;
if(cur / target > KI_TURN_START_PERCENT)
total_err += err;
delta_err = err - last_err;
if(err <= 0.6) {
if(!fullTime) return true;
else if(err >= -0.6) return true;
}

OUT = P * err + I * total_err + D * delta_err;
if(fabs(OUT) < power * 0.1){
  if(OUT > 0) OUT = power * 0.1;
  else        OUT = -(power * 0.1);
}


LeftMotor1.spin(fwd, 3 * OUT, rpm);
LeftMotor2.spin(fwd, 3 * OUT, rpm);
RightMotor1.stop(hold);
RightMotor2.stop(hold);

sleep(10);

}
Stop(hold);
return false;
}

bool turnLeftWithGyroR(int power, float target, float timeLimit, bool fullTime, float P, float I, float D){
ResetMotor();
float cur = GyroGetAngle();
float err = target - cur, last_err = 0, total_err = 0, delta_err = 0, OUT;
Brain.resetTimer();
while (Brain.timer(msec) < timeLimit) {
cur = GyroGetAngle();
last_err = err;
err = target - cur;
if(cur / target > KI_TURN_START_PERCENT)
total_err += err;
delta_err = err - last_err;
if(err <= 0.6) {
if(!fullTime) return true;
else if(err >= -0.6) return true;
}

OUT = P * err + I * total_err + D * delta_err;
if(fabs(OUT) < power * 0.1){
  if(OUT > 0) OUT = power * 0.1;
  else        OUT = -(power * 0.1);
}


RightMotor1.spin(fwd, -(3 * OUT), rpm);
RightMotor2.spin(fwd, -(3 * OUT), rpm);
LeftMotor1.stop(hold);
LeftMotor2.stop(hold);

sleep(10);

}
Stop(hold);
return false;
}