I need some help with my Autonomous Code

Hello guys.

I need help with a line a of code I wanted my robot to turn right but at the moment I just know how to make it turn to the left.

Go to int onauton_autonomous_0() { To see the problem.

I’m using VEXcode Pro V5.

Any help or inspiration will be appreciated.

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       ForsenCD                                                      */
/*    Created:      Wed Dec 04 2019                                           */
/*    Description:  This program will turn right 90 degrees using the         */
/*                  Inertial Sensor.                                          */
/*                                                                            */
/*                                                                            */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// LeftMotorBack        motor         9               
// RightMotorBack       motor         10              
// Inertial20           inertial      20              
// Controller1          controller                    
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include <cmath>
using namespace vex;

competition Competition;

// User defined function
void myblockfunction_InertialTurn_heading_velocity_momentum(double myblockfunction_InertialTurn_heading_velocity_momentum__heading, double myblockfunction_InertialTurn_heading_velocity_momentum__velocity, double myblockfunction_InertialTurn_heading_velocity_momentum__momentum);
// User defined function
void myblockfunction_DriveStraight_distance_heading_velocity_Kp(double myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp);

float error, output;

// User defined function
void myblockfunction_InertialTurn_heading_velocity_momentum(double myblockfunction_InertialTurn_heading_velocity_momentum__heading, double myblockfunction_InertialTurn_heading_velocity_momentum__velocity, double myblockfunction_InertialTurn_heading_velocity_momentum__momentum) {
  if (myblockfunction_InertialTurn_heading_velocity_momentum__heading > Inertial20.rotation(degrees)) {
    while ((myblockfunction_InertialTurn_heading_velocity_momentum__heading - myblockfunction_InertialTurn_heading_velocity_momentum__momentum > Inertial20.rotation(degrees))) {
      RightMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
      LeftMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
      LeftMotorBack.spin(reverse);
      RightMotorBack.spin(forward);
    wait(5, msec);
    }
  }
  else {
    while ((myblockfunction_InertialTurn_heading_velocity_momentum__heading + myblockfunction_InertialTurn_heading_velocity_momentum__momentum < Inertial20.rotation(degrees))) {
      LeftMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
      RightMotorBack.setVelocity(myblockfunction_InertialTurn_heading_velocity_momentum__velocity, percent);
      LeftMotorBack.spin(forward);
      RightMotorBack.spin(reverse);
    wait(5, msec);
    }
  }
  RightMotorBack.stop();
  LeftMotorBack.stop();
}

//My Functions 
void myblockfunction_DriveStraight_distance_heading_velocity_Kp(double myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity, double myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp) {
  LeftMotorBack.setRotation(0.0, degrees);
  RightMotorBack.setRotation(0.0, degrees);
  if (myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity > 0.0) {
    while ((LeftMotorBack.rotation(degrees) < myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance)) {
      error = myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading - Inertial20.rotation(degrees);
      output = error * myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp;
      LeftMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity - output), percent);
      RightMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity + output), percent);
      LeftMotorBack.spin(forward);
      RightMotorBack.spin(forward);
    wait(5, msec);
    }
  }
  else {
    while ((LeftMotorBack.rotation(degrees) > myblockfunction_DriveStraight_distance_heading_velocity_Kp__distance)) {
      error = myblockfunction_DriveStraight_distance_heading_velocity_Kp__heading - Inertial20.rotation(degrees);
      output = error * myblockfunction_DriveStraight_distance_heading_velocity_Kp__Kp;
      LeftMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity - output), percent);
      RightMotorBack.setVelocity((myblockfunction_DriveStraight_distance_heading_velocity_Kp__velocity + output), percent);
      LeftMotorBack.spin(forward);
      RightMotorBack.spin(forward);
    wait(5, msec);
    }
  }
  LeftMotorBack.stop();
  RightMotorBack.stop();
}

// "when driver control" 



