flywheel PID spinning, but once to max rpm stops/motors shake

Programming Support VEXcode Pro V5 Text Tech Support
#1

Everything works normally without flywheel PID. However, when we spin flywheel pid, once “readyToShoot” the flywheel motors shake and the flywheel loses its inertia.

Here’s our code, please help I’m at a competition


// preparing for pd drivetrain. Lots of commented code to be reused.
#include "vex.h"
#include <cmath>
#include <stdlib.h>
brain Bean;
using namespace vex;
competition Competition;
vex::thread pidtask1;
vex::thread pidtask2;

void intake();
void stop_intake(bool brake);
void auto_disc_detection();
bool wantToShoot;
bool readyToShoot;


// Settings (please adjust when testing)
double kP_DT = 0.04; // change these values.
double kD_DT = 0.04;
double turnkP = 0.01;
double turnkD = 0.01;


int errorDT = 0;
int prevErrorDT = 0;
int derivativeDT = 0;
int desiredValueDT = 0;
int desiredTurnValueDT = 0;
int turnErrorDT = 0;
int turnPrevErrorDT = 0;
int turnDerivativeDT = 0;
bool resetDriveSensors = false;
// Allows the PD loop to be enabled in auton - disabled in user control
bool enableDrivePD = true;


int drivePD(){
while (enableDrivePD) {
 if (resetDriveSensors) {
  resetDriveSensors = false;
 //  DT_LeftBackMotor.setPosition(0,degrees);
 //  DT_RightBackMotor.setPosition(0,degrees);
 //  DT_LeftfrontMotor. setPosition(0, degrees);
 //  DT_RightFrontMotor.setPosition(0,degrees);
 }
 //  int leftMotorPosition = DT_LeftFrontMotor.position(degrees);
 //  int leftBMotorPosition = DT_LeftBackMotor.position(degrees);
 //  int rightFMotorPosition = DT_RightFrontMotor.position(degrees);
 //  int rightBMotorPosition = DT_RightBackMotor.position(degrees);
 //  int averagePosition = (leftMotorPosition + leftBMotorPosition + rightFMotorPosition + rightBMotorPosition) /4;
 //  errorDT = averagePosition - desiredValueDT;
 //  derivativeDT = errorDT - prevErrorDT;
 //  double lateralMotorPower = (errorDT * kP_DT + derivativeDT * kD_DT);
 //  int turnDifference = (leftMotorPosition + leftMotorPosition - rightFMotorPosition - rightMotorPosition) /4;
 //  turnErrorDT = turnDifference - desiredTurnValueDT;
 //  turnDerivativeDT = turnErrorDT - turnPrevErrorDT;
 //  double turnMotorPower = (turnErrorDT * turnkP + turnDerivativeDT * turnkD);
 // motors arent configured yet... get back old motor functions?
//    DT_LeftBackMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits: :volt);
//    DT_RightBackMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits: :volt);
//    DT_LeftFrontMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits: :volt);
//    DT_RightFrontMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits: :volt);
//   // coast when called or else drivetrain will spin forever
 }
 return 0;
}


int autonPIDFlywheel();


void pre_auton(void) {
vexcodeInit();
resetDriveSensors = true;
Bean.resetTimer();
}


// void moveForward() {
//   DT_LeftFrontMotor.spin(directionType::fwd);
//   DT_LeftBackMotor.spin(directionType::fwd);
//   DT_RightFrontMotor.spin(directionType::fwd);
//   DT_RightBackMotor.spin(directionType::fwd);
// }


// void turnRight() {
//   DT_LeftFrontMotor.spin(directionType::fwd);
//   DT_LeftBackMotor.spin(directionType::fwd);
// }


// void turnLeft() {
//   DT_RightFrontMotor.spin(directionType::fwd);
//   DT_RightBackMotor.spin(directionType::fwd);
// }


// void moveBackward() {
//   DT_LeftFrontMotor.spin(directionType::rev);
//   DT_LeftBackMotor.spin(directionType::rev);
//   DT_RightFrontMotor.spin(directionType::rev);
//   DT_RightBackMotor.spin(directionType::rev);
// }


// void SetAllVelocity(int DVU) {
//   DT_LeftBackMotor.setVelocity(DVU, percent);
//   DT_LeftFrontMotor.setVelocity(DVU, percent);
//   DT_RightBackMotor.setVelocity(DVU, percent);
//   DT_RightFrontMotor.setVelocity(DVU, percent);
// }


// void brakeall(bool brakee) {
//   if (brakee) {
//     DT_LeftFrontMotor.stop(brake);
//     DT_LeftBackMotor.stop(brake);
//     DT_RightFrontMotor.stop(brake);
//     DT_RightBackMotor.stop(brake);
//   }
//   else {
//     DT_LeftFrontMotor.stop(coast);
//     DT_LeftBackMotor.stop(coast);
//     DT_RightFrontMotor.stop(coast);
//     DT_RightBackMotor.stop(coast);
//   }
// }


