Vex 3 axis CNC machine program acting unexpectedly

Programming Support VEXcode Pro V5 Text Tech Support
#1

So, in my Programming / Robotics class we are making a “3 axis cnc machine”, where the objective is to write letters with a marker on paper. i decided it would be a good idea to write some functions that work with multiple motors at once to simplify the programming and prevent it from completely being a long list of

 XMotor.startSpinToPosition(foo, degrees); Ymotor.spinTo(foo2, degrees);

and so on.
the robot has 3 axes of movement. the x and y axes move the pen over the paper, and the z axis controls if the pen is touching the paper or above the paper.

my program is as follows:

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       DietMilk                                            */
/*    Created:      Wed Feb 24 2021                                           */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// Xmotor               motor         1               
// Ymotor               motor         2               
// Zmotor               motor         3               
// ---- END VEXCODE CONFIGURED DEVICES ----

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

void goTo(int x, int y, int z) {
  Zmotor.spinToPosition(z, degrees);

  Xmotor.setVelocity(
      std::abs(0.25*(x - Xmotor.position(degrees) / y - Ymotor.position(degrees))),
      percent);
  Ymotor.setVelocity(
      std::abs(0.25*(y - Ymotor.position(degrees) / x - Xmotor.position(degrees))),
      percent);

  Ymotor.startSpinTo(y, degrees);
  Xmotor.spinToPosition(x, degrees);
  task::sleep(2000);
}
// allows free movement in any of the 3 axes of the machine.

void goFor(int x, int y, int z) {
  Zmotor.spinToPosition(z, degrees);

  Xmotor.setVelocity(
      std::abs((x - Xmotor.position(degrees) / y - Ymotor.position(degrees))),
      percent);
  Ymotor.setVelocity(
      std::abs((y - Ymotor.position(degrees) / x - Xmotor.position(degrees))),
      percent);

  Ymotor.startSpinFor(y, degrees);
  Xmotor.spinFor(x, degrees);
}
// goFor went unused, it is easier to draw letters based upon absolute points
//curves would be useful for smoother letters. 
void reset() {
  Xmotor.setPosition(0, degrees);
  Ymotor.setPosition(60, degrees);
}
const int home = 360;
// home is where pen touches paper.

int main() {
  // Initializing Robot Configuration. DO NOT REMOV1E!
  vexcodeInit();
  Xmotor.setStopping(hold);
  Ymotor.setStopping(hold);
  Zmotor.setStopping(hold);

  goTo(0, 60, 0);
  reset();
 // goes to the "home" position of the letters.
  Brain.Screen.clearScreen();
  Brain.Screen.printAt(20, 20, "Start Draw letter A");
// start of first letter
  goTo(15, 0, home);
  goTo(30, 60, home);
  goTo(0, 30, 0);
  goTo(30, 30, home); // should draw letter A
                      // end of first letter
  Brain.Screen.printAt(50, 50, "resetting x and y to start positions");
  goTo(50, 60, 0);
  reset();
  Brain.Screen.clearScreen();
  Brain.Screen.printAt(20, 20, "Start Draw Letter B");
// start of second letter
  goTo(0, 0, home);
  goTo(30, 15, home);
  goTo(0, 30, home);
  goTo(30, 45, home);
  goTo(0, 60, home); // should draw letter B
                     // end of second letter
  Brain.Screen.printAt(50, 50, "resetting x and y to start positions");
  goTo(50, 60, 0);
  reset();
  Brain.Screen.clearScreen();
  Brain.Screen.printAt(20, 20, "Start Draw letter C");
// start of third letter
  goTo(0, 0, home);
  goTo(30, 0, home);
  goTo(30, 60, 0);
  goTo(0, 60, home); // should draw letter C
                     // end of third letter
  Brain.Screen.printAt(50, 50, "resetting x and y to start positions");
  goTo(50, 60, 0);
  reset();
}

the “goTo” function is the primary one used, and it gets unintended and unpredictable results from testing. i believe it is from setting the velocities of the motors, which i did so that the motors would have the right ratio of velocities to draw a straight line to the end point, but i may have made a mistake in my math

void goTo(int x, int y, int z) {
  Zmotor.spinToPosition(z, degrees);

  Xmotor.setVelocity(
      std::abs(0.25*(x - Xmotor.position(degrees) / y - Ymotor.position(degrees))),
      percent);
  Ymotor.setVelocity(
      std::abs(0.25*(y - Ymotor.position(degrees) / x - Xmotor.position(degrees))),
      percent);

  Ymotor.startSpinTo(y, degrees);
  Xmotor.spinToPosition(x, degrees);
  task::sleep(2000);
}

anyways, there are probably 50 better ways to do this, so any help is greatly appreciated (if you can stand reading through my horrific code :slight_smile: )

2 Likes
#2

If you’re doing something that requires as much precision as a CNC machine , id highly recommend getting some sort of PID going. You’re going to run into a lot of slop in whatever mechanical system your using if it’s made of vex parts, so you need some sort of way to account for that error, since it’ll just accumulate over time otherwise

#3

Are these “results” replicable?

You probably don’t need a PID loop to just write letters, depending on the desired accuracy.

#4

Yes, the slop is going to be an issue, but I don’t think the error will accumulate over time, if they use absolute target position with spinToPosition(), and don’t reset motors or use relative targeting with spinFor().

Then final error after each step is going to be the same (within the bounds) based on the internal motor PID exit condition and mechanical slop of the system.

I believe that the unpredictability that @DietMilk is experiencing is due to the velocities that are set depending on the size of each step.

I think you can safely set constant velocity around 30-50% of the maximum and let V5 motor’s built-in PID algorithm, which is invoked by spinFor() and spinToPosition() type of functions, to control the exact way it moves.

Then, if you get it to work reasonably well, you could try to switch to your own custom PID algorithm and experiment with coefficients to get best accuracy and performance. V5 motors use default PID coefficients that are good enough for many various modes but are not optimal for any specific one (except for the hypothetical average case).

Edit: I guess, I have to add for extra clarity that V5 built-in PID has exit condition that may not necessarily stop at the exact target:

If this wasn’t the case then it wouldn’t make any difference in using relative or absolute targeting with spinFor() or spinToPosition().

5 Likes