Code Listing

The complete (and messy) code we used for the competition is below.

#include <Arduino.h>

// #define ENCODER_OPTIMIZE_INTERRUPTS

#include <Encoder.h>

#include <Servo.h>

#include <Metro.h>

#include "ICM_20948.h"

//#define USE_TIMER_1     true

#define LEFT_ENABLE 0

#define LEFT_A 1

#define LEFT_B 2

Encoder leftEnc(14, 15);

#define RIGHT_ENABLE 3

#define RIGHT_A 4

#define RIGHT_B 5

Encoder rightEnc(18, 19);

#define BACK_ENABLE 6

#define BACK_A 7

#define BACK_B 8

Encoder backEnc(16, 17);

#define SHOOTER_ENABLE 9

#define SHOOTER_A 10

#define SHOOTER_B 11

Encoder shooterEnc(20, 21);

#define RPM_TUNER A8

#define SERVO_PIN 12

Servo intake;

#define CLOSED 50

#define OPEN 180

#define IR_SENSOR A9

// #define BUTTON_PIN 22

#define ENABLE_GOOD_PIN 13

#define ENABLE_BAD_PIN 28

#define WIRE_PORT Wire2

ICM_20948_I2C imu;

#define BASELINE .127 // meters, wheel to COM

#define COUNTS_PER_REV 1920 // 64 counts per motor rev * 30 gear ratio

#define RADIUS .041275

#define DIAM 0.08255 // meters

const double CIRCUM = M_PI * DIAM;

const double COUNTS_PER_METER = COUNTS_PER_REV / CIRCUM;

const double COUNTS_PER_RAD = COUNTS_PER_REV / 2 / M_PI;

#define SHOOTER_COUNTS_PER_REV 103.76

#define SCAN_TIME 5000

Metro scanTimer = Metro(SCAN_TIME);

Metro gameTimer = Metro(130000);

enum RobotState {

 WAITING,

 ROTATING_TO_TARGET_VOLTAGE,

 FINDING_TARGET_VOLTAGE,

 DRIVING_TO_CORNER,

 DRIVING_TO_SHOOTING_POSE,

 DONE

};

const float BODY_TO_WHEEL[3][3] = {

 {-.5, -.866, BASELINE},

 {-.5, .866, BASELINE},

 {1, 0, BASELINE},

};

const float WHEEL_TO_BODY[3][3] = {

 {-.333, -.333, .667},

 {-.57735, .57735, 0},

 {1.3123, 1.3123, 1.3123}

};

#define GOOD_RPM 1650 // far 1860

#define BAD_RPM 1000

int lastDriveTime = 0;

int lastShooterTime = 0;

int lastYawTime = 0;

double yawInit = 0;

elapsedMillis elapsedTime;

const double maxLQRErr[3] = {0.05, 0.05, 0.0872};  // (m, m, rad => 5 degrees)

