Robot Moving Slow During Driver Control

Hello, I just tested my robot and during the driver control period, our robot’s wheels seems to be moving abnormally slow. Can someone please assist me on what errors are taking place in my code? It’ll be posted down below. Thanks!

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       C:\Users\507C                                             */
/*    Created:      Mon Aug 22 2022                                           */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]   
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include <cmath> //std::abs

using namespace vex;
using namespace std;

vex::brain Brain;

vex::competition Competition;

motor LFront = motor(PORT4, false);
motor LBack = motor(PORT1, false);
motor RFront = motor(PORT5, true);
motor RBack = motor(PORT10, true);
motor inTake = motor(PORT3, false);
motor flyWheel = motor(PORT2, false);
controller Controller1 = controller(primary);
inertial Inertial = inertial(PORT21);
motor_group rightDrive(RFront, RBack);
motor_group leftDrive(LFront, LBack);
drivetrain Drivetrain = drivetrain(leftDrive, rightDrive, 319.19, 295, 40, mm, 1);

//Degrees to Inches
float inch = 28.6487346;

//Auton Select Variaables
int  HS = 1;
int AutonLeft = 0;
int TrialandError = 0;
int AutonRight = 0;
int Skills = 0;

//Tuning Constants for Distance
double kP = 4.0;
double kI = 0.001;
double kD = 2.0;

//Tuning Constants for Turn
double turnkP = 1.0;
double turnkI = 0.001;
double turnkD = 0.1;

//Establish a Maximum Power; Avoid Going Too Fast
double voltageThreshhold = 8;

//Establish Variables Used in Control Loop
int desiredValue = 0;
int DesiredTurnValue = 0;

int error;
int prevError = 0;
int derivative;
int totalError = 0;

int turnError;
int turnPrevError = 0;
int turnDerivative;
int turnTotalError = 0;

//Allows You to Reset Your Loop
bool resetDriveSensors = false;

//Allows You to Turn Off PID Control in Driver Control
bool enableDrivePID = true;

//Drive pid
int drivePID(){

  while(enableDrivePID){

    if (resetDriveSensors) {
      resetDriveSensors = false;
      LFront.setPosition(0,degrees);
      LBack.setPosition(0,degrees);
      RFront.setPosition(0,degrees);
      RBack.setPosition(0,degrees);
      //Inertial.setHeading(0,degrees);
    }

    int LFrontPos = LFront.position(degrees);
    int LBackPos = LBack.position(degrees);
    int RFrontPos = RFront.position(degrees);
    int RBackPos = RBack.position(degrees);

    ////////////////////////////////////////////////////
    //Drive PID
    ////////////////////////////////////////////////////

    int averagePosition = (LFrontPos + LBackPos + RFrontPos + RBackPos)/4;

    error = desiredValue - averagePosition;

    derivative = error - prevError;

    totalError = totalError + error;

    double motorPower = (error * kP + derivative * kD + totalError * kI)/12.0;

    if (fabs(motorPower) > voltageThreshhold){
      motorPower = ( voltageThreshhold * (motorPower / fabs(motorPower) ) );
    }

    ///////////////////////////////////////////////////
    //Turn PID
    ///////////////////////////////////////////////////

    int turnDifference = (LFrontPos + LBackPos) - (RFrontPos + RBackPos);

    turnError = DesiredTurnValue - turnDifference;

    //turnError = DesiredTurnValue - Inertial.heading(degrees);
      
    turnDerivative = turnError - turnPrevError;

    turnTotalError = turnTotalError + turnError;

    double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI)/12.0;

    if (fabs(turnMotorPower) > voltageThreshhold){
      turnMotorPower = ( voltageThreshhold * (turnMotorPower / fabs(turnMotorPower) ) );
    }

    ///////////////////////////////////////////////////
    //Motor Commands
    ///////////////////////////////////////////////////

    double powerScaler = 1.0;

    double leftMotorPower = powerScaler * (motorPower + turnMotorPower);
    double rightMotorPower = powerScaler * (motorPower - turnMotorPower);


    LFront.spin(fwd, leftMotorPower,volt);
    RFront.spin(fwd, rightMotorPower, volt);
    LBack.spin(fwd, leftMotorPower, volt);
    RBack.spin(fwd, rightMotorPower, volt);


    //desiredValue *= inch;
    prevError = error;
    turnPrevError = turnError;
    task::sleep(20);

    }

    return 1;
  }

void Intake(int speed){
  inTake.spin(fwd, speed, pct);
}

void FlyWheel(int VoltageSpeed){
  flyWheel.spin(fwd, VoltageSpeed, volt);

}

//Intake Task
bool IntakeOn = false;
int iT(){
  while(IntakeOn == true){
    inTake.spin(fwd, 100, pct);
  }
  inTake.stop();
  if(Controller1.ButtonL2.pressing()){
    inTake.spin(fwd, -100, pct);
  }
  else{
    inTake.stop(coast);
  }

  return 1;
}

//FlyWheel Task
bool flyWheelOn = false;
int fW(){
  while(flyWheelOn == true){
    if(Controller1.ButtonRight.pressing()){
      flyWheel.spin(fwd, 12, volt);
    }
    else if(Controller1.ButtonUp.pressing()){
      flyWheel.spin(fwd, 10, volt);
    }
    else if(Controller1.ButtonLeft.pressing()){
      flyWheel.spin(fwd, 8, volt);
    }
    else{
      flyWheel.stop(coast);
    }
  }

  return 1;
}

