Guest User

Untitled

a guest
Sep 24th, 2023
210
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 12.28 KB | None | 0 0
  1. #include <AccelStepper.h>
  2. #include <MultiStepper.h>
  3. #include <limits.h>
  4.  
  5.  
  6. AccelStepper stepper1(AccelStepper::DRIVER, 3, 2);    // Main Stepper 1
  7. AccelStepper stepper2(AccelStepper::DRIVER, 5, 4);    // Main Stepepr 2
  8. AccelStepper stepper3(AccelStepper::DRIVER, 7, 6);    // Main Stepepr 2
  9. AccelStepper stepper7(AccelStepper::DRIVER, 9, 8);    // Main Stepepr 7
  10.  
  11. MultiStepper steppers;
  12.  
  13. int counterA;
  14.  
  15. // Set up Stepper Microstep pulse
  16. int microStepSetup[6];
  17.  
  18. void setup() {
  19.   // put your setup code here, to run once:
  20.  
  21.  
  22.   microStepSetup[0] = 800;
  23.   microStepSetup[1] = 800;
  24.   microStepSetup[2] = 800;
  25.   microStepSetup[3] = 200;
  26.  
  27.   // Stepper 1
  28.   pinMode(2, OUTPUT); // Dir
  29.   pinMode(3, OUTPUT); // Pulse
  30.  
  31.   // Stepper 2
  32.   pinMode(4, OUTPUT); // Dir
  33.   pinMode(5, OUTPUT); // Pulse
  34.  
  35.   // Stepper 3
  36.   pinMode(6, OUTPUT); // Dir
  37.   pinMode(7, OUTPUT); // Pulse
  38.  
  39.    // Stepper 7
  40.   pinMode(8, OUTPUT); // Dir
  41.   pinMode(9, OUTPUT); // Pulse
  42.  
  43.   stepper1.setMaxSpeed(4000.0);
  44.   stepper2.setMaxSpeed(4000.0);
  45.   stepper3.setMaxSpeed(4000.0);
  46.   stepper7.setMaxSpeed(4000.0);
  47.  
  48.   stepper7.setMinPulseWidth(500);
  49.  
  50.   Serial.begin(9600);
  51.   //Serial.setTimeout(500);
  52. }
  53.  
  54. int degreeToStep(long degree, int stepResolution){
  55.   float resolution =  360 / (float) stepResolution;
  56.   Serial.print("Resolution; " + String(resolution) + "\n");
  57.   Serial.print("degree: " + String((float) degree) + "\n");
  58.  
  59.   if(degree == -1){
  60.     return -1;
  61.   }
  62.  
  63.  
  64.   float resolvedSteps = ( ( (resolution * degree) / 10 ) ) / 2 * 100;
  65.  
  66.   Serial.print("returning  " + String( (int)resolvedSteps  ) + "\n");
  67.   return (int) resolvedSteps;
  68. }
  69.  
  70. float calcSync(float maxSpeed, float maxAccel, float maxDistance, AccelStepper stepper){
  71.  
  72.   // Ignore motor if it doesnt travel
  73.   if(stepper.distanceToGo() == 0){
  74.     return 0;
  75.   }
  76.  
  77.   // Get positive travel number for calculating
  78.   long stepperDis = stepper.distanceToGo();
  79.   if(stepperDis < 0){ stepperDis = (abs(stepperDis));  }
  80.  
  81.   // Works best so far
  82.   maxAccel = maxSpeed / 5;
  83.  
  84.   float speedOnePerc = (100 / maxDistance);                            // Get Acceleration Speed 1%
  85.   float usageSpeedPerc = speedOnePerc * stepperDis;                    // Calculate speed in percent for stepper
  86.   float stepperSpeed = (maxAccel / 100) * usageSpeedPerc;              // Get final acceleration
  87.   if(stepperSpeed < 0){ stepperSpeed = (abs(stepperSpeed));  }
  88.  
  89.  
  90.   return stepperSpeed;
  91. }
  92.  
  93. void MOVE(float maxSpeed, float accel){
  94.  
  95.   int allSpeeds[3];
  96.  
  97.   // Fuck me
  98.   if(stepper1.distanceToGo() < 0){
  99.     allSpeeds[0] = abs(stepper1.distanceToGo());
  100.   }
  101.   else{
  102.     allSpeeds[0] = (stepper1.distanceToGo());
  103.   }
  104.  
  105.   if(stepper2.distanceToGo() < 0){
  106.     allSpeeds[1] = abs(stepper2.distanceToGo());
  107.   }
  108.   else{
  109.     allSpeeds[1] = (stepper2.distanceToGo());
  110.   }
  111.  
  112.   if(stepper3.distanceToGo() < 0){
  113.     allSpeeds[2] = abs(stepper3.distanceToGo());
  114.   }
  115.   else{
  116.     allSpeeds[2] = (stepper3.distanceToGo());
  117.   }
  118.  
  119.   if(stepper7.distanceToGo() < 0){
  120.     allSpeeds[3] = abs(stepper7.distanceToGo());
  121.   }
  122.   else{
  123.     allSpeeds[3] = (stepper7.distanceToGo());
  124.   }
  125.  
  126.  
  127.   float biggestDistance = findBiggestNumIntArray(allSpeeds, "max");
  128.  
  129.   if(biggestDistance == 0) { biggestDistance = stepper1.distanceToGo(); }
  130.      stepper1.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper1));
  131.      
  132.   if(biggestDistance == 0) { biggestDistance = stepper2.distanceToGo(); }
  133.      stepper2.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper2));
  134.      
  135.   if(biggestDistance == 0) { biggestDistance = stepper3.distanceToGo(); }
  136.      stepper3.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper3));
  137.  
  138.   if(biggestDistance == 0) { biggestDistance = stepper7.distanceToGo(); }
  139.      stepper7.setAcceleration(calcSync(maxSpeed, accel, biggestDistance, stepper7));
  140.  
  141.  
  142. /*  
  143.   Serial.print("Stepper 2 distance: " + String(allSpeeds[1]) + "\n");
  144.   Serial.print("Stepper 2 accel: " + String(calcSync(maxSpeed, accel, biggestDistance, stepper2)) + "\n\n");
  145.  
  146.   Serial.print("Stepper 3 distance: " + String(allSpeeds[2]) + "\n");
  147.   Serial.print("Stepper 3 accel: " + String(calcSync(maxSpeed, accel, biggestDistance, stepper3)) + "\n\n");
  148. */
  149.   //Serial.print("Biggest number: " + String(findBiggestNumIntArray(allSpeeds, "max")) + "\n");
  150.  
  151.  
  152.   while(stepper1.distanceToGo() != 0 ||
  153.   stepper2.distanceToGo() != 0 ||
  154.   stepper3.distanceToGo() != 0 ||
  155.   stepper7.distanceToGo() != 0 ){
  156.  
  157.        
  158.     if(stepper1.distanceToGo() != 0){
  159.       stepper1.run();
  160.     }
  161.  
  162.     if(stepper2.distanceToGo() != 0){
  163.       stepper2.run();
  164.     }
  165.  
  166.     if(stepper3.distanceToGo() != 0){
  167.       stepper3.run();
  168.     }
  169.  
  170.     if(stepper7.distanceToGo() != 0){
  171.       stepper7.run();
  172.     }
  173.    
  174.   }
  175.  
  176. }
  177.  
  178. int findBiggestNumIntArray(int myArray[], String option) {
  179.  
  180.  
  181.    int maxVal = myArray[0];
  182.    int minVal = myArray[0];
  183.    //Serial.print("Size of myArray is: "); Serial.println(sizeof(myArray));
  184.    
  185.    for (int i = 0; i < (sizeof(myArray) * sizeof(myArray)); i++) {
  186.       maxVal = max(myArray[i],maxVal);
  187.       minVal = min(myArray[i],minVal);
  188.    }
  189.  
  190.    //Serial.print("The maximum value of the array is: ");
  191.    //Serial.println(maxVal);
  192.    //Serial.print("The minimum value of the array is: ");
  193.    //Serial.println(minVal);
  194.  
  195.   return maxVal;
  196. }
  197.  
  198. String command;
  199. String type;
  200.  
  201. // Command Parameter
  202. char ss = '#';
  203.  
  204. // Command Transmission Seperator
  205. char mss = ':';
  206.  
  207. void setupMotor(long target,int motor,int speedy,int accel){
  208.   if(target != -1 && motor == 0){
  209.       stepper1.moveTo(calcDistance(target, microStepSetup[motor]));
  210.       stepper1.setMaxSpeed(speedy);
  211.       stepper1.setAcceleration(accel);
  212.   }
  213.  
  214.   if(target != -1 && motor == 1){
  215.       stepper2.moveTo(calcDistance(target, microStepSetup[motor]));
  216.       stepper2.setMaxSpeed(speedy);
  217.       stepper2.setAcceleration(accel);
  218.   }
  219.  
  220.   if(target != -1 && motor == 2){
  221.       stepper3.moveTo(calcDistance(target, microStepSetup[motor]));
  222.       stepper3.setMaxSpeed(speedy);
  223.       stepper3.setAcceleration(accel);
  224.   }
  225.  
  226.   if(target != -1 && motor == 3){
  227.       stepper7.moveTo(calcDistance(target, microStepSetup[motor]));
  228.       stepper7.setMaxSpeed(speedy);
  229.       stepper7.setAcceleration(accel);
  230.   }
  231. }
  232.  
  233. long calcDistance(long steps, int stepperMicro){
  234.   float biggestMicroStep = findBiggestNumIntArray(microStepSetup, "max");
  235.   long stepperDis = steps;
  236.  
  237.   // Translate Different Microstep Setups
  238.   if(stepperMicro < biggestMicroStep){
  239.     float revFactor = biggestMicroStep / stepperMicro;
  240.     stepperDis = stepperDis / revFactor;
  241.   }
  242.  
  243. Serial.print("Axis 1 new dis: " + String(stepperDis) + "\n");
  244.   return stepperDis;
  245. }
  246.  
  247. void error(String title, String text, String type){
  248.  
  249. }
  250.  
  251. void prepairMovement(String type, char ss){
  252.  
  253.           // All the positions
  254.           long target[3];
  255.          
  256.           // Motor 1
  257.           int axis1_degrees = getData(getData(type, ss, 1), ',', 0).toInt();
  258.           target[0] = (long) degreeToStep(axis1_degrees, microStepSetup[0]);
  259.  
  260.           Serial.print("Axis 1 Degrees: " + String(axis1_degrees) + "\n");
  261.           Serial.print("Axis 1 Steps: " + String(target[0]) + "\n");
  262.  
  263.  
  264.           if(axis1_degrees > 360){
  265.             return;
  266.           }
  267.  
  268.  
  269.           // Motor 2
  270.           long axis2_degrees = (unsigned long) getData(getData(type, ss, 1), ',', 1).toInt();
  271.           target[1] = degreeToStep(axis2_degrees, microStepSetup[1]);
  272.  
  273.           if(axis2_degrees > 90 || axis2_degrees < -90){
  274.             return;
  275.           }
  276.  
  277.           // Motor 3
  278.           long axis3_degrees = (unsigned long) getData(getData(type, ss, 1) + "0", ',', 2).toInt();
  279.           target[2] = degreeToStep(axis3_degrees, microStepSetup[2]);  
  280.  
  281.           if(axis3_degrees > 360){
  282.             return;
  283.           }
  284.  
  285.  
  286.  
  287.           // Motor 7
  288.           long axis7_degrees = (unsigned long) getData(getData(type, ss, 1) + "0", ',', 3).toInt();
  289.           target[3] = axis7_degrees;
  290.  
  291.           // Flip Axis 3 Direction
  292.           if(target[2] > 0){
  293.             target[2] = target[2] * (-1);
  294.           }
  295.           else if(target[2] < 0){
  296.             target[2] = abs(target[2]);
  297.           }
  298.  
  299.           // Flip Axis 2 Direction
  300.           if(target[1] > 0){
  301.             target[1] = target[1] * (-1);
  302.           }
  303.           else if(target[1] < 0){
  304.             target[1] = abs(target[1]);
  305.           }
  306.  
  307.           long speeds = getData(type, ss, 2).toInt();
  308.           long accel = getData(type, ss, 3).toInt();
  309.           int motor = getData(type, ss, 4).toInt();
  310.  
  311.           setupMotor(target[0] * 10, 0, speeds, accel);
  312.           setupMotor(target[1] * 20, 1, speeds, accel);
  313.           setupMotor(target[2] * 10, 2, speeds, accel);
  314.           setupMotor(target[3], 3, speeds, accel);
  315.  
  316.        
  317.           //Serial.print("Target: " + String(target) + "~\n");
  318.           //Serial.print("Speed: " + String(speeds) + "~\n");
  319.           //Serial.print("Accel: " + String(accel) + "~\n");
  320.           //Serial.print("Motor: " + String(motor) + "~~\n\n");
  321.           //Serial.print("\n");
  322.  
  323.           //steppers.moveTo(target);
  324.           //steppers.runSpeedToPosition();          
  325.           MOVE(speeds, accel);
  326. }
  327.  
  328. void loop() {
  329.   // put your main code here, to run repeatedly:
  330.  
  331.   if(Serial.available() > 0) {
  332.    
  333.     command = command + Serial.readString();
  334.  
  335.     if(command.indexOf("<EOT>") != -1){
  336.      
  337.       for(int i = 0; i < stringCounter(command, mss); i++) {
  338.  
  339.         type = getData(command, mss, i);    
  340.  
  341.         //Serial.print("Command Detected: " + type + " ~");
  342.  
  343.       long target[2];
  344.        if(type.indexOf("MOVE") != -1){
  345.  
  346.           //Serial.print("MOVE COMMAND~\n");          
  347.  
  348.           /*
  349.            * Get the input in degrees, convert it to steps
  350.            * Both are steps and degrees are important for checks etc
  351.            *
  352.            */
  353.  
  354.            // Multiple commands
  355.            if(command.indexOf("$") != -1){
  356.               int commandCounter = stringCounter(command, '$');
  357.  
  358.               for(int i = 0; i < commandCounter+1; i++){
  359.                 Serial.print("Command: " + String(i) + " "  + getData(command, '~', i) + "\n");
  360.                 prepairMovement(type,ss);
  361.               }
  362.           }
  363.           else{
  364.             prepairMovement(type,ss);
  365.           }
  366.  
  367.          
  368.        } // End Command Move Check
  369.       else if(type.indexOf("NULL") != -1){
  370.           stepper1.setCurrentPosition(0);
  371.           stepper2.setCurrentPosition(0);
  372.           stepper3.setCurrentPosition(0);
  373.       }
  374.       else if(type.indexOf("BREAKS") != -1){
  375.         // Get values.
  376.         int breaks = getData(type, ss, 1).toInt();
  377.         int motor = getData(type, ss, 2).toInt();
  378.  
  379.         // If break equals 1, Free the Motor
  380.         //  If motor equals 0, Free Motor 1
  381.         //  If motor equals 1, Free Motor 2
  382.         if(breaks == 1){
  383.  
  384.           if(motor == 0) {
  385.             digitalWrite(6,HIGH); // Motor 1
  386.           }
  387.           else {
  388.             digitalWrite(9,HIGH); // Motor 2
  389.           }
  390.         }
  391.         else{
  392.           if(motor == 0) {
  393.             digitalWrite(6,LOW); // Motor 1
  394.           }
  395.           else {
  396.             digitalWrite(9,LOW); // Motor 2
  397.           }
  398.         }
  399.  
  400.         // Lil Debug
  401.         Serial.print("Breaks set to " + String(breaks) + " for motor " + String(motor) + "~");
  402.       }
  403.       else{
  404.         Serial.print("Type:" + String(type) + "~");
  405.       }
  406.        
  407.       } // Foreach Command end
  408.  
  409.       Serial.print("POK~");
  410.       command = "";
  411.     } // End EOT Signal IF
  412.     else{
  413.       Serial.print("No EOF Signal detected~");
  414.     }
  415.    
  416.   } // End of Serial
  417.  
  418. }
  419.  
  420.  
  421.  
  422.  
  423.  
  424.  
  425. int stringCounter(String input, char search){
  426.  
  427.   // Count the specific characters in a string
  428.   int counter = 0;
  429.   for(auto x : input)
  430.   {
  431.     if(x == search){
  432.       counter++;
  433.     }
  434.   }
  435.  
  436.   return counter;
  437. }
  438.  
  439. String getData(String data, char delimiter, int sequence)
  440. {
  441.   // Used to magically get specific strings from a big string
  442.   int  stringData = 0;
  443.   String sectionData = "";
  444.  
  445.   for (int i = 0; i < data.length() - 1; i++)
  446.   {
  447.  
  448.     if (data[i] == delimiter)
  449.     {
  450.       stringData++;
  451.     }
  452.  
  453.     else if (stringData == sequence)
  454.     {
  455.       sectionData.concat(data[i]);
  456.     }
  457.  
  458.     else if (stringData > sequence)
  459.     {
  460.       return sectionData;
  461.       break;
  462.     }
  463.   }
  464.  
  465.   return sectionData;
  466. }
Add Comment
Please, Sign In to add comment