Self- aligning mechanum wheel code help

Programming Support VEXcode Pro V5 Text Tech Support
1

I think I have my self aligning code figured out mathematically (unable to test atm), but I keep getting errors in building

WHOLE CODE

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       Serial                                                    */
/*    Created:      3/25/2021                                                 */
/*    Description:  WASD Mecha Wheel                                          */
/*                                                                            */
/*----------------------------------------------------------------------------*/



// Include the V5 Library
#include "vex.h"
#include <string>



// Allows for easier use of the VEX Library
using namespace vex;



//Variables---------------------------------------------------------------------------------------------
  double Modifier = 100;
  double Speed = 1 * (Modifier/100);
  double side_axis = Speed * (Controller1.ButtonLeft.pressing() - Controller1.ButtonRight.pressing());
  double straight_axis = Speed * (Controller1.ButtonUp.pressing() - Controller1.ButtonDown.pressing());
  double TopRightBottomLeft = Speed * (straight_axis *100 + side_axis * 100);
  double TopLeftBottomRight = Speed * (straight_axis *100 - side_axis * 100);
  double RightSide = Speed * (0-Controller1.Axis1.value());
  double LeftSide = Speed * (Controller1.Axis1.value());
  long double FeetZ = 343.78;
  long double MetersZ = 1127.87;
  long double MilimetersZ = 1.129;
  long double InchesZ = 28.649;
  long double FeetX = 343.78;
  long double MetersX = 1127.87;
  long double MilimetersX = 1.129;
  long double InchesX = 28.649;
  long double RC = 360;
  competition Competition = competition();


//Functions---------------------------------------------------------------------------------------------
void BallOut(){
    Controller1.rumble("--");

}


//------------------------------------------------------------------------------------------------------
void BallIn(){
    Controller1.rumble(".");
}


//------------------------------------------------------------------------------------------------------
void MotorTorque(double var){
  FrontRight.setMaxTorque(var, percent);
  FrontLeft.setMaxTorque(var, percent);
  RearRight.setMaxTorque(var, percent);
  RearLeft.setMaxTorque(var, percent);
  Arms.setMaxTorque(var, percent);
  Lift.setMaxTorque(var, percent);
}


//------------------------------------------------------------------------------------------------------
void DrivetrainSpeed(double drivetrainspeed){
  FrontRight.setVelocity(drivetrainspeed, percent);
  FrontLeft.setVelocity(drivetrainspeed, percent);
  RearRight.setVelocity(drivetrainspeed, percent);
  RearLeft.setVelocity(drivetrainspeed, percent);

}


//--------------------------------------------------------------------------------------------------------
void Suck(){
    if (Lift.isSpinning()) {
      Lift.stop();
    }
    if (Arms.isSpinning()) {
      Arms.stop();
    }
    Lift.spin(forward);
    Arms.spin(forward);
    while (Controller1.ButtonL1.pressing()){
        wait(1, msec);
    }
    Lift.stop();
    Arms.stop();
}


//---------------------------------------------------------------------------------------------------------
void Blow(){
    if (Lift.isSpinning()) {
      Lift.stop();
    }
    if (Arms.isSpinning()) {
      Arms.stop();
    }
    Lift.spin(reverse);
    Arms.spin(reverse);
    while (Controller1.ButtonL2.pressing()){
        wait(1, msec);
    }
    Lift.stop();
    Arms.stop();
}


//----------------------------------------------------------------------------------------------------------
double SpinTime = 1.7112;
void RotateRight(double deg, double fastness){
    DrivetrainSpeed(fastness);
    FrontLeft.spin(forward);
    RearLeft.spin(forward);
    FrontRight.spin(reverse);
    RearRight.spin(reverse);
    wait((deg * (SpinTime/360)) * (100/fastness), seconds);
    FrontRight.stop();
    FrontLeft.stop();
    RearRight.stop();
    RearLeft.stop();
    DrivetrainSpeed(100);
}
void RotateLeft(double deg, double fastness){
    DrivetrainSpeed(fastness);
    FrontLeft.spin(reverse);
    RearLeft.spin(reverse);
    FrontRight.spin(forward);
    RearRight.spin(forward);
    wait((deg * (SpinTime/360)) * (100/fastness), seconds);
    FrontRight.stop();
    FrontLeft.stop();
    RearRight.stop();
    RearLeft.stop();
    DrivetrainSpeed(100);
}