void setup() {

 // put your setup code here, to run once:

 // pinMode(LED_BUILTIN, OUTPUT);

 // digitalWrite(LED_BUILTIN, HIGH);

 pinMode(A9, INPUT_PULLDOWN);

 pinMode(A8, INPUT);

 pinMode(LEFT_ENABLE, OUTPUT);

 pinMode(LEFT_A, OUTPUT);

 pinMode(LEFT_B, OUTPUT);

 pinMode(RIGHT_ENABLE, OUTPUT);

 pinMode(RIGHT_A, OUTPUT);

 pinMode(RIGHT_B, OUTPUT);

 pinMode(BACK_ENABLE, OUTPUT);

 pinMode(BACK_A, OUTPUT);

 pinMode(BACK_B, OUTPUT);

 pinMode(SHOOTER_ENABLE, OUTPUT);

 pinMode(SHOOTER_A, OUTPUT);

 pinMode(SHOOTER_B, OUTPUT);

  // pinMode(BUTTON_PIN, INPUT_PULLDOWN);

 pinMode(ENABLE_GOOD_PIN, INPUT_PULLDOWN);

 pinMode(ENABLE_BAD_PIN, INPUT_PULLDOWN);

 intake.attach(SERVO_PIN, 900, 2100);

 intake.write(CLOSED);

 Serial.begin(115200);

 WIRE_PORT.begin();

 WIRE_PORT.setClock(400000);

 // Initialize the ICM-20948

 // If the DMP is enabled, .begin performs a minimal startup. We need to configure the sample mode etc. manually.

 imu.begin(WIRE_PORT, 1);

 Serial.print(F("Initialization of the sensor returned: "));

 Serial.println(imu.statusString());

 if (imu.status != ICM_20948_Stat_Ok)

 {

   Serial.println(F("IMU failed to init"));

 }

 bool success = true; // Use success to show if the DMP configuration was successful

 // Initialize the DMP. initializeDMP is a weak function. You can overwrite it if you want to e.g. to change the sample rate

 success &= (imu.initializeDMP() == ICM_20948_Stat_Ok);

 // DMP sensor options are defined in ICM_20948_DMP.h

 //    INV_ICM20948_SENSOR_ACCELEROMETER               (16-bit accel)

 //    INV_ICM20948_SENSOR_GYROSCOPE                   (16-bit gyro + 32-bit calibrated gyro)

 //    INV_ICM20948_SENSOR_RAW_ACCELEROMETER           (16-bit accel)

 //    INV_ICM20948_SENSOR_RAW_GYROSCOPE               (16-bit gyro + 32-bit calibrated gyro)

 //    INV_ICM20948_SENSOR_MAGNETIC_FIELD_UNCALIBRATED (16-bit compass)

 //    INV_ICM20948_SENSOR_GYROSCOPE_UNCALIBRATED      (16-bit gyro)

 //    INV_ICM20948_SENSOR_STEP_DETECTOR               (Pedometer Step Detector)

 //    INV_ICM20948_SENSOR_STEP_COUNTER                (Pedometer Step Detector)

 //    INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR        (32-bit 6-axis quaternion)

 //    INV_ICM20948_SENSOR_ROTATION_VECTOR             (32-bit 9-axis quaternion + heading accuracy)

 //    INV_ICM20948_SENSOR_GEOMAGNETIC_ROTATION_VECTOR (32-bit Geomag RV + heading accuracy)

 //    INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD           (32-bit calibrated compass)

 //    INV_ICM20948_SENSOR_GRAVITY                     (32-bit 6-axis quaternion)

 //    INV_ICM20948_SENSOR_LINEAR_ACCELERATION         (16-bit accel + 32-bit 6-axis quaternion)

 //    INV_ICM20948_SENSOR_ORIENTATION                 (32-bit 9-axis quaternion + heading accuracy)

 // Enable the DMP Game Rotation Vector sensor

 success &= (imu.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok);

 // Configuring DMP to output data at multiple ODRs:

 // DMP is capable of outputting multiple sensor data at different rates to FIFO.

 // Setting value can be calculated as follows:

 // Value = (DMP running rate / ODR ) - 1

 // E.g. For a 5Hz ODR rate when DMP is running at 55Hz, value = (55/5) - 1 = 10.

 success &= (imu.setDMPODRrate(DMP_ODR_Reg_Quat6, 1) == ICM_20948_Stat_Ok); // Set to the maximum

 //success &= (imu.setDMPODRrate(DMP_ODR_Reg_Accel, 0) == ICM_20948_Stat_Ok); // Set to the maximum

 //success &= (imu.setDMPODRrate(DMP_ODR_Reg_Gyro, 0) == ICM_20948_Stat_Ok); // Set to the maximum

 //success &= (imu.setDMPODRrate(DMP_ODR_Reg_Gyro_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum

 //success &= (imu.setDMPODRrate(DMP_ODR_Reg_Cpass, 0) == ICM_20948_Stat_Ok); // Set to the maximum

 //success &= (imu.setDMPODRrate(DMP_ODR_Reg_Cpass_Calibr, 0) == ICM_20948_Stat_Ok); // Set to the maximum

 // Enable the FIFO

 success &= (imu.enableFIFO() == ICM_20948_Stat_Ok);

 // Enable the DMP

 success &= (imu.enableDMP() == ICM_20948_Stat_Ok);

 // Reset DMP

 success &= (imu.resetDMP() == ICM_20948_Stat_Ok);

 // Reset FIFO

 success &= (imu.resetFIFO() == ICM_20948_Stat_Ok);

   // Check success

 if (success)

 {

   Serial.println(F("DMP enabled!"));

 }

 else

 {

   Serial.println(F("Enable DMP failed!"));

   Serial.println(F("Please check that you have uncommented line 29 (#define ICM_20948_USE_DMP) in ICM_20948_C.h..."));

 }

 Serial.println("Started robot!");

 Serial.print("COUNTS PER METER: ");

 Serial.println(COUNTS_PER_METER);

 Serial.print("COUNTS PER RAD: ");

 Serial.println(COUNTS_PER_RAD);

 // while (!digitalRead(BUTTON_PIN)) {

 //   delay(50);

 // }

 while (!(digitalRead(ENABLE_GOOD_PIN) || digitalRead(ENABLE_BAD_PIN))) {

   delay(50);

 }

 scanTimer.reset();

 gameTimer.reset();

 lastYawTime = micros();

 lastDriveTime = micros();

 elapsedTime = elapsedMillis();

 // for (elapsedMillis t; t < 2000;) {

 //   Serial.print("Time: ");

 //   Serial.println(t);

 //   if (t < 1200) {

 //     driveVectorPID(.5, -.1, 0);

 //   }

 //   else if (t < 1450) {

 //     driveVectorPID(0, .5, 0);

 //   }

 //   else if (t < 1850) {

 //     driveVectorPID(0, 0, M_PI * .6);

 //   }

 //   else {

 //     drive(0, 0, 0);

 //   }

 //   delay(20);

 // }

 // for (elapsedMillis t; t < 7000;) {

 //   if (t < 2000) {

 //     runShooterGood();

 //   }

 //   else if (t < 3500) {

 //     runShooterGood();

 //     servoPos = OPEN;

 //     intake.write(servoPos);

 //   }

 //   else if (t > 7000) {

 //     turnOffShooter();

 //   }

 //   delay(20);

 // }

//  Serial.println(BOARD_TYPE);

//  Serial.println(TIMER_INTERRUPT_VERSION);

//  Serial.print(F("CPU Frequency = ")); Serial.print(F_CPU / 1000000); Serial.println(F(" MHz"));

}

