PID Code Starting

Programming Support VEXcode V5 Tech Support
, ,
#1

Hello there! I just wrote a pid code but idk how to start this code. For example, in this pid code, how can i set my pid code for going 30 inches forward in autonomous process?

This is my code:
/----------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: VEX /
/ Created: Thu Sep 26 2019 /
/ Description: Competition Template /
/ /
/----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// InertialSensor       inertial      19              
// ElevatorLift         motor         10              
// RFM                  motor         15              
// RBM                  motor         16              
// LFM                  motor         13              
// LBM                  motor         14              
// Controller1          controller                    
// BackLift             motor         4               
// ArmGroup             motor_group   1, 2            
// digoutG              digital_out   G               
// digoutH              digital_out   H               
// digoutA              digital_out   A               
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include <cmath>

using namespace vex;

// A global instance of competition
competition Competition;

// define your global instances of motors and other devices here

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the V5 has been powered on and        */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

//constant for pid true/false
bool enableDrivePID = true;

//Movement PID Settings
double kP = 0.3;
double kD = 0.1;

//Turn PID Settings
double turnkI = 0.2;
double turnkP = 0.5;
double turnkD = 0.1;

//Autonomous Settings
int desiredValue = InertialSensor.rotation(degrees);
int desiredTurnValue = 0;


//Movement Errors
int error; // Sensor Value - Desired Value --> Positional Value -> speed -> acceleration
int prevError = 0; // Position 20 miliseconds ago
int derivative; // Error - prevError : speed(velocity)
int totalError = 0; // totalError = totalError + error;

//Turning Errors
int turnError; // Sensor Value - Desired Value --> Positional Value -> speed -> acceleration 
int turnPrevError = 0; // Position 20 miliseconds ago
int turnDerivative; // Err  or - prevError : speed(velocity)
int turnTotalError = 0; // totalError = totalError + error;

//reset drive sensors
bool resetDriveSensors = false;

int drivePID(){
  while(enableDrivePID){
if(resetDriveSensors){
  resetDriveSensors = false;

  LBM.setPosition(0,degrees);
  LFM.setPosition(0,degrees);
  RBM.setPosition(0,degrees);
  RBM.setPosition(0,degrees);
}
//getting positions of motors
int lfmP = LFM.position(degrees);
int lbmP = LBM.position(degrees);
int rfmP = RFM.position(degrees);
int rbmP = RBM.position(degrees);

//////////////////////////////////////////////
///////Lateral Motor Power
//////////////////////////////////////////////

//getting average position
int averagePosition = (lfmP+lbmP+rfmP+rbmP)/4;

//Potential
error = desiredValue - averagePosition;

//Derivative
derivative = error - prevError;

//calculating motor power
double lateralMotorPower = (error * kP + derivative * kD);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////
///////Lateral Motor Power
//////////////////////////////////////////////



//////////////////////////////////////////////
///////Turning Motor Power
//////////////////////////////////////////////

//getting average position
int turnDifference = (lfmP+lbmP)-(rfmP+rbmP);

//Potential
turnError = desiredTurnValue - turnDifference;

//Derivative
turnDerivative = turnError - turnPrevError;

//calculating motor power
double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD);
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//start motors
LFM.spin(forward,lateralMotorPower + turnMotorPower,volt);
LBM.spin(forward,lateralMotorPower + turnMotorPower,volt);
RFM.spin(forward,lateralMotorPower - turnMotorPower,volt);
RBM.spin(forward,lateralMotorPower - turnMotorPower,volt);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
  }
  return 1;
}
void autonomous(void) {
  vex::task start(drivePID);

  resetDriveSensors = true;
  desiredTurnValue = 200;


}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/
int counter=0;
int counter2=1;
int counter3 = 0;
int counter4 = 0;
void pressedFunc(){
  if(counter==0){
counter=1;
  }
  else{
counter=0;
  }
}
void pressedFunc2(){
  if(counter2==1){
counter2=0;
  }
  else{
counter2=1;
  }
}
void pressedFunc3(){
  counter3=1;
}
void pressedFunc4(){
  counter4=1;
}

