6 Drive PID Auton Problems

Programming Support VEXcode Pro V5 Text Tech Support
1

Hey everyone,

I’ve been trying to code a PID autonomous with the aid of a youtube video, made by @Connor. The code on there is meant for a four motor drive, when my team’s is a six motor. I attempted to edit it myself, but all it does is spin around infinitely. (Actually I’m not sure I haven’t had infinite time to test it out) Here is the code:

//Auton PID Program
//Based from Connor 1814D's PID Program

 // ---- START VEXCODE CONFIGURED DEVICES ----
 // Robot Configuration:
 // [Name]               [Type]        [Port]
 // LeftFrontDrive       18_1          3
 // LeftMiddleDrive      18_1          1
 // LeftBackDrive        18_1          7        
 // RightFrontDrive      18_1          6
 // RightMiddleDrive     18_1          4
 // RightBackDrive       18_1          8
 // ConveyorBelt         18_1          9
 // MobileLift           18_1          10
 // ---- END VEXCODE CONFIGURED DEVICES ----

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

using namespace vex;
competition Competition;

void wait(float mSec){
vex::task::sleep(mSec);
 }

//-------------------------
//PID Coding
 double kP = 0.1;
 double kI = 0.1;
 double kD = 0.1;

 double turnkP = 0.1;
 double turnkI = 0.1;
 double turnkD = 0.1;

 int desiredValue = 200;
 int desiredTurnValue = 0;

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

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

 bool resetDriveSensors = false;

 bool enableDrivePID = true;

 int drivePID(){
  while(enableDrivePID){
    
    if (resetDriveSensors){
      resetDriveSensors = false;
      LeftFrontDrive.setPosition(0, degrees);
      LeftMiddleDrive.setPosition(0, degrees);
      LeftBackDrive.setPosition(0, degrees);

      RightFrontDrive.setPosition(0, degrees);
      RightMiddleDrive.setPosition(0, degrees);
      RightBackDrive.setPosition(0, degrees);
      
    }
    
    int leftFrontPosition = LeftFrontDrive.position(degrees);
    int leftMiddlePosition = LeftMiddleDrive.position(degrees);
    int leftBackPosition = LeftBackDrive.position(degrees);
    int rightFrontPosition = RightFrontDrive.position(degrees);
    int rightMiddlePosition = RightMiddleDrive.position(degrees);
    int rightBackPosition = RightBackDrive.position(degrees);

    int averagePosition = (leftFrontPosition + rightFrontPosition + leftMiddlePosition + rightMiddlePosition + leftBackPosition + rightBackPosition)/6;

    error = averagePosition - desiredValue;
    derivative = error - prevError;
    totalError += error;

    double lateralMotorPower = error * kP + derivative * kD + totalError * kI;

    int turnDifference = leftFrontPosition - rightFrontPosition;

    turnError = turnDifference - desiredTurnValue;
    turnDerivative = turnError - turnPrevError;
    turnTotalError += turnError;

    double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;

    LeftFrontDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    LeftMiddleDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    LeftBackDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);

    RightFrontDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    RightMiddleDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    RightBackDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    
    prevError = error;
    turnPrevError = turnError;
    vex::task::sleep(20);
  }
  return 1;
 }
//Autonomous
 void autonomous(void){

  vex::task wooooo(drivePID);
  
  resetDriveSensors = true;
  desiredValue = 300;
  desiredTurnValue = 600;

  vex::task::sleep(1000);

  resetDriveSensors = true;
  desiredValue = 300;
  desiredTurnValue = 600;

  vex::task::sleep(1000);

 }

   
 
//User Control
 void usercontrol(void) {
  Controller1.rumble(rumbleShort); 

  enableDrivePID = false;

  while(Competition.isDriverControl()){
   LeftFrontDrive.spin(fwd, Controller1.Axis3.value() + (Controller1.Axis1.value()*.45),  pct);
   LeftMiddleDrive.spin(fwd, Controller1.Axis3.value() + (Controller1.Axis1.value()*.45),  pct);
   LeftBackDrive.spin(fwd, Controller1.Axis3.value() + (Controller1.Axis1.value()*.45),  pct);

   RightFrontDrive.spin(fwd, Controller1.Axis3.value() - (Controller1.Axis1.value()*.45),  pct);
   RightMiddleDrive.spin(fwd, Controller1.Axis3.value() - (Controller1.Axis1.value()*.45),  pct);
   RightBackDrive.spin(fwd, Controller1.Axis3.value() - (Controller1.Axis1.value()*.45),  pct);
  
   bool R1Button = Controller1.ButtonR1.pressing();
   bool R2Button = Controller1.ButtonR2.pressing();
   bool L1Button = Controller1.ButtonL1.pressing();
   bool L2Button = Controller1.ButtonL2.pressing();
   bool UpButton = Controller1.ButtonUp.pressing();
   bool DownButton = Controller1.ButtonDown.pressing();
  
  //Front Mobile Goal Lift (Up/Down)
   if(UpButton){
     MobileLift.spin(fwd, 12, volt);
   }
   else if(DownButton){
     MobileLift.spin(fwd, -12, volt);
   }
   else {
     MobileLift.stop(hold);
   }

  //Back Mobile Goal Lift (R1/R2)
   if(R1Button){
    LeftFrontDrive.spin(fwd, 12, volt), 
    LeftMiddleDrive.spin(fwd, -12, volt);
    RightFrontDrive.spin(fwd, 12, volt);
    RightMiddleDrive.spin(fwd, -12, volt);
   }
   else if(R2Button){
    LeftFrontDrive.spin(fwd, -12, volt);
    LeftMiddleDrive.spin(fwd, 12, volt);
    RightFrontDrive.spin(fwd, -12, volt);
    RightMiddleDrive.spin(fwd, 12, volt);
   }
   
  
  //Conveyor Belt (L1/L2)
   if(L1Button){
    ConveyorBelt.spin(fwd, 12, volt);
   }
   else if (L2Button){
    ConveyorBelt.spin(fwd, -12, volt);
   }
   else{
    ConveyorBelt.stop(hold);
   }

   }
  }