int mode = 0;

int positionRight = 0;

int positionLeft = 0;

int positionBack = 0;

int positionShooter = 0;

int lastShooterPos = 0;

double lastShooterErr = 0;

double shooter_kp = 5.0;

double shooter_kd = 1.0;

int shooterPID(double goalRPM) {

 unsigned curTime = micros();

 double delta = (curTime - lastShooterTime) / 1000000.0; // seconds

 int newPosition = shooterEnc.read();

 double curVel = (newPosition - lastShooterPos) / delta;

 curVel = curVel * 60.0 / SHOOTER_COUNTS_PER_REV; // convert to RPM

 double err = goalRPM - curVel;

 double err_d = (err - lastShooterErr) / delta * 60.0 / SHOOTER_COUNTS_PER_REV;

 double power = shooter_kp * err + shooter_kd * err_d;

 if (power < 0) {

   power = 0;

 }

 else if (power > 256) {

   power = 256;

 }

 // Serial.print("curTime: ");

 // Serial.print(curTime);

 // Serial.print(" curPos: ");

 // Serial.print(newPosition);

 // Serial.print(" lastPos: ");

 // Serial.println(lastShooterPos);

 lastShooterPos = newPosition;

 lastShooterTime = curTime;

 lastShooterErr = err;

 // Serial.print("Delta: ");

 // Serial.print(delta);

 // Serial.print(" Vel: ");

 // Serial.print(curVel);

 // Serial.print(" Err: ");

 // Serial.print(err);

 // Serial.print(" Power: ");

 // Serial.print(power);

 // Serial.print(" ");

 // Serial.println((int) power);

 return (int) power;

}

// provide duty cycle in [-1, 1], order: left right back

void drive(double p1, double p2, double p3) {

 // PWM is clipped between 0 and 255

 int pwm1 = constrain(abs(p1) * 255, 0, 255);

 int pwm2 = constrain(abs(p2) * 255, 0, 255);

 int pwm3 = constrain(abs(p3) * 255, 0, 255);

 // find the maximum PWM value and scale all PWMs down if necessary

 int maxPWM = max(pwm1, max(pwm2, pwm3));

 // find scaling factor

 double scale = 1.0;

 if (maxPWM >= 230) {

   scale = 230.0 / maxPWM;

 }

 pwm1 *= scale;

 pwm2 *= scale;

 pwm3 *= scale;

 // Serial.print("Power: left: ");

 // Serial.print(p1);

 // Serial.print(" right: ");

 // Serial.print(p2);

 // Serial.print(" back: ");

 // Serial.println(p3);

 // Serial.print("PWM: left: ");

 // Serial.print(pwm1);

 // Serial.print(" right: ");

 // Serial.print(pwm2);

 // Serial.print(" back: ");

 // Serial.println(pwm3);

 // Default to hard brake when power is 0

 int left_a = HIGH, left_b = HIGH, right_a = HIGH, right_b = HIGH, back_a = HIGH, back_b = HIGH;

 if (p1 > 0) {

   left_a = HIGH;

   left_b = LOW;

 }

 else if (p1 < 0) {

   left_a = LOW;

   left_b = HIGH;

 }

 if (p2 > 0) {

   right_a = HIGH;

   right_b = LOW;

 }

 else if (p2 < 0) {

   right_a = LOW;

   right_b = HIGH;

 }

 if (p3 > 0) {

   back_a = HIGH;

   back_b = LOW;

 }

 else if (p3 < 0) {

   back_a = LOW;

   back_b = HIGH;

 }

 analogWrite(LEFT_ENABLE, pwm1);

 analogWrite(RIGHT_ENABLE, pwm2);

 analogWrite(BACK_ENABLE, pwm3);

 digitalWrite(LEFT_A, left_a);

 digitalWrite(LEFT_B, left_b);

 digitalWrite(RIGHT_A, right_a);

 digitalWrite(RIGHT_B, right_b);

 digitalWrite(BACK_A, back_a);

 digitalWrite(BACK_B, back_b);

}