void usercontrol(void) {
  // User control code here, inside the loop
enableDrivePID = false;

//Settings
double turnImportance = 0.5;
  Controller1.ButtonLeft.pressed(pressedFunc);
  Controller1.ButtonRight.pressed(pressedFunc2);
  Controller1.ButtonR1.pressed(pressedFunc3);
  Controller1.ButtonR2.pressed(pressedFunc4);
while(1){
    if(Controller1.Axis3.position(pct) != 0){
        LFM.spin(reverse,Controller1.Axis3.position(),pct);
        LBM.spin(reverse,Controller1.Axis3.position(),pct);
        RFM.spin(forward,Controller1.Axis3.position(),pct);
        RBM.spin(forward,Controller1.Axis3.position(),pct);
        if(Controller1.Axis1.position(pct) != 0){
        LFM.spin(reverse,Controller1.Axis1.position(),pct);
        LBM.spin(reverse,Controller1.Axis1.position(),pct);
        RFM.spin(reverse,Controller1.Axis1.position(),pct);
        RBM.spin(reverse,Controller1.Axis1.position(),pct);
    }
    }
    else if(Controller1.Axis1.position(pct) != 0){
        LFM.spin(reverse,Controller1.Axis1.position(),pct);
        LBM.spin(reverse,Controller1.Axis1.position(),pct);
        RFM.spin(reverse,Controller1.Axis1.position(),pct);
        RBM.spin(reverse,Controller1.Axis1.position(),pct);
        if(Controller1.Axis3.position(pct) != 0){
        LFM.spin(reverse,Controller1.Axis3.position(),pct);
        LBM.spin(reverse,Controller1.Axis3.position(),pct);
        RFM.spin(forward,Controller1.Axis3.position(),pct);
        RBM.spin(forward,Controller1.Axis3.position(),pct);}
    }
    else{
        LFM.stop(brake);
        LBM.stop(brake);
        RFM.stop(brake);
        RBM.stop(brake);
    }


  bool topRightButton = Controller1.ButtonL1.pressing();
  bool bottomRightButton = Controller1.ButtonL2.pressing();

  if(topRightButton){
    ArmGroup.spin(forward,7.0,volt);
  }
  else if(bottomRightButton){
    ArmGroup.spin(forward,-6.0,volt);
  }
  else{
    ArmGroup.stop(brakeType::brake);
  }

  bool topLeftButton = Controller1.ButtonUp.pressing();
  bool bottomLeftButton = Controller1.ButtonDown.pressing();

  if(topLeftButton){
    BackLift.spin(forward,12.0,volt);
  }
  else if(bottomLeftButton){
    BackLift.spin(forward,-12.0,volt);
  }
  else{
    BackLift.stop(brakeType::brake);
  }
  
  if(Controller1.ButtonX.pressing()){
    ElevatorLift.spin(forward,12.0,volt);
  }
  else if(Controller1.ButtonB.pressing()){
    ElevatorLift.spin(forward,-12.0,volt);
  }
  else{
    ElevatorLift.stop(brakeType::brake);
  }
  if(counter==1){
    digoutG.set(true);
  }
  else if(counter==0){
    digoutG.set(false);
  }

  if(counter2==0){
    digoutH.set(true);
  }
  else if(counter2==1){
    digoutH.set(false);
  }
  if(counter3==1){
    digoutA.set(true);
  }
  if(counter4==1){
    digoutA.set(false);
  }
  }

}

//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
wait(100, msec);
  }
}
#2

@242EProgrammer

#3

Did… you get those mixed up…?

I think you have the basic idea here:

With a couple of modifications, it would work:

errorMargin = /* for example */ 10
resetDriveSensors = true;
desiredValue = 500;
waitUntil(error < errorMargin);
// and/or
wait(500, msec);

And of course, desiredValue is in motor degree units, not inches. You could make a function for going forward that converts it:

void drive(double inches) {
  inches *= 45; /* You'd have to figure out 
  and possibly tune the conversion ratio 
  from inches to motor degrees */
  resetDriveSensors = true;
  desiredValue = inches;
  waitUntil(error < errorMargin);
}
3 Likes
#4

You might also want to note that your inertial sensor is not going to be used for turning or keeping straight; both your forward error and your turn error are calculated off of motor degrees.

I’m not sure if you want to or not.

You should probably also start

these variables at zero and zero your motors immediately at the beginning of autonomous.

2 Likes