//----------------------------------------------------------------------------------------------------------
void straight(double d, int fastness){
    XAxisEncoder.setPosition(0, degrees);
    ZAxisEncoder.setPosition(0, degrees);
    Inertial1.setHeading(180, degrees);
    DrivetrainSpeed(fastness);
    FrontLeft.spin(vex::directionType::fwd, (fastness * ((180-(inertial.heading(degrees)-180))/180))+XAxisEncoder.position(degrees),  vex::velocityUnits::pct);
    FrontRight.spin(vex::directionType::fwd, (fastness * ((180+(inertial.heading(degrees)-180))/180))-XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearLeft.spin(vex::directionType::fwd, (fastness * ((180-(inertial.heading(degrees)-180))/180))-XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearRight.spin(vex::directionType::fwd, (fastness * ((180+(inertial.heading(degrees)-180))/180))+XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    while (!(ZAxisEncoder.position(turns) == d/12.56637)){
      wait(1,msec);
      break;
    }
    FrontLeft.stop();
    FrontRight.stop();
    RearLeft.stop();
    RearRight.stop();
    DrivetrainSpeed(100);
}

void side(double d, int fastness){
    Inertial1.setHeading(180, degrees);
    XAxisEncoder.setPosition(0, degrees);
    ZAxisEncoder.setPosition(0, degrees);
    DrivetrainSpeed(fastness);
    FrontLeft.spin(vex::directionType::fwd, (fastness * ((180-(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees),  vex::velocityUnits::pct);
    FrontRight.spin(vex::directionType::rev, (fastness * ((180-(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearLeft.spin(vex::directionType::rev, (fastness * ((180+(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearRight.spin(vex::directionType::fwd, (fastness * ((180+(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    while (!(XAxisEncoder.position(turns) == d/12.56637)){
      wait(1,msec);
      break;
    }
    FrontLeft.stop();
    FrontRight.stop();
    RearLeft.stop();
    RearRight.stop();
    DrivetrainSpeed(100);
}


void East(double d, int fastness){
    int GearRatio(200);
    DrivetrainSpeed(fastness);
    FrontRight.spin(reverse);
    FrontLeft.spin(forward);
    RearRight.spin(forward);
    RearLeft.spin(reverse);
    wait(((d/11.6637075716)/(GearRatio/60)) * (100/fastness),seconds);
    FrontRight.stop();
    FrontLeft.stop();
    RearRight.stop();
    RearLeft.stop();
    DrivetrainSpeed(100);
}
void West(double d, int fastness){
    int GearRatio(200);
    DrivetrainSpeed(fastness);
    FrontRight.spin(forward);
    FrontLeft.spin(reverse);
    RearRight.spin(reverse);
    RearLeft.spin(forward);
    wait(((d/11.6637075716)/(GearRatio/60)) * (100/fastness),seconds);
    FrontRight.stop();
    FrontLeft.stop();
    RearRight.stop();
    RearLeft.stop();
    DrivetrainSpeed(100);
}
void North(double d, int fastness){
    int GearRatio(200);
    DrivetrainSpeed(fastness);
    FrontRight.spin(forward);
    FrontLeft.spin(forward);
    RearRight.spin(forward);
    RearLeft.spin(forward);
    wait(((d/12.56637)/(GearRatio/60)) * (100/fastness),seconds);
    FrontRight.stop();
    FrontLeft.stop();
    RearRight.stop();
    RearLeft.stop();
    DrivetrainSpeed(100);
}
void South(double d, int fastness){
    int GearRatio(200);
    DrivetrainSpeed(fastness);
    FrontRight.spin(reverse);
    FrontLeft.spin(reverse);
    RearRight.spin(reverse);
    RearLeft.spin(reverse);
    wait(((d/12.56637)/(GearRatio/60)) * (100/fastness),seconds);
    FrontRight.stop();
    FrontLeft.stop();
    RearRight.stop();
    RearLeft.stop();
    DrivetrainSpeed(100);
}
//AUTONOMUS---------------------------------------------------------------------------------------------
void autonomus_control(){
    
}



//DRIVER CONTROL----------------------------------------------------------------------------------------
void driver_control(){
while(true){
    

FrontLeft.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value() + Controller1.Axis4.value(),  vex::velocityUnits::pct);
FrontRight.spin(vex::directionType::fwd, Controller1.Axis3.value() - Controller1.Axis1.value() - Controller1.Axis4.value(), vex::velocityUnits::pct);
RearLeft.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value() - Controller1.Axis4.value(), vex::velocityUnits::pct);
RearRight.spin(vex::directionType::fwd, Controller1.Axis3.value() - Controller1.Axis1.value() + Controller1.Axis4.value(), vex::velocityUnits::pct);
    Controller1.ButtonL1.pressed(Suck);
    Controller1.ButtonL2.pressed(Blow);
    BallOutDetect.pressed(BallOut);
    while (BallDetectEntry.distance(inches)<4) {
        BallIn();
    }
}
}




//Main--------------------------------------------------------------------------------------------------
int main() {
    Inertial1.calibrate();
    wait(2,seconds);
    Arms.setVelocity(100, percent);
    Lift.setVelocity(100, percent);
    DrivetrainSpeed(100);
    MotorTorque(100);
    FrontRight.setStopping(hold);
    FrontLeft.setStopping(hold);
    RearRight.setStopping(hold);
    RearLeft.setStopping(hold);
    Arms.setStopping(hold);
    Lift.setStopping(hold);
    Competition.autonomous(autonomus_control);
    Competition.drivercontrol(driver_control);
}

CODE WITH ERRORS

void straight(double d, int fastness){
    XAxisEncoder.setPosition(0, degrees);
    ZAxisEncoder.setPosition(0, degrees);
    Inertial1.setHeading(180, degrees);
    DrivetrainSpeed(fastness);
    FrontLeft.spin(vex::directionType::fwd, (fastness * ((180-(inertial.heading(degrees)-180))/180))+XAxisEncoder.position(degrees),  vex::velocityUnits::pct);
    FrontRight.spin(vex::directionType::fwd, (fastness * ((180+(inertial.heading(degrees)-180))/180))-XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearLeft.spin(vex::directionType::fwd, (fastness * ((180-(inertial.heading(degrees)-180))/180))-XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearRight.spin(vex::directionType::fwd, (fastness * ((180+(inertial.heading(degrees)-180))/180))+XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    while (!(ZAxisEncoder.position(turns) == d/12.56637)){
      wait(1,msec);
      break;
    }
    FrontLeft.stop();
    FrontRight.stop();
    RearLeft.stop();
    RearRight.stop();
    DrivetrainSpeed(100);
}

void side(double d, int fastness){
    Inertial1.setHeading(180, degrees);
    XAxisEncoder.setPosition(0, degrees);
    ZAxisEncoder.setPosition(0, degrees);
    DrivetrainSpeed(fastness);
    FrontLeft.spin(vex::directionType::fwd, (fastness * ((180-(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees),  vex::velocityUnits::pct);
    FrontRight.spin(vex::directionType::rev, (fastness * ((180-(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearLeft.spin(vex::directionType::rev, (fastness * ((180+(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearRight.spin(vex::directionType::fwd, (fastness * ((180+(inertial.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    while (!(XAxisEncoder.position(turns) == d/12.56637)){
      wait(1,msec);
      break;
    }
    FrontLeft.stop();
    FrontRight.stop();
    RearLeft.stop();
    RearRight.stop();
    DrivetrainSpeed(100);
}

SCREENSHOTS

Screenshot 2021-03-29 10.30.57
Screenshot 2021-03-29 10.30.57917×257 18.2 KB

Screenshot 2021-03-29 10.31.11
Screenshot 2021-03-29 10.31.11917×257 18.4 KB

1 Like
2

well the first thing is you probably want to do calculations inside the functions instead of calculating everything when the program starts.

Also you really should use at the very least a P loop. Just stopping the drive base instantly when you reach the target will cause massive slip.

1 Like
3

Can you explain what you mean by p loop?
And I thought I was calculating it inside the function

4

P loop scales down the power supplied to the motors with the distance from you to the target position

and it looks like those are calculated globally.

1 Like
5

Can you give an example of calculating inside the function?
So it’s like a linear function based off distance to target?

6

yes. you just have a constant kP and the Error, which is the difference between the target and the position. you multiply kP by the Error to get the power. this should take the form of a while loop that breaks out when you reach the target. There are more advanced and faster things like PID, but P is more enough for starting.

1 Like
7

velocity=(error/intended distance)*100?

8

no.

velocity = error * kP
error = intended - current

1 Like
9

Is kP a known value?

10

Sorry its taking me a bit, this is my first coding project.

11

kp is a constant you need to figure out.

1 Like
12

I see

Is there anyone who knows how to get the errors to just go away so I can test?
I will try and learn the p loop stuff if it does not work

13

Fixed, the issue was simple capitalization and naming errors.

void straight(double d, int fastness){
    XAxisEncoder.setPosition(0, degrees);
    ZAxisEncoder.setPosition(0, degrees);
    Inertial1.setHeading(180, degrees);
    DrivetrainSpeed(fastness);
    FrontLeft.spin(vex::directionType::fwd, (fastness * ((180-(Inertial1.heading(degrees)-180))/180))+XAxisEncoder.position(degrees),  vex::velocityUnits::pct);
    FrontRight.spin(vex::directionType::fwd, (fastness * ((180+(Inertial1.heading(degrees)-180))/180))-XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearLeft.spin(vex::directionType::fwd, (fastness * ((180-(Inertial1.heading(degrees)-180))/180))-XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearRight.spin(vex::directionType::fwd, (fastness * ((180+(Inertial1.heading(degrees)-180))/180))+XAxisEncoder.position(degrees), vex::velocityUnits::pct);
    while (!(ZAxisEncoder.position(turns) == d/12.56637)){
      wait(1,msec);
      break;
    }
    FrontLeft.stop();
    FrontRight.stop();
    RearLeft.stop();
    RearRight.stop();
    DrivetrainSpeed(100);
}

void side(double d, int fastness){
    Inertial1.setHeading(180, degrees);
    XAxisEncoder.setPosition(0, degrees);
    ZAxisEncoder.setPosition(0, degrees);
    DrivetrainSpeed(fastness);
    FrontLeft.spin(vex::directionType::fwd, (fastness * ((180-(Inertial1.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees),  vex::velocityUnits::pct);
    FrontRight.spin(vex::directionType::rev, (fastness * ((180-(Inertial1.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearLeft.spin(vex::directionType::rev, (fastness * ((180+(Inertial1.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    RearRight.spin(vex::directionType::fwd, (fastness * ((180+(Inertial1.heading(degrees)-180))/180))-ZAxisEncoder.position(degrees), vex::velocityUnits::pct);
    while (!(XAxisEncoder.position(turns) == d/12.56637)){
      wait(1,msec);
      break;
    }
    FrontLeft.stop();
    FrontRight.stop();
    RearLeft.stop();
    RearRight.stop();
    DrivetrainSpeed(100);
}
1 Like