void drive(double power[3]) {

 drive(power[0], power[1], power[2]);

}

// order: left, right, back

// float DRIVE_KP[3] = {.18, .18, .18};

float DRIVE_KP[3] = {.5, .5, .7};

float DRIVE_KI[3] = {.00, .00, .00};

float DRIVE_KD[3] = {.0, .0, .0};

int lastDrivePos[3];

double lastDriveErr[3];

double driveErrInt[3];

double lastGlobalPose[3] = {0, 0, 0};  // (x, y, yaw)

double filteredVel[3];

#define EMA_ALPHA .75

double filterVelEMA(double vel, double lastVel, double alpha) {

 // implement an exponential moving average filter

 return alpha * vel + (1 - alpha) * lastVel;

}

// provide wheel angular velocity, order: left, right, back

void drivePID(double w[3]) {

 unsigned long curTime = micros();

 double delta = (curTime - lastDriveTime) / 1000000.0; // seconds

 // Angular velocity in rad/s

 int curPos[3] = {leftEnc.read(), rightEnc.read(), backEnc.read()};

 double curVel[3];

 double curErr[3];

 double power[3];

 // Serial.print("Err deriv ");

 for (int i = 0; i < 3; i++) {

   curVel[i] = (curPos[i] - lastDrivePos[i]) / delta / COUNTS_PER_RAD; // rad/s

   filteredVel[i] = filterVelEMA(curVel[i], filteredVel[i], EMA_ALPHA);

   curErr[i] = w[i] - filteredVel[i];

   driveErrInt[i] += curErr[i] * delta;

   if (abs(driveErrInt[i]) > 500) {

     driveErrInt[i] = 0;

   }

   double deriv = (curErr[i] - lastDriveErr[i]) / delta;

   deriv = constrain(deriv, -10, 10);

   power[i] = DRIVE_KP[i] * curErr[i] + DRIVE_KI[i] * driveErrInt[i] + DRIVE_KD[i] * deriv;

   // Serial.print(i);

   // Serial.print(": ");

   // Serial.print(deriv);

   // Serial.print(" ");

   lastDrivePos[i] = curPos[i];

   lastDriveErr[i] = curErr[i];

 }

 // Serial.println();

 // update last global pose

 double curBodyVel[3];

 for (int i = 0; i < 3; i++) {

   curBodyVel[i] = 0;

   for (int j = 0; j < 3; j++) {

     curBodyVel[i] += WHEEL_TO_BODY[i][j] * filteredVel[j] * RADIUS; // Convert angular to linear

   }

 }

 for (int i = 0; i < 3; i++) {

   lastGlobalPose[i] =+ curBodyVel[i] * delta;

 }

 lastDriveTime = curTime;

 // Serial.print("Delta: ");

 // Serial.println(delta);

 // Serial.print("Vel: left: ");

 // Serial.print(curVel[0]);

 // Serial.print(" right: ");

 // Serial.print(curVel[1]);

 // Serial.print(" back: ");

 // Serial.println(curVel[2]);

 // Serial.print("Filtered: left: ");

 // Serial.print(filteredVel[0]);

 // Serial.print(" right: ");

 // Serial.print(filteredVel[1]);

 // Serial.print(" back: ");

 // Serial.println(filteredVel[2]);

 // Serial.print("Err: left: ");

 // Serial.print(curErr[0]);

 // Serial.print(" right: ");

 // Serial.print(curErr[1]);

 // Serial.print(" back: ");

 // Serial.println(curErr[2]);

 // Serial.print("Int: left: ");

 // Serial.print(driveErrInt[0]);

 // Serial.print(" right: ");

 // Serial.print(driveErrInt[1]);

 // Serial.print(" back: ");

 // Serial.println(driveErrInt[2]);

  drive(power);

}

void drivePID(double w1, double w2, double w3) {

 double w[3] = {w1, w2, w3};

 drivePID(w);

}