//Main
 int main() {
  Competition.drivercontrol(usercontrol);
  
   while (true) {
    wait(1, msec);
  }
 }
//-------------------------

I would appreciate any and all feedback you have for me.
-Omnom

1 Like
2

Hello,
I am still relatively new to PID, but it looks like you only have your motor output as turning. You should have another motor output for forward/backward movement.


motor1.spin(forward, lateralMotorPower, voltageUnits::Volts)
motor2.spin(forward, lateralMotorPower, voltageUnits::Volts)
motor3.spin(forward, lateralMotorPower, voltageUnits::Volts)

This should stop it from turning forever. Also keep in mind that you are going to have to put an exit on the while loop to keep it from running forever. I hope this helps

21417B

3

I would start by setting all the constants to 0, except for kP. Probably leave kP at 0.1.

Then see what it does.

Another tool I like to use for feedback is the terminal on the computer.

Using the printf command, you can send text to display to the terminal. I have printf commands in my PID loop constantly resending what values the motors are receiving.

Example would be:

printf("Left motor powers: %f  and %f \n", lateralMotorPower, turnMotorPower);
printf("Right motor power: %f  and %f \n", lateralMotorPower, turnMotorPower);
2 Likes
4

The first step into learning PID, is to not copy code. Go read a document about it instead, and you’ll gain a way better understanding of it.

1 Like
5

I would like to point out a difference between copying someones code and reading to understand how that code works. I find it very helpful to read other peoples code to understand how they do “it” but copying that code with out understanding how it works leads to problems like this.

1 Like
6

I understand what you mean, and it is personally very frustrating for me to do it this way, because I don’t understand much of the actual language and coding aspect of it. I am only familiar with the codes that I have been using consistently in past years. In the past I have tried to read some things online about the overall mechanics of PID, and while I understand it, I just do not know how to put it into code. Is there anything I can do to understand how to write code from scratch? How did you guys learn?

7

I find learning coding by coding from scratch to be difficult. I find that looking at well-written code is often less intimidating. Sometimes it can be hard to know if code is “well-written” or not. Often a good guide is whether its easy to understand or not. PID is such a common algorithm, one should be able to find lots of implementations on github (some better written than others).

Another approach is to write everything in pseudocode. This lets you not worry about the exact language mechanics of a particular language (argh, I forgot a paranthesis and now the compiler hates me!), and instead to focus on the logic. Walk thru your code in your head and use a piece of paper to keep track of variable values. This will help make sure your loops and conditional logic statements behave as you intend.

3 Likes
8

That is a good idea, one I will definitely use in the future. But the problem is is I don’t know the language, how different codes react and work, and really just where to start. My code is always a hodgepodge of other people’s work, but I want it to be my own, be able to just write it down and understand how it does what it does. I feel like most people can just write their own easily. It would just be nice to know how exactly they got there, where they learned from.

9
Slides helpful Mike Dane video on C++

C++ Tutorial for Beginners - Full Course - YouTube
Absolute chad in teaching this stuff

PID video that I found helpful;

You’re welcome :wink:

2 Likes
10

PID Documents
PID Beginner’s Guide • Renegade Robotics
https://www.vexforum.com/uploads/short-url/buexv3XI4CflyuDzkxC9wNzTOg4.pdf This is a download
PID | HD Open Source Projects (hdprojects.dev)

Some good sources

2 Likes
11

I learned coding originally on Khan Academy, with JavaScript. Then, for vex, I learned C++ from a book and lots of random searches on stack overflow.
I find that the best way, for me at least, to learn coding is to work on projects that are of interest to me. By trying to design my own projects, I learned a huge amount about code design in a fairly short amount of time. Through debugging these badly put together projects, I learned what works and what does not.

1 Like
12

A good first step is to learn the language. Sit at home and find some good YouTube tutorials, W3 schools also has some good tutorials.