//HomeScreen Of Brain On AutonSelect
void HomeScreen(){
   Brain.Screen.clearScreen();

   Brain.Screen.drawRectangle(0,0,240,120, black);
   Brain.Screen.printAt(45,60, false, "Auton Left");

   Brain.Screen.drawRectangle(240,0,240,120, black);
   Brain.Screen.printAt(285,60, false, "AutonL T&E");

   Brain.Screen.drawRectangle(0,120,240,120, blue);
   Brain.Screen.printAt(45,180, false, "AutonR Right");

   Brain.Screen.drawRectangle(240,120,240,120, blue);
   Brain.Screen.printAt(285,180, false, "AutonR Skills");

   Brain.Screen.drawLine(235,0,235,240);
   Brain.Screen.drawLine(0,117,480,117);

  //While Loop Used To Determine Which Side Of Autonomous Will Be Used
  while(HS == 1){
    if(Brain.Screen.pressing()){
      //Set Up Auton Left 
      if(Brain.Screen.xPosition() >= 0 && Brain.Screen.xPosition() <= 240 && Brain.Screen.yPosition() >= 0 && Brain.Screen.yPosition() <= 120){
          AutonLeft = 1;
          HS = 0;
          Brain.Screen.print("Autonomous Left Side");
          Brain.Screen.clearScreen();
          Brain.Screen.setCursor(1,1);
      }

      //Set Up Auton T&E
      else if(Brain.Screen.xPosition() >= 240 && Brain.Screen.xPosition() <= 480 && Brain.Screen.yPosition() >= 0 && Brain.Screen.yPosition() <= 120){
          TrialandError = 1;
          HS = 0;
          Brain.Screen.print("Trial and Error");
          Brain.Screen.clearScreen();
          Brain.Screen.setCursor(1,1);
      }

      //Set Up Auton Right 
      else if(Brain.Screen.xPosition() >= 0 && Brain.Screen.xPosition() <= 240 && Brain.Screen.yPosition() >= 120 && Brain.Screen.yPosition() <= 240){
          AutonRight = 1;
          HS = 0;
          Brain.Screen.print("Autonomous Right Side");
          Brain.Screen.clearScreen();
          Brain.Screen.setCursor(1,1);
      }

      //Set Up Auton Skills
      else if(Brain.Screen.xPosition() >= 240 && Brain.Screen.xPosition() <= 480 && Brain.Screen.yPosition() >= 120 && Brain.Screen.yPosition() <= 240){
          Skills = 1;
          HS = 0;
          Brain.Screen.print("Skills Auton");
          Brain.Screen.clearScreen();
          Brain.Screen.setCursor(1,1);
      }

      else{}

  }
}
}

void pre_auton( void ){
  //calibrating inertial
  //Inertial.calibrate();
  //Inertial.setRotation(0, degrees);
  //while(Inertial.isCalibrating()){
    //wait(10, msec);
  //}
  HomeScreen();
  
}

void auton_left(){
  task testDrivePID(drivePID);
  resetDriveSensors = true;
  desiredValue = 0;
  DesiredTurnValue= 0;
  vex::task::sleep(1000);
}


void auton_tandE(){
}


void auton_right(){
  FlyWheel(10);
  wait(1, sec);
  FlyWheel(0);
} 

void auton_skills(){
} 


//Void Function For Autonomous
void autonomous( void ){
 //Code for left side of field will be displayed for autonomous
  if (AutonLeft == 1){
      auton_left();
  }

  //Code for trial and error will be displayed for autonomous
  else if (TrialandError == 1){
      auton_tandE();
  }

  //Code for right side of field will be displayed for autonomous
  else if (AutonRight == 1){
      auton_right();
  }

  //Code for skills will be displayed for autonomous
  else if (Skills == 1){
      auton_skills();
  }

  else{}
}

//Drivetrain
double turnImportance = 0.5;

void drivercontrol( void ){
  
  while(1) {
    enableDrivePID = false;

    double turnVal = Controller1.Axis1.position(percent);
    double forwardVal = Controller1.Axis3.position(percent);

    double turnVolts = turnVal * 0.12;
    double forwardVolts = forwardVal * 0.12 * (1 - (std::abs(turnVolts)/12.0) * turnImportance);

    //0 - 12 = -12
    //0 + 12 = 12(due to cap)

    leftDrive.spin(fwd, forwardVolts + turnVolts, volt);
    rightDrive.spin(fwd, forwardVolts - turnVolts, volt);

    IntakeOn = true;
    task testInTake(iT);

    flyWheelOn = true;
    task testFlyWheel(fW);

    LFront.stop(coast);
    LBack.stop(coast);
    RFront.stop(coast);
    RBack.stop(coast);

   }
}

int main() {

  pre_auton();
  Competition.drivercontrol(drivercontrol);
  Competition.autonomous(autonomous);
  
}

You have ur robot set to move at 12% speed if I’m seeing the code correctly

2 Likes

Isn’t 12 the max volatage?

looks like you send stop to the drive motors inside the while loop.

also.
not really a good idea to start tasks inside a while loop.

7 Likes