void driveVector(double vx, double vy, double w) {

 double wheel_vel[3];

 for (int i = 0; i < 3; i++) {

   wheel_vel[i] = BODY_TO_WHEEL[i][0] * vx + BODY_TO_WHEEL[i][1] * vy + BODY_TO_WHEEL[i][2] * w;

 }

 drive(wheel_vel);

}

// Provide body velocities, uses individual PIDs to achieve them

void driveVectorPID(double vx, double vy, double w) {

 double wheel_vel[3];

 for (int i = 0; i < 3; i++) {

   wheel_vel[i] = BODY_TO_WHEEL[i][0] * vx + BODY_TO_WHEEL[i][1] * vy + BODY_TO_WHEEL[i][2] * w;

   wheel_vel[i] /= RADIUS; // convert linear to angular velocity

 }

 drivePID(wheel_vel);

}

double yawKp = 0.4;

// double yawKi = 0.8;

double yawKd = 0.0;

double lastYawError = 0;

double yawInt = 0;

double curYaw; // Set inside loop by IMU, in radians

double yawThresh = 3.0 / 180 * M_PI;

void yawPID(double yawGoal, double yawKi) {

 unsigned curTime = micros();

 double delta = (curTime - lastYawTime) / 1000000.0; // seconds

 double yawDiff = yawGoal - curYaw - yawInit;

 // Compute the error between the current and desired yaw, handling wrapping issues

 double error = atan2(sin(yawDiff), cos(yawDiff));

 // Compute the derivative of the error over time

 double derivative = (error - lastYawError) / delta;

 yawInt += error * delta;

 // // Compute the PID output based on the current error and derivative

 double pid_output = yawKp * error + yawKi * yawInt + yawKd * derivative;

 if (error < yawThresh) {

   pid_output = 0;

 }

 if (abs(yawInt) > 15) {

   yawInt /= abs(yawInt);

 }

 Serial.print("Yaw: ");

 Serial.print(curYaw * 180 / M_PI);

 Serial.print(" Goal: ");

 Serial.print(yawGoal * 180 / M_PI);

 Serial.print(" Err: ");

 Serial.print(error);

 Serial.print(" Deriv: ");

 Serial.print(derivative);

 Serial.print(" Int: ");

 Serial.print(yawInt);

 Serial.print(" Ang vel out: ");

 Serial.println(pid_output);

 lastYawTime = curTime;

 lastYawError = error;

 drive(pid_output, pid_output, pid_output);

 // driveVectorPID(0, 0, pid_output);

}

float targetIRBeaconVoltage = 2.5;

float minIRBeaconVoltage = 2.5;  // assumes access to voltage divider

// Initialize state machine

RobotState currentState = FINDING_TARGET_VOLTAGE;

void onEnterFindingTargetVoltage() {

 // set motor speeds

 drive(.6, .6, .6);

 int voltageReadingRaw = analogRead(IR_SENSOR);

 float currentVoltage = voltageReadingRaw * (5.0 / 1023.0);

 if (currentVoltage < minIRBeaconVoltage) {

   minIRBeaconVoltage = currentVoltage;

   targetIRBeaconVoltage = minIRBeaconVoltage + 0.3;

   Serial.print("New min voltage: ");

   Serial.println(minIRBeaconVoltage);

 }

 if (scanTimer.check()) {

   currentState = ROTATING_TO_TARGET_VOLTAGE;

 }

}

void onEnterRotatingToTarget() {

 // set motor speeds

 drive(.6, .6, .6);

 int voltageReadingRaw = analogRead(IR_SENSOR);

 float currentVoltage = voltageReadingRaw * (5.0 / 1023.0);

 if (currentVoltage < targetIRBeaconVoltage) {

   Serial.print("Current voltage: ");

   Serial.print(currentVoltage);

   Serial.print(" Target voltage: ");

   Serial.println(targetIRBeaconVoltage);

   Serial.println("Now waiting!");

   delay(500);

   elapsedTime = elapsedMillis();

   currentState = DRIVING_TO_CORNER;

 }

}

void onEnterDrivingToCorner() {

 if (elapsedTime < 1000) {

   driveVectorPID(0, -.3, 0);

 }

 else if (elapsedTime < 2500) {

   driveVectorPID(-.4, -.2, 0);

 }

 else {

   drive(0, 0, 0);

   if (yawInit == 0) {

     yawInit = curYaw;

   }

   elapsedTime = elapsedMillis();

   currentState = DRIVING_TO_SHOOTING_POSE;

 }

}

double targetBodyVel[3];

