/**************************************************************************************************** RepRapFirmware - Move This is all the code to deal with movement and kinematics. ----------------------------------------------------------------------------------------------------- Version 0.1 18 November 2012 Adrian Bowyer RepRap Professional Ltd http://reprappro.com Licence: GPL ****************************************************************************************************/ #include "RepRapFirmware.h" Move::Move(Platform* p, GCodes* g) { char i; active = false; platform = p; gCodes = g; // Build the DDA ring ddaRingAddPointer = new DDA(this, platform, NULL); dda = ddaRingAddPointer; for(i = 1; i < RING_LENGTH; i++) dda = new DDA(this, platform, dda); ddaRingAddPointer->next = dda; dda = NULL; lookAheadRingAddPointer = new LookAhead(this, platform, NULL); lookAheadRingGetPointer = lookAheadRingAddPointer; for(i = 1; i < RING_LENGTH; i++) lookAheadRingGetPointer = new LookAhead(this, platform, lookAheadRingGetPointer); lookAheadRingAddPointer->next = lookAheadRingGetPointer; // Set the backwards pointers lookAheadRingGetPointer = lookAheadRingAddPointer; for(i = 0; i <= RING_LENGTH; i++) { lookAheadRingAddPointer = lookAheadRingAddPointer->Next(); lookAheadRingAddPointer->previous = lookAheadRingGetPointer; lookAheadRingGetPointer = lookAheadRingAddPointer; } lookAheadDDA = new DDA(this, platform, NULL); } void Move::Init() { unsigned char i, j; for(i = 0; i < DRIVES; i++) platform->SetDirection(i, FORWARDS); for(i = 0; i <= AXES; i++) currentPosition[i] = 0.0; currentFeedrate = START_FEED_RATE; // Empty the rings ddaRingGetPointer = ddaRingAddPointer; ddaRingLocked = false; lookAheadRingGetPointer = lookAheadRingAddPointer; lookAheadRingCount = 0; addNoMoreMoves = false; larWaiting = NULL; // Put the origin on the lookahead ring so it corresponds with currentPosition for(i = 0; i < DRIVES; i++) nextMove[i] = 0.0; nextMove[DRIVES] = currentFeedrate; LookAheadRingAdd(nextMove, 0.0, 0.0); // Now remove it from the ring; it will remain as what is now the // previous move, so the first real move will see that as // the place to move from. LookAheadRingGet(); // The stepDistances arrays are look-up tables of the Euclidean distance // between the start and end of a step. If the step is just along one axis, // it's just that axis's step length. If it's more, it is a Pythagoran // sum of all the axis steps that take part. float d, e; for(i = 0; i < (1<DriveStepsPerUnit(j); d += e*e; } } stepDistances[i] = sqrt(d); } for(i = 0; i < (1<<(DRIVES-AXES)); i++) { d = 0.0; for(j = 0; j < (DRIVES-AXES); j++) { if(i & (1<DriveStepsPerUnit(AXES + j); d += e*e; } } extruderStepDistances[i] = sqrt(d); } // We don't want 0. If no axes/extruders are moving these should never be used. // But try to be safe. stepDistances[0] = 1.0/platform->DriveStepsPerUnit(AXES); extruderStepDistances[0] = stepDistances[0]; currentFeedrate = START_FEED_RATE; moveWaiting = false; lastTime = platform->Time(); active = true; } void Move::Exit() { active = false; } void Move::Spin() { if(!active) return; //FIXME float u = 0.0; // This will provoke the code to select the jerk values. float v = 0.0; if(larWaiting != NULL) { u = larWaiting->U(); v = larWaiting->V(); if(DDARingAdd(larWaiting->Previous()->Movement(), larWaiting->Movement(), u, v)) larWaiting = NULL; } else { larWaiting = LookAheadRingGet(); } if(moveWaiting) { if(!addNoMoreMoves) { if(LookAheadRingAdd(nextMove, u, v)) { for(char i = 0; i < AXES; i++) currentPosition[i] = nextMove[i]; currentFeedrate = nextMove[DRIVES]; moveWaiting = false; } } } else { moveWaiting = gCodes->ReadMove(nextMove); } } boolean Move::GetCurrentState(float m[]) { if(DDARingFull()) return false; for(char i = 0; i < DRIVES; i++) { if(i < AXES) m[i] = currentPosition[i]; else m[i] = 0.0; } m[DRIVES] = currentFeedrate; return true; } boolean Move::DDARingAdd(float currentPosition[], float targetPosition[], float& u, float& v) { if(GetDDARingLock()) { if(DDARingFull()) { ReleaseDDARingLock(); return false; } if(ddaRingAddPointer->Active()) { platform->Message(HOST_MESSAGE, "Attempt to alter an active ring buffer entry!\n"); ReleaseDDARingLock(); return false; } if(ddaRingAddPointer->Init(currentPosition, targetPosition, u, v) == nothing) { // Throw it away ReleaseDDARingLock(); return true; } ddaRingAddPointer = ddaRingAddPointer->Next(); ReleaseDDARingLock(); return true; } return false; } DDA* Move::DDARingGet() { DDA* result = NULL; if(GetDDARingLock()) { if(DDARingEmpty()) { ReleaseDDARingLock(); return NULL; } result = ddaRingGetPointer; ddaRingGetPointer = ddaRingGetPointer->Next(); ReleaseDDARingLock(); return result; } return NULL; } void Move::Interrupt() { // Have we got a live DDA? if(dda == NULL) { // No - see if a new one is available. dda = DDARingGet(); if(dda != NULL) dda->Start(true); // Yes - got it. So fire it up. return; } // We have a DDA. Has it finished? if(dda->Active()) { // No - it's still live. Step it and return. dda->Step(true); return; } // Yes - it's finished. Throw it away so the code above will then find a new one. // (N.B. This is not a memory leak. The DDAs are stored in the ring buffer, initialised // on boot, and never renewed or overwritten. dda is just a copied pointer.) dda = NULL; } boolean Move::LookAheadRingAdd(float m[], float uu, float vv) { if(LookAheadRingFull()) return false; lookAheadRingAddPointer->Init(m, uu, vv); lookAheadRingAddPointer = lookAheadRingAddPointer->Next(); lookAheadRingCount++; return true; } LookAhead* Move::LookAheadRingGet() { LookAhead* result; if(LookAheadRingEmpty()) return NULL; result = lookAheadRingGetPointer; if(!result->Processed()) return NULL; lookAheadRingGetPointer = lookAheadRingGetPointer->Next(); lookAheadRingCount--; return result; } // This function is never normally called. It is a test to time // the interrupt function. To activate it, uncomment the line that calls // this in Platform.ino. void Move::InterruptTime() { char buffer[50]; float a[] = {1.0, 2.0, 3.0, 4.0, 5.0}; float b[] = {2.0, 3.0, 4.0, 5.0, 6.0}; float u = 50; float v = 50; lookAheadDDA->Init(a, b, u, v); lookAheadDDA->Start(false); unsigned long t = platform->Time(); for(long i = 0; i < 100000; i++) lookAheadDDA->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"); } //**************************************************************************************************** DDA::DDA(Move* m, Platform* p, DDA* n) { active = false; move = m; platform = p; next = n; } /* DDA::Init(...) Sets up the DDA to take us between two positions and extrude states. The start velocity is u, and the end one is v. The requested maximum feedrate is in targetPosition[DRIVES]. Almost everything that needs to be done to set this up is also useful for GCode look-ahead, so this one function is used for both. It flags when u and v cannot be satisfied with the distance available and reduces them proportionately to give values that can just be achieved, which is why they are passed by reference. The return value is true for an actual move, false for a zero-length (i.e. null) move. Every drive has an acceleration associated with it, so when more than one drive is moving there have to be rules of precedence that say which acceleration (and which jerk value) to use. The rules are these: if Z is moving Use Z acceleration else if X and/or Y are moving Use X acceleration else Use the acceleration for the extruder that's moving. In the case of multiple extruders moving at once, their minimum acceleration (and its associated jerk) are used. The variables axesMoving and extrudersMoving track what's going on. The bits in the char axesMoving are ORed: msb -> 00000ZYX <- lsb a 1 meaning that that axis is moving. The bits of extrudersMoving contain a similar pattern for the moving extruders. Note that all this assumes that X and Y accelerations are equal, though in fact there is a value stored for each. In the case of only extruders moving, the distance moved is taken to be the Pythagoran distance in the configuration space of the extruders. TODO: Worry about having more than eight extruders... */ MovementProfile DDA::Init(float currentPosition[], float targetPosition[], float& u, float& v) { char drive; active = false; MovementProfile result = nothing; totalSteps = -1; distance = 0.0; // X+Y+Z float eDistance = 0.0; float d; unsigned char axesMoving = 0; unsigned char extrudersMoving = 0; // How far are we going, both in steps and in mm? for(drive = 0; drive < DRIVES; drive++) { if(drive < AXES) { d = targetPosition[drive] - currentPosition[drive]; //Absolute distance += d*d; delta[drive] = (long)(d*platform->DriveStepsPerUnit(drive)); if(delta[drive]) axesMoving |= 1<DriveStepsPerUnit(drive)); // Relative eDistance += targetPosition[drive]*targetPosition[drive]; if(delta[drive]) extrudersMoving |= 1<<(drive - AXES); } if(delta[drive] >= 0) directions[drive] = FORWARDS; else directions[drive] = BACKWARDS; delta[drive] = abs(delta[drive]); // Keep track of the biggest drive move in totalSteps if(delta[drive] > totalSteps) totalSteps = delta[drive]; } // Not going anywhere? if(totalSteps <= 0) return result; // Set up the DDA result = moving; counter[0] = totalSteps/2; for(drive = 1; drive < DRIVES; drive++) counter[drive] = counter[0]; // Acceleration and velocity calculations distance = sqrt(distance); if(axesMoving & (1<Acceleration(Z_AXIS); jerk = platform->Jerk(Z_AXIS); } else if(axesMoving) // X or Y involved? { acceleration = platform->Acceleration(X_AXIS); jerk = platform->Jerk(X_AXIS); } else // Must be extruders only { acceleration = FLT_MAX; // Slight hack distance = sqrt(eDistance); for(drive = AXES; drive < DRIVES; drive++) { if(extrudersMoving & (1<<(drive - AXES))) { if(platform->Acceleration(drive) < acceleration) { acceleration = platform->Acceleration(drive); jerk = platform->Jerk(drive); } } } } // If velocities requested are (almost) zero, set them to the jerk if(v < 0.01) // Set change here? v = jerk; if(u < 0.01) u = jerk; // At which DDA step should we stop accelerating? targetPosition[DRIVES] contains // the desired feedrate. d = 0.5*(targetPosition[DRIVES]*targetPosition[DRIVES] - u*u)/acceleration; // d = (v1^2 - v0^2)/2a stopAStep = (long)((d*totalSteps)/distance); // At which DDA step should we start decelerating? d = 0.5*(v*v - targetPosition[DRIVES]*targetPosition[DRIVES])/acceleration; // This should be 0 or negative... startDStep = totalSteps + (long)((d*totalSteps)/distance); // If acceleration stop is at or after deceleration start, then the distance moved // is not enough to get to full speed. if(stopAStep >= startDStep) { result = noFlat; // Work out the point at which to stop accelerating and then // immediately start decelerating. dCross = 0.5*(0.5*(v*v - u*u)/acceleration + distance); if(dCross < 0.0 || dCross > distance) { // With the acceleration available, it is not possible // to satisfy u and v within the distance; reduce u and v // proportionately to get ones that work and flag the fact. // The result is two velocities that can just be accelerated // or decelerated between over the distance to get // from one to the other. result = change; float k = v/u; u = 2.0*acceleration*distance/(k*k - 1); if(u >= 0.0) { u = sqrt(u); v = k*u; } else { v = sqrt(-u); u = v/k; } dCross = 0.5*(0.5*(v*v - u*u)/acceleration + distance); } // The DDA steps at which acceleration stops and deceleration starts stopAStep = (long)((dCross*totalSteps)/distance); startDStep = stopAStep + 1; } // The initial velocity velocity = u; // Sanity check if(velocity <= 0.0) { velocity = 1.0; platform->Message(HOST_MESSAGE, "DDA.Init(): Zero or negative initial velocity!"); } // How far have we gone? stepCount = 0; // Guess that the first DDA move will be in roughly the direction // recorded in axesMoving. This is a simple heuristic, and any // small error will be forgotten with the very next step. timeStep = move->stepDistances[axesMoving]/velocity; return result; } void DDA::Start(boolean noTest) { for(char drive = 0; drive < DRIVES; drive++) platform->SetDirection(drive, directions[drive]); if(noTest) platform->SetInterrupt((long)(1.0e6*timeStep)); active = true; } void DDA::Step(boolean noTest) { if(!active && noTest) return; unsigned char axesMoving = 0; unsigned char extrudersMoving = 0; for(char drive = 0; drive < DRIVES; drive++) { counter[drive] += delta[drive]; if(counter[drive] > 0) { if(noTest) platform->Step(drive); counter[drive] -= totalSteps; if(drive < AXES) axesMoving |= 1<stepDistances[axesMoving]/velocity; else timeStep = move->extruderStepDistances[extrudersMoving]/velocity; velocity += acceleration*timeStep; if(noTest) platform->SetInterrupt((long)(1.0e6*timeStep)); } if(stepCount >= startDStep) { if(axesMoving) timeStep = move->stepDistances[axesMoving]/velocity; else timeStep = move->extruderStepDistances[extrudersMoving]/velocity; velocity -= acceleration*timeStep; if(noTest) platform->SetInterrupt((long)(1.0e6*timeStep)); } stepCount++; active = stepCount < totalSteps; if(!active && noTest) platform->SetInterrupt(STANDBY_INTERRUPT_RATE); } //*************************************************************************************************** LookAhead::LookAhead(Move* m, Platform* p, LookAhead* n) { move = m; platform = p; next = n; } void LookAhead::Init(float m[], float uu, float vv) { u = uu; v = vv; processed = true; // Fixme for(char i = 0; i <= DRIVES; i++) movement[i] = m[i]; } // This returns the cosine of the angle between // the movement starting at start, and the movement // starting at the end of that. Note that it // includes Z movements, though Z values will almost always // not change. float LookAhead::Cosine(LookAhead* start) { LookAhead* n1 = start->Next(); LookAhead* n2 = n1->Next(); float sum = 0.0; float a2 = 0.0; float b2 = 0.0; float m1; float m2; for(char i = 0; i < AXES; i++) { m1 = n1->movement[i] - start->movement[i]; m2 = n2->movement[i] - n1->movement[i]; a2 += m1*m1; b2 += m2*m2; sum += m1*m2; } sum = sum/( (float)sqrt(a2) * (float)sqrt(b2) ); return sum; }