diff --git a/.~lock.acceleration-aprx-simulation.ods# b/.~lock.acceleration-aprx-simulation.ods# new file mode 100644 index 0000000..a697bcf --- /dev/null +++ b/.~lock.acceleration-aprx-simulation.ods# @@ -0,0 +1 @@ +Adrian Bowyer,ensab,Charles,21.05.2013 15:01,file:///home/ensab/.config/libreoffice/3; \ No newline at end of file diff --git a/GCodes.ino b/GCodes.ino index 9840e74..8482a60 100644 --- a/GCodes.ino +++ b/GCodes.ino @@ -88,7 +88,7 @@ void GCodes::SetUpMove(GCodeBuffer *gb) // Deal with feedrate if(gb->Seen(gCodeLetters[DRIVES])) - moveBuffer[DRIVES] = gb->GetFValue()*distanceScale; + moveBuffer[DRIVES] = gb->GetFValue()*distanceScale/60.0; // Feedrates are in mm/minute; we need mm/sec // Remember for next time if we are switched // to absolute drive moves diff --git a/LeibRamp.pdf b/LeibRamp.pdf new file mode 100644 index 0000000..03a1500 Binary files /dev/null and b/LeibRamp.pdf differ diff --git a/Move.h b/Move.h index 704485d..1df8aee 100644 --- a/Move.h +++ b/Move.h @@ -24,23 +24,27 @@ Licence: GPL #define BUFFER_LENGTH 10 class DDA -{ +{ public: DDA(Move* m, Platform* p); boolean Init(float currentPosition[], float targetPosition[]); - void Start(); - void Step(); + void Start(boolean noTest); + void Step(boolean noTest); boolean Active(); - private: + private: Move* move; Platform* platform; long counter[DRIVES+1]; long delta[DRIVES+1]; - boolean directions[DRIVES+1]; + char directions[DRIVES+1]; long totalSteps; - long stepCountDown; - boolean active; + long stepCount; + float timeStep; + float velocity; + long stopAStep; + long startDStep; + volatile boolean active; }; class Move @@ -55,7 +59,7 @@ class Move void GetCurrentState(float m[]); void Interrupt(); - + friend class DDA; private: @@ -67,6 +71,7 @@ class Move float currentFeedrate; float currentPosition[AXES]; // Note - drives above AXES are always relative moves float nextMove[DRIVES + 1]; // Extra is for feedrate + float stepDistances[8]; // Index bits: lsb -> dx, dy, dz <- msb }; inline boolean DDA::Active() @@ -76,7 +81,7 @@ inline boolean DDA::Active() inline void Move::Interrupt() { - dda->Step(); + dda->Step(true); } diff --git a/Move.ino b/Move.ino index a3732d2..a459f18 100644 --- a/Move.ino +++ b/Move.ino @@ -30,13 +30,26 @@ Move::Move(Platform* p, GCodes* g) void Move::Init() { - lastTime = platform->Time(); - for(char i = 0; i < DRIVES; i++) + char i; + + for(i = 0; i < DRIVES; i++) platform->SetDirection(i, FORWARDS); - for(char i = 0; i <= AXES; i++) + for(i = 0; i <= AXES; i++) currentPosition[i] = 0.0; lastTime = platform->Time(); currentFeedrate = START_FEED_RATE; + float dx = 1.0/platform->DriveStepsPerUnit(X_AXIS); + float dy = 1.0/platform->DriveStepsPerUnit(Y_AXIS); + float dz = 1.0/platform->DriveStepsPerUnit(Z_AXIS); + stepDistances[0] = dx; // Should never be used. Wrong, but safer than 0.0... + stepDistances[1] = dx; + stepDistances[2] = dy; + stepDistances[3] = sqrt(dx*dx + dy*dy); + stepDistances[4] = dz; + stepDistances[5] = sqrt(dx*dx + dz*dz); + stepDistances[6] = sqrt(dy*dy + dz*dz); + stepDistances[7] = sqrt(dx*dx + dy*dy + dz*dz); + active = true; } @@ -73,7 +86,7 @@ void Move::Qmove() currentPosition[i] = nextMove[i]; if(work) - dda->Start(); + dda->Start(true); } void Move::GetCurrentState(float m[]) @@ -102,14 +115,29 @@ boolean DDA::Init(float currentPosition[], float targetPosition[]) char drive; active = false; totalSteps = -1; + float dist = 0; // X+Y+Z + float d; + char axesMoving = 0; for(drive = 0; drive < DRIVES; drive++) { if(drive < AXES) - delta[drive] = (long)((targetPosition[drive] - currentPosition[drive])*platform->DriveStepsPerUnit(drive)); //Absolute - else + { + d = targetPosition[drive] - currentPosition[drive]; + delta[drive] = (long)(d*platform->DriveStepsPerUnit(drive)); //Absolute + dist += d*d; + } else delta[drive] = (long)(targetPosition[drive]*platform->DriveStepsPerUnit(drive)); // Relative - directions[drive] = (delta[drive] >= 0); + if(delta[drive] >= 0) + directions[drive] = FORWARDS; + else + directions[drive] = BACKWARDS; delta[drive] = abs(delta[drive]); + if(drive == X_AXIS && delta[drive] > 0) + axesMoving |= 1; + if(drive == Y_AXIS && delta[drive] > 0) + axesMoving |= 2; + if(drive == Z_AXIS && delta[drive] > 0) + axesMoving |= 4; if(delta[drive] > totalSteps) totalSteps = delta[drive]; } @@ -118,35 +146,112 @@ boolean DDA::Init(float currentPosition[], float targetPosition[]) counter[0] = totalSteps/2; for(drive = 1; drive < DRIVES; drive++) counter[drive] = counter[0]; - stepCountDown = totalSteps; + dist = sqrt(dist); + float acc; + if(axesMoving|4) + { + acc = platform->Acceleration(Z_AXIS); + velocity = platform->Jerk(Z_AXIS); + } else + { + acc = platform->Acceleration(X_AXIS); + velocity = platform->Jerk(X_AXIS); + } + + timeStep = move->stepDistances[1]/velocity; + d = 0.5*(targetPosition[DRIVES]*targetPosition[DRIVES] - velocity*velocity)/acc; // d = (v^2 - u^2)/2a + stopAStep = (long)((d*totalSteps)/dist); + startDStep = totalSteps - stopAStep; + if(stopAStep > startDStep) + { + stopAStep = totalSteps/2; + startDStep = stopAStep + 1; + } + stepCount = 0; return true; } -void DDA::Start() +void DDA::Start(boolean noTest) { for(char drive = 0; drive < DRIVES; drive++) platform->SetDirection(drive, directions[drive]); - platform->SetInterrupt(300); + if(noTest) + platform->SetInterrupt((long)(1.0e6*timeStep)); active = true; } -void DDA::Step() +void DDA::Step(boolean noTest) { - if(!active) + if(!active && noTest) return; + + char axesMoving = 0; + + counter[X_AXIS] += delta[X_AXIS]; + if(counter[X_AXIS] > 0) + { + if(noTest) + platform->Step(X_AXIS); + axesMoving |= 1; + counter[X_AXIS] -= totalSteps; + } + + counter[Y_AXIS] += delta[Y_AXIS]; + if(counter[Y_AXIS] > 0) + { + if(noTest) + platform->Step(Y_AXIS); + axesMoving |= 2; + counter[Y_AXIS] -= totalSteps; + } + + counter[Z_AXIS] += delta[Z_AXIS]; + if(counter[Z_AXIS] > 0) + { + if(noTest) + platform->Step(Z_AXIS); + axesMoving |= 4; + counter[Z_AXIS] -= totalSteps; + } + - for(char drive = 0; drive < DRIVES; drive++) + for(char drive = AXES; drive < DRIVES; drive++) { counter[drive] += delta[drive]; if(counter[drive] > 0) { - platform->Step(drive); + if(noTest) + platform->Step(drive); counter[drive] -= totalSteps; } } - // Deal with feedrates here + if(axesMoving) + { + if(stepCount < stopAStep) + { + timeStep = move->stepDistances[axesMoving]/velocity; + if(axesMoving & 4) + velocity += platform->Acceleration(Z_AXIS)*timeStep; + else + velocity += platform->Acceleration(X_AXIS)*timeStep; + if(noTest) + platform->SetInterrupt((long)(1.0e6*timeStep)); + } + if(stepCount >= startDStep) + { + timeStep = move->stepDistances[axesMoving]/velocity; + if(axesMoving & 4) + velocity -= platform->Acceleration(Z_AXIS)*timeStep; + else + velocity -= platform->Acceleration(X_AXIS)*timeStep; + if(noTest) + platform->SetInterrupt((long)(1.0e6*timeStep)); + } + } - stepCountDown--; - active = stepCountDown > 0; + stepCount++; + active = stepCount < totalSteps; + if(!active && noTest) + platform->SetInterrupt(-1); } diff --git a/Platform.h b/Platform.h index 0a94dfc..709f5ae 100644 --- a/Platform.h +++ b/Platform.h @@ -69,10 +69,11 @@ Licence: GPL #define HIGH_STOP_PINS {-1, -1, -1, -1} #define ENDSTOP_HIT 1 // when a stop == this it is hit #define MAX_FEEDRATES {300, 300, 3, 45} // mm/sec -#define MAX_ACCELERATIONS {800, 800, 30, 250} // mm/sec^2?? Maximum start speed for accelerated moves. +#define ACCELERATIONS {800, 800, 30, 250} // mm/sec^2?? +//#define ACCELERATIONS {80, 80, 3, 25} #define DRIVE_STEPS_PER_UNIT {91.4286, 91.4286, 4000, 929} #define JERKS {15.0, 15.0, 0.4, 15.0} // (mm/sec) - +//#define JERKS {5.0, 5.0, 0.1, 5.0} // (mm/sec) // AXES @@ -219,6 +220,8 @@ class Platform void Disable(byte drive); // There is no drive enable; drives get enabled automatically the first time they are used. void Home(byte axis); float DriveStepsPerUnit(char drive); + float Acceleration(char drive); + float Jerk(char drive); float ZProbe(); // Return the height above the bed. Returned value is negative if probing isn't implemented void ZProbe(float h); // Move to height h above the bed using the probe (if there is one). h should be non-negative. @@ -255,7 +258,7 @@ class Platform char lowStopPins[DRIVES]; char highStopPins[DRIVES]; float maxFeedrates[DRIVES]; - float maxAccelerations[DRIVES]; + float accelerations[DRIVES]; float driveStepsPerUnit[DRIVES]; float jerks[DRIVES]; @@ -304,11 +307,6 @@ inline unsigned long Platform::Time() return micros(); } -inline float Platform::DriveStepsPerUnit(char drive) -{ - return driveStepsPerUnit[drive]; -} - inline void Platform::Exit() { active = false; @@ -346,6 +344,21 @@ inline char* Platform::GetTempDir() // Drive the RepRap machine +inline float Platform::DriveStepsPerUnit(char drive) +{ + return driveStepsPerUnit[drive]; +} + +inline float Platform::Acceleration(char drive) +{ + return accelerations[drive]; +} + +inline float Platform::Jerk(char drive) +{ + return jerks[drive]; +} + inline void Platform::SetDirection(byte drive, bool direction) { digitalWrite(directionPins[drive], direction); diff --git a/Platform.ino b/Platform.ino index 851d5df..baf12a3 100644 --- a/Platform.ino +++ b/Platform.ino @@ -26,7 +26,8 @@ Licence: GPL void setup() { - reprap.Init(); + reprap.Init(); + //reprap.InterruptTime(); // Uncomment this line to time the interrupt routine on startup } void loop() @@ -99,7 +100,7 @@ void Platform::Init() lowStopPins = LOW_STOP_PINS; highStopPins = HIGH_STOP_PINS; maxFeedrates = MAX_FEEDRATES; - maxAccelerations = MAX_ACCELERATIONS; + accelerations = ACCELERATIONS; driveStepsPerUnit = DRIVE_STEPS_PER_UNIT; jerks = JERKS; diff --git a/RepRapFirmware.h b/RepRapFirmware.h index 2e971d2..eec8fb7 100644 --- a/RepRapFirmware.h +++ b/RepRapFirmware.h @@ -41,6 +41,9 @@ boolean StringStartsWith(char* string, char* starting); boolean StringEquals(char* s1, char* s2); int StringContains(char* string, char* match); +//#include +//#include + #include "Configuration.h" #include "Platform.h" #include "Webserver.h" diff --git a/RepRapFirmware.ino b/RepRapFirmware.ino index 99d60cd..fbecff1 100644 --- a/RepRapFirmware.ino +++ b/RepRapFirmware.ino @@ -100,6 +100,24 @@ void RepRap::Spin() heat->Spin(); } +void RepRap::InterruptTime() +{ + char buffer[50]; + DDA* dda = new DDA(move, platform); + float a[] = {1.0, 2.0, 3.0, 4.0, 5.0}; + float b[] = {2.0, 3.0, 4.0, 5.0, 6.0}; + dda->Init(a, b); + dda->Start(false); + unsigned long t = platform->Time(); + for(long i = 0; i < 100000; i++) + dda->Step(false); + t = platform->Time() - t; + platform->Message(HOST_MESSAGE, "Time for 100000 calls of the interrupt function: "); + sprintf(buffer, "%ld", t); + platform->Message(HOST_MESSAGE, buffer); + platform->Message(HOST_MESSAGE, " microseconds.\n"); +} + //************************************************************************************************* diff --git a/Reprap.h b/Reprap.h index 1a5b4bc..73a8e6d 100644 --- a/Reprap.h +++ b/Reprap.h @@ -30,6 +30,7 @@ class RepRap void Spin(); void Exit(); void Interrupt(); + void InterruptTime(); boolean debug(); void debug(boolean d); Platform* GetPlatform(); diff --git a/acceleration-aprx-simulation.ods b/acceleration-aprx-simulation.ods new file mode 100644 index 0000000..666cdba Binary files /dev/null and b/acceleration-aprx-simulation.ods differ