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 )