Why does the reverse for Motor groups not work?

Programming Support
, , , ,
1

I am trying to have my two groups of motors run in reverse in a function but it is stuttering back and forth rapidly. However, the forward motion of the motors works very well. Can anyone look over my code and let me know what may be causing the problem and perhaps how I could fix it?

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// Controller1          controller                    
// LeftMotors           motor_group   1, 2            
// RightMotors          motor_group   9, 10           
// BucketArmMotor       motor_group   3, 8            
// ClawPivotMotor       motor         6               
// ClawMotor            motor         7               
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include "robot-config.h"

using namespace vex;

bool forwardDrive = true;

// Control drivetrain and drivetrain reversal
void Drivetrain(){
  //Declare all variables used in the function here
  int speed = 100;
  float leftPower = (100 + ((100.0/127)*Controller1.Axis4.value()));
  float rightPower = (100 - ((100.0/127)*Controller1.Axis4.value()));

  
  
  if(Controller1.Axis4.value() == 0){
    LeftMotors.setVelocity(speed, percent);
    RightMotors.setVelocity(speed, percent);
  }

  // Increase increase low speed mode factor when R1 pressed
  if(Controller1.ButtonR1.pressing()){speed = speed/3;}
  else{speed = 100;} // Reset low speed mode factor when R1 is released


  if(Controller1.ButtonL2.pressing()){ // Set robot to drive forward
    LeftMotors.spin(forward);
    RightMotors.spin(forward);
  } else if (Controller1.ButtonR2.pressing()) { // Set robot to drive backward
    LeftMotors.spin(reverse);
    RightMotors.spin(reverse);
  } else { // Set robot to to stop
    LeftMotors.stop();
    RightMotors.stop();
  }

  // Controll the direction of the robot
  if(Controller1.ButtonR2.pressing() || Controller1.ButtonL2.pressing()){
    if(Controller1.Axis4.value()>0){ // Set right group of wheels to slow down by the factor of the input
      RightMotors.setVelocity(rightPower*(speed/100), percent);
    } else if(Controller1.Axis4.value()<0){ // Set left group of wheels to slow down by the factor of the input
      LeftMotors.setVelocity(leftPower*(speed/100), percent);
    }
  } else{
    if(Controller1.Axis4.value()>0){ // Set right group of wheels to reverse and left to spin forward
      RightMotors.spin(reverse);
      LeftMotors.spin(forward);
    } else if(Controller1.Axis4.value()<0){ // Set left group of wheels to reverse and right to spin forward
      RightMotors.spin(forward);
      LeftMotors.spin(reverse);
    }
  }
}

// Open and close the claw depending on the position of it
bool ControlClaw(bool open) {
  // Set the Claw Arm Pivot to hold its position when it is stationary
  ClawPivotMotor.setStopping(brakeType::hold);
  
  // Adjust the 
  if(open == false){
    ClawMotor.spinFor(reverse, 0.48,turns);
    open = true;
  }else if(open == true){
    ClawMotor.spinFor(forward, 0.48,turns);
    open = false;
  }
  return open;
}

void ClawArmMotion(){
  //Set the claw arm to hold its position when it is not given commands
  ClawPivotMotor.setStopping(brakeType::hold);

  if(Controller1.ButtonUp.pressing()){
    ClawPivotMotor.spin(forward);
  } else if(Controller1.ButtonDown.pressing()){
    ClawPivotMotor.spin(reverse);
  } else {
    ClawPivotMotor.stop();
  }
}

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All varibles for main function initialised bellow
  bool clawOpen = true;

  // Set all velocities for motors used in the main function
  ClawMotor.setVelocity(40, percent);
  ClawPivotMotor.setVelocity(60, percent);
  
  // Infinitly loop processies bellow to operate robot
  while(true){
    // Call function to accelerate and reverse the robot
    Drivetrain();

    // Set the bucket arms to be controlled by the left joystick
    BucketArmMotor.spin(vex::directionType::fwd, Controller1.Axis2.value(), vex::velocityUnits::pct);
    // Set the Bucket Arm to hold its position when it is not given commands
    BucketArmMotor.setStopping(brakeType::hold);

    // Call function to collect ring when Button Y is pressed of the contoller
    if(Controller1.ButtonY.pressing()){clawOpen = ControlClaw(clawOpen);}

    // Call function to control motion of the claw arm
    ClawArmMotion();
  }
}
2