int ondriver_drivercontrol_0() {
  
  
    double turnImportance = 0.5;
  while(1){
   
    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)


    LeftMotorBack.spin(forward, forwardVolts + turnVolts, voltageUnits::volt);
    RightMotorBack.spin(forward, forwardVolts - turnVolts, voltageUnits::volt);

    if (Controller1.ButtonR1.pressing()){

    IntakeLeft.spin(vex:: directionType::fwd, 85, vex::velocityUnits::pct);
    IntakeRight.spin(vex:: directionType::rev, 85, vex::velocityUnits::pct);
  
  }

  else if (Controller1.ButtonL1.pressing()) {

   IntakeLeft.spin(vex::directionType::rev, 75, vex::velocityUnits::pct);
   IntakeRight.spin(vex::directionType::fwd, 75, vex::velocityUnits::pct);
  
  }

  else {
   
   IntakeLeft.stop(vex::brakeType::brake);
   IntakeRight.stop(vex::brakeType::brake);
  
  }

  if(Controller1.ButtonR2.pressing()){
   
   RollerBottomFront.spin(vex::directionType::rev, 95, vex::velocityUnits::pct);
   RollerTopBack.spin(vex::directionType::fwd, 95, vex::velocityUnits::pct);
   
  }
  
  else if (Controller1.ButtonL2.pressing()) {
   
  
   RollerTopBack.spin(vex::directionType::rev, 95, vex::velocityUnits::pct);
  
  }
  
  else {
   
   RollerBottomFront.stop(vex::brakeType::brake);
   RollerTopBack.stop(vex::brakeType::brake);
  
  }

  if (Controller1.ButtonUp.pressing()){

  
  }


  if (Controller1.ButtonX.pressing()){

   RollerBottomFront.spin(vex::directionType::fwd,75,vex::velocityUnits::pct);

  }
    
    
    wait(20, msec);
  }

  
  return 0;
}

// "when autonomous" 
int onauton_autonomous_0() {
  Inertial20.startCalibration();
  while (Inertial20.isCalibrating()) { task::sleep(50); }
  
  // NOTE Each time the robot turns 90 degrees, we add 90 degrees to the next turn ex. First turn = 90 degrees second turn =180 degrees 
  // NOTE DriveStraight = (DriveFor, Past Angle, velocity, Kp)
  
  //drives robot 900 ticks forward 
  myblockfunction_DriveStraight_distance_heading_velocity_Kp(900.0, 0.0, 50.0, 2.0);
  //turns the robot 90.0 degrees Left 
  myblockfunction_InertialTurn_heading_velocity_momentum(90.0, 20.0, 3.0);
  //drives robot 900 ticks forward Left
  myblockfunction_DriveStraight_distance_heading_velocity_Kp(900.0, 90.0, 50.0, 2.0);
    //turns the robot 90.0 degrees Left
  myblockfunction_InertialTurn_heading_velocity_momentum(180.0, 20.0, 3.0);
  //drives robot 900 ticks forward 
  myblockfunction_DriveStraight_distance_heading_velocity_Kp(900.0, 180.0, 50, 2.0);

  // I want to turn right 90 degrees next time and go backwards how will the next values look after the two actions?
  
  

  return 0;
}



void VEXcode_driver_task() {
  // Start the driver control tasks....
  vex::task drive0(ondriver_drivercontrol_0);


  while(Competition.isDriverControl() && Competition.isEnabled()) 
  {this_thread::sleep_for(10);}
  drive0.stop();
  return;
}

void VEXcode_auton_task() {
  // Start the auton control tasks....
  vex::task auto0(onauton_autonomous_0);
  while(Competition.isAutonomous() && Competition.isEnabled()) 
  {this_thread::sleep_for(10);}
  auto0.stop();
  return;
}



int main() {
  vex::competition::bStopTasksBetweenModes = false;
  Competition.autonomous(VEXcode_auton_task);
  Competition.drivercontrol(VEXcode_driver_task);

}

That looks like PROS to me. That turning function looks custom, and not part of the PROS library. If you could show us the declaration for the function, it would be much easier to help. Also, use the three tick marks ` before and after code to format it,

Instead of using screenshots.
1 Like

I’m using VEXcode Pro V5.

And I will update the screenshot for an actual code.
I’m still learning the format of the website.
Thank you for the answer. :slight_smile:

Example?

Perfect. Odd that pros and vexcode pro look similar. Maybe that’s just me. My gut answer to your original question is that you just need to add or subtract 90 (not sure which since your function seems to act a bit weirdly) from the last value you passed into the turn function, and it will “undo” the turn by reversing the motor powers and turning the opposite direction. That would be if this is some sort of proportional loop.

1 Like

I will try to add my code to the forum but it is really long…

I think I get what you mean


//will turn the robot, right... at 20 pct velocity.

InertialTurn(-90.0, 20.0, 0.3) // your suggestion

//NOTE the turn is at 180 degrees at the moment.
(180.0, 20.0, 3.0); //because I'm using Inertial rotations and not headings

Well, if you are using rotation and NOT heading, then I was thinking along the lines of writing in 90 instead of 180. I would think that would go back 90 degrees. You’ll have to test, this will be easy to fix if you have access to the robot.

2 Likes

Yeah, I think I need to test it first and see what happens.
I will upload the whole code so people will get more information about the post and not just me getting the info.

Thank you for your time and sorry for the inconvenience.
:slight_smile:

1 Like