Autonomous_1_11040A

#include "robot-config.h"

//Max Points: 6 pts
//Min Points: 3 pts
//Starting Point: BS1
//Language: Vex C++
          
void moveForward(int speed){
    if speed == 0 {
        LeftFront.stop();
        LeftBack.stop();
        RightFront.stop();
        RightBack.stop();
    }
    else {
    //Spin motors in fwd direciton; set speed to 50rpm
    LeftFront.spin(directionType::fwd,speed,velocityUnits::rpm);
    LeftBack.spin(directionType::fwd,speed,velocityUnits::rpm);
    RightFront.spin(directionType::fwd,speed,velocityUnits::rpm);
    RightBack.spin(directionType::fwd,speed,velocityUnits::rpm);
    }
}

void moveBack(int speed){
    if speed == 0 {
        LeftFront.stop();
        LeftBack.stop();
        RightFront.stop();
        RightBack.stop();
    }
    else {
    //Spin motors in reverse direciton; set speed to 50rpm
    LeftFront.spin(directionType::rev,speed,velocityUnits::rpm);
    LeftBack.spin(directionType::rev,speed,velocityUnits::rpm);
    RightFront.spin(directionType::rev,speed,velocityUnits::rpm);
    RightBack.spin(directionType::rev,speed,velocityUnits::rpm);
    }
} 

void turnRight(int speed){
    //Spin motors in fwd direciton; set velocity to 50rpm
    LeftFront.spin(directionType::fwd,-speed,velciotyUnits::rpm);
    LeftBack.spin(directionType::fwd,-speed,velciotyUnits::rpm);
    RightFront.spin(directionType::fwd,speed,velocityUnits::rpm);
    RightBack.spin(directionType::fwd,speed,velocityUnits::rpm);
} 

void turnLeft(int speed){
    //Spin motors in fwd direciton; set velocity to 50rpm
    LeftFront.spin(directionType::fwd,speed,velciotyUnits::rpm);
    LeftBack.spin(directionType::fwd,speed,velciotyUnits::rpm);
    RightFront.spin(directionType::fwd,-speed,velciotyUnits::rpm);
    RightBack.spin(directionType::fwd,-speed,velciotyUnits::rpm);
}

void distance(){
    double pi == 3.1415926535897;   
    return 4*pi*(RightEncoder.rotation(rotationUnits::deg)/360);
}

int main() {
   //Resets encoder values sensor values 
   LeftEncoder.resetRotation();
   RightEncoder.resetRotation();
    
   //Picks up ball
   Intake.spin(directionType::fwd,100,velocityUnits::rpm);
   
   //Turn 90 degrees clockwise towards BF2
   while (abs(Gyro.value()) < 880){
       turnRight(100);
   }
   turnRight(0);
   
   /*
   //In case Gyro doesn't work
   turnRight(100);
   //Wait 3 seconds
   task::sleep(3000);
   //Turn off motors
   turnRight(0);
   */
   
   //Shoots ball at BF2 (2 points)
   Intake.spin(directionType::fwd,100,velocityUnits::rpm);
   Intake.spin(directionType::fwd,0,velocityUnits::rpm);
   
   //Move forward towards BF1, hit BF1 (1 point)
   while(abs(distance() < 100) {
       moveForward(100);
   }
   moveForward(0);
   
   //Move backwards, until in line with BPL
   while(abs(distance() < 42)
       moveBack(100);
   }
   moveBack(0);  //Resets driver motors to 0

   //Set gyro sensor value to 0 (found no function in vcs command reference for resetting gyro value)
       
   //Turn 90 degrees counter-clockwise
   while(abs(Gyro.value() < 880){
       leftTurn(100);
   }
   leftTurn(0); //Resets drive motors to 0
   
   /*
   //In case Gyro doesn't work
   turnLeft(100);
   //Wait 3 seconds
   task::sleep(3000);
   //Turn off motors
   turnLeft(0);
   */
   
   //Move backward onto BPL(3 points)
   moveBack(100);
   task::sleep(3000);
   moveBack(0);
}