1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798 |
- #include "Arduino.h"
- #include "TeensyStep.h"
- constexpr unsigned rpm = 15;
- constexpr unsigned spindleSPR = 6400;
- constexpr unsigned spindleSpeed = rpm * spindleSPR / 60;
- constexpr unsigned spindleStepPin = 1;
- constexpr unsigned spindleDirPin = 2;
- constexpr unsigned slideAmplitude = 10000;
- constexpr unsigned slideSpeed = 35000;
- constexpr unsigned slideStepPin = 3;
- constexpr unsigned slideDirPin = 4;
- IntervalTimer tickTimer;
- constexpr unsigned recalcPeriod = 25'000;
- constexpr float dt = recalcPeriod / 1E6;
- constexpr int n = 5;
- constexpr int d = 8;
- constexpr float k = (float)n / d;
- float slideFunc(float spindleAngle)
- {
- float phi = fmodf(spindleAngle * k, TWO_PI);
- return slideAmplitude * cosf(phi);
- }
- RotateControl slideController;
- RotateControl spindleController;
- Stepper spindle(spindleStepPin, spindleDirPin);
- Stepper slide(slideStepPin, slideDirPin);
- void tick()
- {
- float spindleAngle = spindle.getPosition() * (TWO_PI / spindleSPR);
- float slidePosition = slide.getPosition();
- float slideTarget = slideFunc(spindleAngle);
- float newSpeed = (slideTarget - slidePosition) / dt;
- float speedFac = newSpeed / slideSpeed;
- slideController.overrideSpeed(speedFac);
- }
- void setup()
- {
- pinMode(LED_BUILTIN, OUTPUT);
-
- while (!Serial && millis() < 1000);
- spindle
- .setMaxSpeed(spindleSpeed)
- .setAcceleration(50000);
- slide
- .setMaxSpeed(slideSpeed)
- .setAcceleration(150000)
- .setPosition(slideFunc(0));
- spindleController.rotateAsync(spindle);
- slideController.rotateAsync(slide);
- slideController.overrideSpeed(0);
- tick();
-
- tickTimer.priority(255);
- tickTimer.begin(tick, recalcPeriod);
- }
- void loop()
- {
- digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN));
-
- float phi = spindle.getPosition() * (TWO_PI / spindleSPR);
- int32_t r = slide.getPosition();
- Serial.printf("%f\t%d\n", phi, r);
- delay(20);
- }
|