BnEc

Acceleration for multiple stepper motors

Feb 16th, 2018
90
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 4.26 KB | None | 0 0
  1. #define DIR_PIN1          2
  2. #define STEP_PIN1         3
  3. #define ENABLE_PIN1       4
  4. #define DIR_PIN2          5
  5. #define STEP_PIN2         6
  6. #define ENABLE_PIN2       7
  7. #define DIR_PIN3          8
  8. #define STEP_PIN3         9
  9. #define ENABLE_PIN3       10
  10. #define DIR_PIN4          11
  11. #define STEP_PIN4         12
  12. #define ENABLE_PIN4       13
  13. #define DIR_PIN5          14
  14. #define STEP_PIN5         15
  15. #define ENABLE_PIN5       16
  16.  
  17. void setup() {
  18.   pinMode(STEP_PIN1,   OUTPUT);
  19.   pinMode(DIR_PIN1,    OUTPUT);
  20.   pinMode(ENABLE_PIN1, OUTPUT);
  21.   pinMode(STEP_PIN2,   OUTPUT);
  22.   pinMode(DIR_PIN2,    OUTPUT);
  23.   pinMode(ENABLE_PIN2, OUTPUT);
  24.   pinMode(STEP_PIN3,   OUTPUT);
  25.   pinMode(DIR_PIN3,    OUTPUT);
  26.   pinMode(ENABLE_PIN3, OUTPUT);
  27.   pinMode(STEP_PIN4,   OUTPUT);
  28.   pinMode(DIR_PIN4,    OUTPUT);
  29.   pinMode(ENABLE_PIN4, OUTPUT);
  30.   pinMode(STEP_PIN5,   OUTPUT);
  31.   pinMode(DIR_PIN5,    OUTPUT);
  32.   pinMode(ENABLE_PIN5, OUTPUT);
  33. }
  34.  
  35. #define STEPS 3000
  36.  
  37. void constantAccel() {
  38.   int delays[STEPS];
  39.   float angle = 6;
  40.   float accel = 0.05;
  41.   float c0 = 5000 * sqrt( 2 * angle / accel ) * 0.9000;
  42.   float lastDelay = 0;
  43.   int highSpeed = 100000;
  44.   for (int i = 0; i < STEPS; i++) {
  45.     float d = c0;
  46.     if ( i > 0 )
  47.       d = lastDelay - (2 * lastDelay) / (4 * i + 1);
  48.     if ( d < highSpeed )
  49.       d = highSpeed;
  50.     delays[i] = d;
  51.     lastDelay = d;
  52.   }
  53.  
  54.   // use delays from the array, forward
  55.   for (int i = 0; i < STEPS; i++) {
  56.     digitalWrite(STEP_PIN1, HIGH);
  57.     delayMicroseconds( delays[i] );
  58.     digitalWrite(STEP_PIN1, LOW);
  59.   }
  60.  
  61.   // use delays from the array, backward
  62.   for (int i = 0; i < STEPS; i++) {
  63.     digitalWrite(STEP_PIN1, HIGH);
  64.     delayMicroseconds( delays[STEPS-i-1] );
  65.     digitalWrite(STEP_PIN1, LOW);
  66.   }
  67.  
  68.    // use delays from the array, forward
  69.   for (int i = 0; i < STEPS; i++) {
  70.     digitalWrite(STEP_PIN2, HIGH);
  71.     delayMicroseconds( delays[i] );
  72.     digitalWrite(STEP_PIN2, LOW);
  73.   }
  74.  
  75.   // use delays from the array, backward
  76.   for (int i = 0; i < STEPS; i++) {
  77.     digitalWrite(STEP_PIN2, HIGH);
  78.     delayMicroseconds( delays[STEPS-i-1] );
  79.     digitalWrite(STEP_PIN2, LOW);
  80.   }
  81.  
  82.    // use delays from the array, forward
  83.   for (int i = 0; i < STEPS; i++) {
  84.     digitalWrite(STEP_PIN3, HIGH);
  85.     delayMicroseconds( delays[i] );
  86.     digitalWrite(STEP_PIN3, LOW);
  87.   }
  88.  
  89.   // use delays from the array, backward
  90.   for (int i = 0; i < STEPS; i++) {
  91.     digitalWrite(STEP_PIN3, HIGH);
  92.     delayMicroseconds( delays[STEPS-i-1] );
  93.     digitalWrite(STEP_PIN3, LOW);
  94.   }
  95.      // use delays from the array, forward
  96.   for (int i = 0; i < STEPS; i++) {
  97.     digitalWrite(STEP_PIN4, HIGH);
  98.     delayMicroseconds( delays[i] );
  99.     digitalWrite(STEP_PIN4, LOW);
  100.   }
  101.  
  102.   // use delays from the array, backward
  103.   for (int i = 0; i < STEPS; i++) {
  104.     digitalWrite(STEP_PIN4, HIGH);
  105.     delayMicroseconds( delays[STEPS-i-1] );
  106.     digitalWrite(STEP_PIN4, LOW);
  107.   }
  108.  
  109.    // use delays from the array, forward
  110.   for (int i = 0; i < STEPS; i++) {
  111.     digitalWrite(STEP_PIN5, HIGH);
  112.     delayMicroseconds( delays[i] );
  113.     digitalWrite(STEP_PIN5, LOW);
  114.   }
  115.  
  116.   // use delays from the array, backward
  117.   for (int i = 0; i < STEPS; i++) {
  118.     digitalWrite(STEP_PIN5, HIGH);
  119.     delayMicroseconds( delays[STEPS-i-1] );
  120.     digitalWrite(STEP_PIN5, LOW);
  121.   }
  122.  
  123. }
  124. void loop() {
  125.  
  126.   digitalWrite(DIR_PIN1, LOW);
  127.   constantAccel();
  128.   digitalWrite(DIR_PIN1, HIGH);
  129.   constantAccel();
  130.   digitalWrite(DIR_PIN2, LOW);
  131.   constantAccel();
  132.   digitalWrite(DIR_PIN2, HIGH);
  133.   constantAccel();
  134.   digitalWrite(DIR_PIN3, LOW);
  135.   constantAccel();
  136.   digitalWrite(DIR_PIN3, HIGH);
  137.   constantAccel();
  138.   digitalWrite(DIR_PIN4, LOW);
  139.   constantAccel();
  140.   digitalWrite(DIR_PIN4, HIGH);
  141.   constantAccel();
  142.   digitalWrite(DIR_PIN5, LOW);
  143.   constantAccel();
  144.   digitalWrite(DIR_PIN5, HIGH);
  145.   constantAccel();
  146.   digitalWrite(DIR_PIN4, LOW);
  147.   constantAccel();
  148.   digitalWrite(DIR_PIN4, HIGH);
  149.   constantAccel();
  150.   digitalWrite(DIR_PIN3, LOW);
  151.   constantAccel();
  152.   digitalWrite(DIR_PIN3, HIGH);
  153.   constantAccel();
  154.   digitalWrite(DIR_PIN2, LOW);
  155.   constantAccel();
  156.   digitalWrite(DIR_PIN2, HIGH);
  157.   constantAccel();
  158.   digitalWrite(DIR_PIN1, LOW);
  159.   constantAccel();
  160.   digitalWrite(DIR_PIN1, HIGH);
  161.   constantAccel();
  162.  
  163.   while (true);
  164. }
Advertisement
Add Comment
Please, Sign In to add comment