void updateTargetBodyVel(double x_curr[3], double x_goal[3], const double K[3][3]) {

 double diff[3];

 for (int i = 0; i < 3; i++) {

     diff[i] = x_curr[i] - x_goal[i];

     if (i == 2) {

         diff[2] *= 2;           

     }

     for (int j = 0; j < 3; j++) {

       targetBodyVel[i] += -K[i][j] * diff[j];

     }

 }

}

const double K[3][3] = {{0.97323762, 0.17917812, 1.00146822},

                   {0.17917812, 0.56621798, 0.62736087},

                   {1.00146822, 0.62736087, 2.36658033}};

double SHOOT_POSE[3] = {0.8, 0.8, M_PI / 4};

double STUDIO_POSE[3] = {0.4, 0.4, M_PI / 4};

bool reachedTarget(double xGoal[3], double xCurr[3]) {

 return fabs(xGoal[0] - xCurr[0]) < 0.1 && fabs(xGoal[1] - xCurr[1]) < 0.1 && fabs(xGoal[2] - xCurr[2]) < 0.1;

}

void onEnterDrivingToShootPose() {

 updateTargetBodyVel(lastGlobalPose, SHOOT_POSE, K);

 driveVectorPID(targetBodyVel[0], targetBodyVel[1], targetBodyVel[2]);

 // at shoot

 //  pose if deviation from xGoal is less than 0.1

 if (reachedTarget(SHOOT_POSE, lastGlobalPose)) {

   // currentState = DRIVING_TO_STUDIO_POSE;

   drive(0, 0, 0);

   currentState = WAITING;

 }

}

void runShooter(double RPM) {

 int power = shooterPID(RPM);

 analogWrite(SHOOTER_ENABLE, power);

 digitalWrite(SHOOTER_A, HIGH);

 digitalWrite(SHOOTER_B, LOW);

}

void turnOffShooter() {

 analogWrite(SHOOTER_ENABLE, 0);

 digitalWrite(SHOOTER_A, LOW);

 digitalWrite(SHOOTER_B, LOW);

}

void resetState() {

 elapsedTime = elapsedMillis();

 lastYawTime = micros();

 lastDriveTime = micros();

}

void resetYaw() {

 lastYawTime = micros();

 lastYawError = 0;

 yawInt = 0;

}

int seq = 0;

void shootGoodSequence() {

 Serial.print("Time: ");

 Serial.println(elapsedTime);

 if (seq == 0) {

   if (elapsedTime < 1200) {

     driveVectorPID(.5, -.1, 0);

   }

   else if (elapsedTime < 1450) {

     driveVectorPID(0, .5, 0);

   }

   else if (elapsedTime < 1725) {

     drive(0.65, 0.65 ,0.65);

     // driveVectorPID(0, 0, M_PI * .6); /// 1850

     // yawPID(38 / 180.0 * PI);

   }

   else {

     drive(0, 0, 0);

     elapsedTime = elapsedMillis();

     seq = 1;

   }

 }

 if (seq == 1) {

   if (elapsedTime > 2000) {

     intake.write(OPEN);

   }

   if (elapsedTime > 7000) {

     turnOffShooter();

     intake.write(CLOSED);

     elapsedTime = elapsedMillis();

     seq = 2;

   }

   else {

     runShooter(GOOD_RPM);

   }

 }

 if (seq == 2) {

   if (elapsedTime < 275) {

     drive(-.65, -.65, -.65);

   }

   else if (elapsedTime < 550) {

     driveVectorPID(0, -.5, 0);

   }

   else if (elapsedTime < 1900) {

     driveVectorPID(-.5, -.1, 0);

   }

   else {

     drive(0, 0, 0);

     seq = 3;

     delay(3000);

     elapsedTime = elapsedMillis();

   }

 }

 if (seq == 3) {

   if (elapsedTime < 2100) {

     driveVectorPID(.5, -.1, 0);

   }

   else if (elapsedTime < 2400) {

     driveVectorPID(0, .3, 0);

   }

   else if (elapsedTime < 3225) {

     drive(.65, .65, .65);

   }

   else {

     drive(0, 0, 0);

     seq = 4;

     elapsedTime = elapsedMillis();

   }

 }

 if (seq == 4) {

   if (elapsedTime > 2000) {

     intake.write(OPEN);

   }

   if (elapsedTime > 7000) {

     turnOffShooter();

     intake.write(CLOSED);

     elapsedTime = elapsedMillis();

     seq = 5;

   }

   else {

     runShooter(BAD_RPM);

   }

 }

 if (seq == 5) {

   if (elapsedTime < 800) {

     drive(-.65, -.65, -.65);

   }

   else if (elapsedTime < 1150) {

     driveVectorPID(0, -.5, 0);

   }

   else if (elapsedTime < 3300) {

     driveVectorPID(-.5, -.1, 0);

   }

   else {

     drive(0, 0, 0);

     seq = 6;

     elapsedTime = elapsedMillis();

   }

 }

}

