Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <AccelStepper.h>
- #include <MultiStepper.h>
- #include <limits.h>
- AccelStepper stepper1(AccelStepper::DRIVER, 3, 2); // Main Stepper 1
- AccelStepper stepper2(AccelStepper::DRIVER, 5, 4); // Main Stepepr 2
- AccelStepper stepper3(AccelStepper::DRIVER, 7, 6); // Main Stepepr 2
- AccelStepper stepper7(AccelStepper::DRIVER, 9, 8); // Main Stepepr 7
- MultiStepper steppers;
- int counterA;
- // Set up Stepper Microstep pulse
- int microStepSetup[6];
- void setup() {
- // put your setup code here, to run once:
- microStepSetup[0] = 800;
- microStepSetup[1] = 800;
- microStepSetup[2] = 800;
- microStepSetup[3] = 200;
- // Stepper 1
- pinMode(2, OUTPUT); // Dir
- pinMode(3, OUTPUT); // Pulse
- // Stepper 2
- pinMode(4, OUTPUT); // Dir
- pinMode(5, OUTPUT); // Pulse
- // Stepper 3
- pinMode(6, OUTPUT); // Dir
- pinMode(7, OUTPUT); // Pulse
- // Stepper 7
- pinMode(8, OUTPUT); // Dir
- pinMode(9, OUTPUT); // Pulse
- stepper1.setMaxSpeed(4000.0);
- stepper2.setMaxSpeed(4000.0);
- stepper3.setMaxSpeed(4000.0);
- stepper7.setMaxSpeed(4000.0);
- stepper7.setMinPulseWidth(500);
- Serial.begin(9600);
- //Serial.setTimeout(500);
- }
- int degreeToStep(long degree, int stepResolution){
- float resolution = 360 / (float) stepResolution;
- Serial.print("Resolution; " + String(resolution) + "\n");
- Serial.print("degree: " + String((float) degree) + "\n");
- if(degree == -1){
- return -1;
- }
- float resolvedSteps = ( ( (resolution * degree) / 10 ) ) / 2 * 100;
- Serial.print("returning " + String( (int)resolvedSteps ) + "\n");
- return (int) resolvedSteps;
- }
- float calcSync(float maxSpeed, float maxAccel, float maxDistance, AccelStepper stepper){
- // Ignore motor if it doesnt travel
- if(stepper.distanceToGo() == 0){
- return 0;
- }
- // Get positive travel number for calculating
- long stepperDis = stepper.distanceToGo();
- if(stepperDis < 0){ stepperDis = (abs(stepperDis)); }
- // Works best so far
- maxAccel = maxSpeed / 5;
- float speedOnePerc = (100 / maxDistance); // Get Acceleration Speed 1%
- float usageSpeedPerc = speedOnePerc * stepperDis; // Calculate speed in percent for stepper
- float stepperSpeed = (maxAccel / 100) * usageSpeedPerc; // Get final acceleration
- if(stepperSpeed < 0){ stepperSpeed = (abs(stepperSpeed)); }
- return stepperSpeed;
- }
- void MOVE(float maxSpeed, float accel){
- int allSpeeds[3];
- // Fuck me
- if(stepper1.distanceToGo() < 0){
- allSpeeds[0] = abs(stepper1.distanceToGo());
- }
- else{
- allSpeeds[0] = (stepper1.distanceToGo());
- }
- if(stepper2.distanceToGo() < 0){
- allSpeeds[1] = abs(stepper2.distanceToGo());
- }
- else{
- allSpeeds[1] = (stepper2.distanceToGo());
- }
- if(stepper3.distanceToGo() < 0){
- allSpeeds[2] = abs(stepper3.distanceToGo());
- }
- else{
- allSpeeds[2] = (stepper3.distanceToGo());
- }
- if(stepper7.distanceToGo() < 0){
- allSpeeds[3] = abs(stepper7.distanceToGo());
- }
- else{
- allSpeeds[3] = (stepper7.distanceToGo());
- }
- float biggestDistance = findBiggestNumIntArray(allSpeeds, "max");
- if(biggestDistance == 0) { biggestDistance = stepper1.distanceToGo(); }
- stepper1.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper1));
- if(biggestDistance == 0) { biggestDistance = stepper2.distanceToGo(); }
- stepper2.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper2));
- if(biggestDistance == 0) { biggestDistance = stepper3.distanceToGo(); }
- stepper3.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper3));
- if(biggestDistance == 0) { biggestDistance = stepper7.distanceToGo(); }
- stepper7.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper7));
- /*
- Serial.print("Stepper 2 distance: " + String(allSpeeds[1]) + "\n");
- Serial.print("Stepper 2 accel: " + String(calcSync(maxSpeed, accel, biggestDistance, stepper2)) + "\n\n");
- Serial.print("Stepper 3 distance: " + String(allSpeeds[2]) + "\n");
- Serial.print("Stepper 3 accel: " + String(calcSync(maxSpeed, accel, biggestDistance, stepper3)) + "\n\n");
- */
- //Serial.print("Biggest number: " + String(findBiggestNumIntArray(allSpeeds, "max")) + "\n");
- while(stepper1.distanceToGo() != 0 ||
- stepper2.distanceToGo() != 0 ||
- stepper3.distanceToGo() != 0 ||
- stepper7.distanceToGo() != 0 ){
- if(stepper1.distanceToGo() != 0){
- stepper1.run();
- }
- if(stepper2.distanceToGo() != 0){
- stepper2.run();
- }
- if(stepper3.distanceToGo() != 0){
- stepper3.run();
- }
- if(stepper7.distanceToGo() != 0){
- stepper7.run();
- }
- }
- }
- int findBiggestNumIntArray(int myArray[], String option) {
- int maxVal = myArray[0];
- int minVal = myArray[0];
- //Serial.print("Size of myArray is: "); Serial.println(sizeof(myArray));
- for (int i = 0; i < (sizeof(myArray) * sizeof(myArray)); i++) {
- maxVal = max(myArray[i],maxVal);
- minVal = min(myArray[i],minVal);
- }
- //Serial.print("The maximum value of the array is: ");
- //Serial.println(maxVal);
- //Serial.print("The minimum value of the array is: ");
- //Serial.println(minVal);
- return maxVal;
- }
- String command;
- String type;
- // Command Parameter
- char ss = '#';
- // Command Transmission Seperator
- char mss = ':';
- void setupMotor(long target,int motor,int speedy,int accel){
- if(target != -1 && motor == 0){
- stepper1.moveTo(calcDistance(target, microStepSetup[motor]));
- stepper1.setMaxSpeed(speedy);
- stepper1.setAcceleration(accel);
- }
- if(target != -1 && motor == 1){
- stepper2.moveTo(calcDistance(target, microStepSetup[motor]));
- stepper2.setMaxSpeed(speedy);
- stepper2.setAcceleration(accel);
- }
- if(target != -1 && motor == 2){
- stepper3.moveTo(calcDistance(target, microStepSetup[motor]));
- stepper3.setMaxSpeed(speedy);
- stepper3.setAcceleration(accel);
- }
- if(target != -1 && motor == 3){
- stepper7.moveTo(calcDistance(target, microStepSetup[motor]));
- stepper7.setMaxSpeed(speedy);
- stepper7.setAcceleration(accel);
- }
- }
- long calcDistance(long steps, int stepperMicro){
- float biggestMicroStep = findBiggestNumIntArray(microStepSetup, "max");
- long stepperDis = steps;
- // Translate Different Microstep Setups
- if(stepperMicro < biggestMicroStep){
- float revFactor = biggestMicroStep / stepperMicro;
- stepperDis = stepperDis / revFactor;
- }
- Serial.print("Axis 1 new dis: " + String(stepperDis) + "\n");
- return stepperDis;
- }
- void error(String title, String text, String type){
- }
- void prepairMovement(String type, char ss){
- // All the positions
- long target[3];
- // Motor 1
- int axis1_degrees = getData(getData(type, ss, 1), ',', 0).toInt();
- target[0] = (long) degreeToStep(axis1_degrees, microStepSetup[0]);
- Serial.print("Axis 1 Degrees: " + String(axis1_degrees) + "\n");
- Serial.print("Axis 1 Steps: " + String(target[0]) + "\n");
- if(axis1_degrees > 360){
- return;
- }
- // Motor 2
- long axis2_degrees = (unsigned long) getData(getData(type, ss, 1), ',', 1).toInt();
- target[1] = degreeToStep(axis2_degrees, microStepSetup[1]);
- if(axis2_degrees > 90 || axis2_degrees < -90){
- return;
- }
- // Motor 3
- long axis3_degrees = (unsigned long) getData(getData(type, ss, 1) + "0", ',', 2).toInt();
- target[2] = degreeToStep(axis3_degrees, microStepSetup[2]);
- if(axis3_degrees > 360){
- return;
- }
- // Motor 7
- long axis7_degrees = (unsigned long) getData(getData(type, ss, 1) + "0", ',', 3).toInt();
- target[3] = axis7_degrees;
- // Flip Axis 3 Direction
- if(target[2] > 0){
- target[2] = target[2] * (-1);
- }
- else if(target[2] < 0){
- target[2] = abs(target[2]);
- }
- // Flip Axis 2 Direction
- if(target[1] > 0){
- target[1] = target[1] * (-1);
- }
- else if(target[1] < 0){
- target[1] = abs(target[1]);
- }
- long speeds = getData(type, ss, 2).toInt();
- long accel = getData(type, ss, 3).toInt();
- int motor = getData(type, ss, 4).toInt();
- setupMotor(target[0] * 10, 0, speeds, accel);
- setupMotor(target[1] * 20, 1, speeds, accel);
- setupMotor(target[2] * 10, 2, speeds, accel);
- setupMotor(target[3], 3, speeds, accel);
- //Serial.print("Target: " + String(target) + "~\n");
- //Serial.print("Speed: " + String(speeds) + "~\n");
- //Serial.print("Accel: " + String(accel) + "~\n");
- //Serial.print("Motor: " + String(motor) + "~~\n\n");
- //Serial.print("\n");
- //steppers.moveTo(target);
- //steppers.runSpeedToPosition();
- MOVE(speeds, accel);
- }
- void loop() {
- // put your main code here, to run repeatedly:
- if(Serial.available() > 0) {
- command = command + Serial.readString();
- if(command.indexOf("<EOT>") != -1){
- for(int i = 0; i < stringCounter(command, mss); i++) {
- type = getData(command, mss, i);
- //Serial.print("Command Detected: " + type + " ~");
- long target[2];
- if(type.indexOf("MOVE") != -1){
- //Serial.print("MOVE COMMAND~\n");
- /*
- * Get the input in degrees, convert it to steps
- * Both are steps and degrees are important for checks etc
- *
- */
- // Multiple commands
- if(command.indexOf("$") != -1){
- int commandCounter = stringCounter(command, '$');
- for(int i = 0; i < commandCounter+1; i++){
- Serial.print("Command: " + String(i) + " " + getData(command, '~', i) + "\n");
- prepairMovement(type,ss);
- }
- }
- else{
- prepairMovement(type,ss);
- }
- } // End Command Move Check
- else if(type.indexOf("NULL") != -1){
- stepper1.setCurrentPosition(0);
- stepper2.setCurrentPosition(0);
- stepper3.setCurrentPosition(0);
- }
- else if(type.indexOf("BREAKS") != -1){
- // Get values.
- int breaks = getData(type, ss, 1).toInt();
- int motor = getData(type, ss, 2).toInt();
- // If break equals 1, Free the Motor
- // If motor equals 0, Free Motor 1
- // If motor equals 1, Free Motor 2
- if(breaks == 1){
- if(motor == 0) {
- digitalWrite(6,HIGH); // Motor 1
- }
- else {
- digitalWrite(9,HIGH); // Motor 2
- }
- }
- else{
- if(motor == 0) {
- digitalWrite(6,LOW); // Motor 1
- }
- else {
- digitalWrite(9,LOW); // Motor 2
- }
- }
- // Lil Debug
- Serial.print("Breaks set to " + String(breaks) + " for motor " + String(motor) + "~");
- }
- else{
- Serial.print("Type:" + String(type) + "~");
- }
- } // Foreach Command end
- Serial.print("POK~");
- command = "";
- } // End EOT Signal IF
- else{
- Serial.print("No EOF Signal detected~");
- }
- } // End of Serial
- }
- int stringCounter(String input, char search){
- // Count the specific characters in a string
- int counter = 0;
- for(auto x : input)
- {
- if(x == search){
- counter++;
- }
- }
- return counter;
- }
- String getData(String data, char delimiter, int sequence)
- {
- // Used to magically get specific strings from a big string
- int stringData = 0;
- String sectionData = "";
- for (int i = 0; i < data.length() - 1; i++)
- {
- if (data[i] == delimiter)
- {
- stringData++;
- }
- else if (stringData == sequence)
- {
- sectionData.concat(data[i]);
- }
- else if (stringData > sequence)
- {
- return sectionData;
- break;
- }
- }
- return sectionData;
- }
Add Comment
Please, Sign In to add comment