void startFlywheel() {
Flywheel1_Motor.spin(directionType::fwd);
Flywheel2_Motor.spin(directionType::fwd);
}
void startIntake() {
Intake_Motor.spin(directionType::fwd);
}
void startRoller() {
Roller_Motor.spin(directionType::fwd);
}
void setVelocityFIR(int DEVU) {
 Intake_Motor.setVelocity(DEVU, percent);
 Roller_Motor.setVelocity(DEVU, percent);
 Flywheel1_Motor.setVelocity(DEVU, percent);
 Flywheel2_Motor.setVelocity(DEVU, percent);
}
float kp = 0.07;
float ki = 0;
float kd = 0.11;
int targetFlywheelSpeedrpm = 600; // put desired rpm flywheel speed here
int flywheelVelocityrpm;
int error = targetFlywheelSpeedrpm - flywheelVelocityrpm;
int lastError = 0;
int totalError = 0;
int integralActiveZone = 15;
float finalAdjustment = error * kp;
int flywheelPowerVolts;
int autonPIDFlywheel() {
while (1) {
  if (wantToShoot) {
   flywheelVelocityrpm = (Flywheel1_Motor.velocity(rpm) + Flywheel2_Motor.velocity(rpm)) / -2;
   error = targetFlywheelSpeedrpm - flywheelVelocityrpm;
   if (error <= 5) { // 0-5 noise rpm?
     readyToShoot = true;
     printf("done\n");
   }
   else {
     readyToShoot = false;
   }
   flywheelPowerVolts = ((error * kp) + (totalError * ki) + ((error - lastError) * kd));
   Flywheel1_Motor.spin(reverse, flywheelPowerVolts, volt);
   Flywheel2_Motor.spin(reverse, flywheelPowerVolts, volt);
   lastError = error;
   totalError += error;
  }
  else {
    Flywheel1_Motor.stop(coast);
    Flywheel2_Motor.stop(coast);
  }
  this_thread::sleep_for(20);
}
return 0;
}
void firstAuton() {
  thread pidTask = thread(autonPIDFlywheel);
  printf("this works!\n");
  setVelocityFIR(100);
}
void autonomous(void) {
wantToShoot = true;
enableDrivePD = true;
firstAuton();
}
// void movement() {
//   // for the arcade
//   // DT_LeftFrontMotor.spin(directionType::fwd, Controller1.Axis3.position(), percent);
//   // DT_LeftBackMotor.spin(directionType::fwd, Controller1.Axis3.position(), percent);
//   // DT_RightBackMotor.spin(directionType::fwd, Controller1.Axis2.position(), percent);
//   // DT_RightFrontMotor.spin(directionType::fwd, Controller1.Axis2.position(), percent);
// }
void killTasks() {
pidtask1.interrupt();
}
void usercontrol(void) {
pidtask1 = thread(autonPIDFlywheel);
enableDrivePD = false;
Drivetrain.setDriveVelocity(100, percent);
setVelocityFIR(100);
while (1) {
  ///////////////// other buttons /////////////////
  
  if (Controller1.ButtonR1.pressing() || Controller1.ButtonR2.pressing()) { // manual flywheel
    Roller_Motor.spin(fwd);
  }
  else {
    Roller_Motor.stop();
  }
  if (Controller1.ButtonL1.pressing() || Controller1.ButtonL2.pressing()) { // manual intake
    wantToShoot = true;
    if (readyToShoot) {
      // Intake_Motor.spin(fwd);
    }
  }
  else {
    wantToShoot = false;
    Flywheel1_Motor.stop(coast);
    Flywheel2_Motor.stop(coast);
    // Intake_Motor.stop(coast);
  }
  if (Controller1.ButtonLeft.pressing()) { // press x to start expansion
    expansion1.set(false);
  }
  else if (Controller1.ButtonRight.pressing()) { // press y to "retract" expansion
    expansion1.set(true);
  }

  wait(20, msec);
}
}
int main() {
  Competition.bStopAllTasksBetweenModes = true;
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);
  pre_auton();
  while (true) {
    if (Competition.DISABLED) {
      
    }
    this_thread::sleep_for(20);
  }
  return 0;
}
#2

Think about what the value of flywheelPowerVolts will be if the error were small, lets say 20. Ignoring the I and D terms, you would have 20 * 0.07 which is 1.4 (although flywheelPowerVolts is also integer, that’s a separate issue). So as the flywheel spins up and error decreases you reduce voltage to the motor, that’s obviously an issue.

3 Likes