void shootSequence() {

 int rpmDelta = analogRead(RPM_TUNER);

 rpmDelta = map(rpmDelta, 0, 1023, -100, 100);

 // int rpmDelta = 0;

 Serial.print("rpm delta: ");

 Serial.println(rpmDelta);

 if (digitalRead(ENABLE_GOOD_PIN)) {

   if (seq == 0) {

     if (elapsedTime < 1200) {

       driveVectorPID(.5, -.1, 0);

     }

     else if (elapsedTime < 1450) {

       driveVectorPID(0, .5, 0);

     }

     else if (elapsedTime < 1700) {

       // drive(0.65, 0.65 ,0.65);

       driveVectorPID(0, 0, M_PI * .7); /// 1850

       // yawPID(30 / 180.0 * PI, .4);

     }

     else if (elapsedTime < 2000) {

       drive(0, 0, 0);

     }

     else if (elapsedTime > 2000) {

       intake.write(OPEN);

     }

     // if (elapsedTime < 1000) {

     //   // double setpoint[] = {.34, 1.55, 0};

     //   linearVelProfile(1000, setpoint);

     //   resetYaw();

     // }

     // else if (elapsedTime < 5000) {

     //   yawPID(21.0 / 180.0 * M_PI, .6);

     // }

     // else if (elapsedTime > 5000) {

     //   intake.write(OPEN);

     //   drive(0, 0, 0);

     // }

     if (elapsedTime > 4500) {

       turnOffShooter();

       intake.write(CLOSED);

       seq = 1;

       resetState();

     }

     else {

       runShooter(GOOD_RPM + rpmDelta);

     }

   }

   else if (seq == 1) {

     // if (elapsedTime < 2500) {

     //   double setpoint[] = {-0.3, -0.5, -.4};

     //   linearVelProfile(2500, setpoint);

     // }

     // else if (elapsedTime < 4200) {

     //   drive(0, 0, 0);

     // }

     if (elapsedTime < 250) {

       drive(-.65, -.65, -.65);

     }

     else if (elapsedTime < 550) {

       driveVectorPID(0, -.5, 0);

     }

     else if (elapsedTime < 1900) {

       driveVectorPID(-.5, -.1, 0);

     }

     else if (elapsedTime < 4000) {

       drive(0, 0, 0);

     }

     else {

       seq = 0;

       resetState();

     }

   }

 }

 else if (digitalRead(ENABLE_BAD_PIN)) {

   if (seq == 0) {

     if (elapsedTime < 2100) {

       driveVectorPID(.5, -.1, 0);

     }

     else if (elapsedTime < 2400) {

       driveVectorPID(0, .3, 0);

       resetYaw();

     }

     else if (elapsedTime < 3150) {

       drive(.65, .65, .65);  // 3200

       // yawPID(M_PI / 2, .44);

     }

     else {

       drive(0, 0, 0);

       intake.write(OPEN);

     }

     if (elapsedTime > 5200) {

       turnOffShooter();

       intake.write(CLOSED);

       seq = 1;

       resetState();

     }

     else {

       runShooter(BAD_RPM + rpmDelta);

     }

   }

   else if (seq == 1) {

     if (elapsedTime < 800) {

       drive(-.65, -.65, -.65);

     }

     else if (elapsedTime < 1150) {

       driveVectorPID(0, -.5, 0);

     }

     else if (elapsedTime < 3300) {

       driveVectorPID(-.5, -.1, 0);

     }

     else if (elapsedTime < 5300) {

       drive(0, 0, 0);

     }

     else {

       seq = 0;

       resetState();

     }

   }

 }

 else {

   seq = 0;

   drive(0, 0, 0);

   turnOffShooter();

   intake.write(CLOSED);

   resetState();

 }

}

void linearVelProfile(double totalTime, double maxVel[3]) {

 double vel[3];

 if (elapsedTime < totalTime) {

   for (int i = 0; i < 3; i++) {

     double power;

     if (elapsedTime < totalTime / 2) {

       power = elapsedTime / totalTime * 2. * maxVel[i];

     }

     else {

       power = (totalTime - elapsedTime) / totalTime * 2. * maxVel[i];

     }

     vel[i] = power;

   }

   driveVectorPID(vel[0], vel[1], vel[2]);

 }

 else {

   drive(0, 0, 0);

 }

}