I know this may sound like a janky solution, but instead of reversing the motor, use negative inputs. It’s what my team did, and it worked out fine and helped out with the readability of the code.

5 Likes
3

To pitch into what was said here, a negative velocity is just backward at that velocity! I would 100% recommend this solution as you can just set the value of the input as the velocity. As for the issue, when does it occur? all the time? while turning? while having the input be a small number? (Pushing the joystick a small amount)

1 Like
4

This was definitely logical error. I would highly recommend re-writing your Drivetrain function to be more readable and maintainable.

I took half an hour looking over your code, trying to understand what you are doing here… and I think I understand what you were trying to do. It was clever, in a kind of roundabout way. :wink: (except for that it doesn’t work)

However, I wouldn’t try to fix your code… I would re-write it to be a lot more simple. To quote Elon Musk, “possibly the most common error of a smart engineer is to optimise a thing that should not exist."

Maybe consider using a more simple method of controlling your robot, like a tank drive:

void Drivetrain ()
{
    //initialize speed_factor with correct factor.
    //1 is 100%, 0.5 would be 50%, and 1/3 is 33.3%
    float speed_factor = (Controller1.ButtonR1.pressing())? 1/3 : 1;

    //The right joystick controlls right wheels, and the left joystick controlls left wheels.
    float right_velocity = Controller1.Axis2.value() * speed_factor;
    float left_velocity = Controller1.Axis3.value() * speed_factor;

    //annoyingly, before you use the .setVelocity() method, you must call the .spin() method first.
    //both are forward, because if you push the joystick into the negative, this would be the same as reversing the motor.
    RightMotors.spin(forward);
    LeftMotors.spin(forward);

    //apply velocities to wheels:
    RightMotors.setVelocity(right_velocity, pct);
    LeftMotors.setVelocity(left_velocity, pct);
}

Or, if you really want to, you could also use an arcade drive… though I prefer tank because of its simplicity.

void Drivetrain ()
{
    //initialize speed_factor with correct factor.
    //1 is 100%, 0.5 would be 50%, and 1/3 is 33.3%
    float speed_factor = (Controller1.ButtonR1.pressing())? 1/3 : 1;

    float forward_velocity = Controller1.Axis3.value();
    float clockwise_angular_velocity = Controller1.Axis1.value();

    //annoyingly, before you use the .setVelocity() method, you must call the .spin() method first.
    //both are forward, because if you push the joystick into the negative, this would be the same as reversing the motor.
    RightMotors.spin(forward);
    LeftMotors.spin(forward);

    //apply velocities to wheels:
    RightMotors.setVelocity((forward_velocity - clockwise_angular_velocity) * speed_factor, pct);
    LeftMotors.setVelocity((forward_velocity + clockwise_angular_velocity) * speed_factor, pct);
}
6

You have multiple lines changing the speed of the motor groups which can cause unexpected behavior. It’s better to only assign the velocity (and direction) in one spot.

Take a look at this code below. I am assuming that you want to control the robot with Split Arcade.
One other note, the motor speeds are between -100 and 100. The V5 joysticks and motors do not use -127 to 127 like the older Cortex technology.

double speedRatio  = 1.0;  //Global variable to hold speed mode

void SetSpeedMode(){  //This function switches the speed mode.
  if(speedRatio == 1.0) {
    speedRatio = 0.3;
  } else {
    speedRatio = 1.0;
  }
}

//Sets the speed based on the joysticks and adjusts for speed mode.
void Drive(){
  double leftGroup  = Controller1.Axis2.position() + Controller1.Axis4.position();
  double rightGroup = Controller1.Axis2.position() - Controller1.Axis4.position();
  LeftMotors.setVelocity(leftGroup * speedRatio , percent);
  RightMotors.setVelocity(rightGroup * speedRatio , percent);
}

int main() {
  //Create an event to switch the speed
  Controller1.ButtonX.pressed(SetSpeedMode);
  while(true){
    Drive();
    wait(20, msec);
  }
}

Finally if you still want an instant reverse, using a button you could add a similar function like SetSpeedMode and program it to multiply the speedRatio by -1. As mentioned above, changing the motor values to negative reverses the direction (although it might be hard on your motors if done while driving).