Differential Drive/Mobile Goal Lift Program Issue

Programming Support VEXcode Pro V5 Text Tech Support
1

Hey everyone,

I’ve been trying to write a program for my team’s differential drive/mobile goal. For some reason this works on an old program but on here the mobile goal lift does not move at all. The motor inverting is exactly the same as the old program. Here is the code:

#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();

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

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

I think something might be fighting it (probably the drive control) but I’ve tried a lot and I’m not sure what to do.

Thanks!
-Omnom

2

I don’t see any mention of the mobile goal motors, are you sharing motors? I’m assuming you are. In that case, the drive code probably is not fighting it. To make sure, you could make variables for the drive speed, change the variables based on the if statements and controller input, and then pass the variables into one spin command just to be sure that they are not blocking.

3

Hey again, so I have changed the program countless times and still I can not get the R1 and R2 buttons to make those motors turn. Here is what I am stuck on currently.

int driveControlTask(){
  while (Competition.isDriverControl()){
  LeftSideDrive.spin(fwd, Controller1.Axis3.value() + (Controller1.Axis1.value()*.45),  pct);
  RightSideDrive.spin(fwd, Controller1.Axis3.value() - (Controller1.Axis1.value()*.45),  pct);
  

    if(Controller1.ButtonR1.pressing()){
    LeftFrontDrive.spin(fwd, 100, pct); 
    LeftMiddleDrive.spin(fwd, -100, pct);
    RightFrontDrive.spin(fwd, 100, pct);
    RightMiddleDrive.spin(fwd, -100, pct);
    }
    else if(Controller1.ButtonR2.pressing()){
    LeftFrontDrive.spin(fwd, -100, pct);
    LeftMiddleDrive.spin(fwd, 100, pct);
    RightFrontDrive.spin(fwd, -100, pct);
    RightMiddleDrive.spin(fwd, 100, pct);
    }
}
return(1);}

In theory I thought it would work. All help would be appreciated. Thanks!

4

Where and how is driveControlTask called?

1 Like
5

Here.

void usercontrol(void) {
  Controller1.rumble(rumbleShort);

 while(Competition.isDriverControl()){
     vex::task driveControl(driveControlTask);
  
  //Back Mobile Goal Lift (R1/R2)
   if(Controller1.ButtonR1.pressing()){
    LeftFrontDrive.spin(fwd, 100, volt); 
    LeftMiddleDrive.spin(fwd, -100, volt);
    RightFrontDrive.spin(fwd, 100, volt);
    RightMiddleDrive.spin(fwd, -100, volt);
    }
   else if(Controller1.ButtonR2.pressing()){
    LeftFrontDrive.spin(fwd, -100, volt);
    LeftMiddleDrive.spin(fwd, 100, volt);
    RightFrontDrive.spin(fwd, -100, volt);
    RightMiddleDrive.spin(fwd, 100, volt);
    }
  
  //Front Mobile Goal Lift (Up/Down)
   if(Controller1.ButtonUp.pressing()){
     MobileLift.spin(fwd, 100, pct);
   }
   else if(Controller1.ButtonDown.pressing()){
     MobileLift.spin(fwd, -100, pct);
   }
   else {
     MobileLift.stop(brake);
   }

   
  
  //Conveyor Belt (L1/L2)
   if(Controller1.ButtonL1.pressing()){
    ConveyorBelt.spin(fwd, 100, pct);
   }
   else if (Controller1.ButtonL2.pressing()){
    ConveyorBelt.spin(fwd, -100, pct);
   }
   else{
    ConveyorBelt.stop(brake);
   }
   }

  wait(20, msec); 
 }

But I don’t believe it is the calling that is the problem, because the joystick driving works just fine.

6

Generally speaking, do not put task creation statements in while loops. Move that line before the while statement.

You’ll next run into a problem where the code in the driveControlTask is trying to control the same motors as the code in usercontrol.

To solve this, you’ll probably want to remove the driveControlTask entirely and figure out how to blend the logic of the button presses into your main usercontrol function.

4 Likes