void celebrationTime() {

 drive(0, 0, 0);

 turnOffShooter();

 if (elapsedTime < 1000) {

   // driveVectorPID(0, 0, 2*M_PI);

 }

}

void loop() {

 icm_20948_DMP_data_t data;

 imu.readDMPdataFromFIFO(&data);

 if ((imu.status == ICM_20948_Stat_Ok) || (imu.status == ICM_20948_Stat_FIFOMoreDataAvail)) // Was valid data available?

 {

   if ((data.header & DMP_header_bitmap_Quat6) > 0) // We have asked for GRV data so we should receive Quat6

   {

     // Q0 value is computed from this equation: Q0^2 + Q1^2 + Q2^2 + Q3^2 = 1.

     // In case of drift, the sum will not add to 1, therefore, quaternion data need to be corrected with right bias values.

     // The quaternion data is scaled by 2^30.

     // Scale to +/- 1

     double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0; // Convert to double. Divide by 2^30

     double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0; // Convert to double. Divide by 2^30

     double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0; // Convert to double. Divide by 2^30

     double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));

     // Convert the quaternions to Euler angles (roll, pitch, yaw)

     // https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles&section=8#Source_code_2

     double q2sqr = q2 * q2;

     // roll (x-axis rotation)

     double t0 = +2.0 * (q0 * q1 + q2 * q3);

     double t1 = +1.0 - 2.0 * (q1 * q1 + q2sqr);

     double roll = atan2(t0, t1) * 180.0 / PI;

     // pitch (y-axis rotation)

     double t2 = +2.0 * (q0 * q2 - q3 * q1);

     t2 = t2 > 1.0 ? 1.0 : t2;

     t2 = t2 < -1.0 ? -1.0 : t2;

     double pitch = asin(t2) * 180.0 / PI;

     // yaw (z-axis rotation)

     double t3 = +2.0 * (q0 * q3 + q1 * q2);

     double t4 = +1.0 - 2.0 * (q2sqr + q3 * q3);

     curYaw = atan2(t3, t4);

     // Serial.print(F("Roll:"));

     // Serial.print(roll, 1);

     // Serial.print(F(" Pitch:"));

     // Serial.print(pitch, 1);

     // Serial.print(F(" Yaw:"));

     // Serial.println(curYaw * 180 / PI, 1);

   }

 }

 if (gameTimer.check()) {

   // Game Over!

   currentState = DONE;

   resetState();

 }

 // put your main code here, to run repeatedly:

 switch (currentState) {

 case FINDING_TARGET_VOLTAGE:

   onEnterFindingTargetVoltage();

   return;

 case ROTATING_TO_TARGET_VOLTAGE:

   onEnterRotatingToTarget();

   return;

 case DRIVING_TO_CORNER:

   onEnterDrivingToCorner();

   break;

 case DRIVING_TO_SHOOTING_POSE:

   // shootGoodSequence();

   shootSequence();

   break;

 case WAITING:

   break;

 case DONE:

   celebrationTime();

   break;

 }

 // if (digitalRead(BUTTON_PIN)) {

 //   Serial.println("Got button press");

 //   // shootGoodSequence();

 //   if (currentState == DRIVING_TO_SHOOTING_POSE) {

 //     seq = 0;

 //     elapsedTime = elapsedMillis();

 //   }

 // }

 // runShooterGood();

 // drive(0.7, 0.7, 0.7);

  // int val = analogRead(A9);

 // int mapped = map(val, 0, 1023, 0, 255);

 // if (mapped != lastPower) {

 //   Serial.print("Potentiometer mapped: ");

 //   Serial.println(mapped);

 //   lastPower = mapped;

 // }

 // drivePID(3*M_PI*2, 3*M_PI*2, 3*M_PI*2);

 // driveVectorPID(0, .7, .5);

 // onEnterDrivingToShootPose();

 // long newRight = rightEnc.read();

 // if (newRight != positionRight) {

 //   Serial.print("Right = ");

 //   Serial.println(newRight);

 //   positionRight = newRight;

 // }

 // long newLeft = leftEnc.read();

 // if (newLeft != positionLeft) {

 //   Serial.print("Left = ");

 //   Serial.println(newLeft);

 //   positionLeft = newLeft;

 // }

 // long newBack = backEnc.read();

 // if (newBack != positionBack) {

 //   Serial.print("Back = ");

 //   Serial.println(newBack);

 //   positionBack = newBack;

 // }

 // long newShooter = shooterEnc.read();

 // if (newShooter != positionShooter) {

 //   Serial.print("Shooter = ");

 //   Serial.println(newShooter);

 //   positionShooter = newShooter;

 // }

 delay(20);

 return;

}