testing offset after homing

lib/general/point.cpp

@@ -10,12 +10,12 @@
 void point::toString() const {
 }
 
-void point::stepForward(uint32_t anAxis) {
- ++inSteps[anAxis];
- inMm[anAxis] = inSteps[anAxis] / 1.0f;
-}
+// void point::stepForward(uint32_t anAxis) {
+//  ++inSteps[anAxis];
+//  inMm[anAxis] = inSteps[anAxis] / 1.0f;
+// }
 
-void point::stepBackward(uint32_t anAxis) {
- --inSteps[anAxis];
- inMm[anAxis] = inSteps[anAxis] / 1.0f;
-}
+// void point::stepBackward(uint32_t anAxis) {
+//  --inSteps[anAxis];
+//  inMm[anAxis] = inSteps[anAxis] / 1.0f;
+// }

lib/general/point.h

@@ -16,7 +16,5 @@ class point {
  int32_t inSteps[nmbrAxis]{0}; // caution, SIGNED int, as this could go negative..
 
  void toString() const;
- void stepForward(uint32_t anAxis);
- void stepBackward(uint32_t anAxis);
  private:
 };

lib/inputs/debouncedinput.cpp

@@ -26,6 +26,14 @@ bool debouncedInput::getState() const {
  return currentState;
 }
 
+bool debouncedInput::isClosed() const {
+ return (false == currentState);
+}
+
+bool debouncedInput::isOpen() const {
+ return (true == currentState);
+}
+
 event debouncedInput::getEvent(bool undebouncedInput) {
  if (undebouncedInput) {
  if (debounceCounter < debounceMaxCount) {

lib/inputs/debouncedinput.h

@@ -16,12 +16,14 @@ class debouncedInput {
  void initialize(bool undebouncedInput); // initialize to prevent transient events on startup
  event getEvent(bool undebouncedInput); // get press or release event
  bool getState() const; // get current state of the input
+ bool isClosed() const; //
+ bool isOpen() const; //
  static constexpr uint32_t debounceMaxCount{4U}; // sets the upper boundary for debounceCounter
  const event onOpen; // event to generate when this input opens
  const event onClose; // event to generate when this input closes
 
  private:
  uint32_t debounceCounter{0U}; // counts up (input high) or down (input low) until it hits boundaries 0 or maxCount
- bool currentState{false}; // after debouncing : true : button is pressed, false : button is not pressed
+ bool currentState{false}; // after debouncing : true : switch is closed / button pressed, false : switch open / button released
  bool previousState{false}; // remembers previous state, to detect flanks
 };

lib/maincontroller/homing.cpp

@@ -12,15 +12,17 @@ extern debouncedInput myInputs[nmbrInputs];
 extern eventBuffer theEventBuffer;
 
 void homingController::start() {
- homingAxisIndex = 0;
+ homingSequenceIndex = 0;
  if (selectAxis()) {
  theEventBuffer.pushEvent(event::homingStarted);
  if (theMotionController.isRunning()) {
  goTo(homingState::stopping);
  } else {
- if (!myInputs[theLimitSwitch].getState()) {
+ if (myInputs[theLimitSwitch].isClosed()) {
+ //Serial.printf("limitSwitch %s is closed\n", toString(currentHomingAxis));
  goTo(homingState::opening);
  } else {
+ //Serial.printf("limitSwitch %s is open\n", toString(currentHomingAxis));
  goTo(homingState::closing);
  }
  }
@@ -30,13 +32,13 @@ void homingController::start() {
 }
 
 bool homingController::nextAxis() {
- homingAxisIndex++;
+ homingSequenceIndex++;
  return selectAxis();
 }
 
 bool homingController::selectAxis() {
- while (homingAxisIndex < nmbrAxis) {
- currentHomingAxis = theMachineProperties.homingSequence[homingAxisIndex];
+ while (homingSequenceIndex < nmbrAxis) {
+ currentHomingAxis  = theMachineProperties.homingSequence[homingSequenceIndex];
  currentHomingAxisIndex = static_cast<uint32_t>(currentHomingAxis);
  if (static_cast<uint32_t>(currentHomingAxis) < nmbrAxis) {
  if (theMachineProperties.homingDirection[currentHomingAxisIndex]) {
@@ -85,7 +87,7 @@ bool homingController::selectAxis() {
  }
  }
  }
- homingAxisIndex++;
+ homingSequenceIndex++;
  }
  return false;
 }
@@ -99,9 +101,11 @@ void homingController::handleEvents(event theEvent) {
  case homingState::stopping:
  switch (theEvent) {
  case event::motionStopped:
- if (myInputs[theLimitSwitch].getState()) {
+ if (myInputs[theLimitSwitch].isClosed()) {
+ //Serial.printf("limitSwitch %s is closed\n", toString(currentHomingAxis));
  goTo(homingState::opening);
  } else {
+ //Serial.printf("limitSwitch %s is open\n", toString(currentHomingAxis));
  goTo(homingState::closing);
  }
  break;
@@ -110,6 +114,7 @@ void homingController::handleEvents(event theEvent) {
  break;
  }
  break;
+
  case homingState::closing:
  if (limitSwitchClose == theEvent) {
  goTo(homingState::closedWaitForStop);
@@ -148,9 +153,11 @@ void homingController::handleEvents(event theEvent) {
  switch (theEvent) {
  case event::motionStopped:
  if (nextAxis()) {
- if (!myInputs[theLimitSwitch].getState()) {
+ if (myInputs[theLimitSwitch].isClosed()) {
+ //Serial.printf("limitSwitch %s is closed\n", toString(currentHomingAxis));
  goTo(homingState::opening);
  } else {
+ //Serial.printf("limitSwitch %s is open\n", toString(currentHomingAxis));
  goTo(homingState::closing);
  }
  } else {
@@ -173,13 +180,13 @@ void homingController::handleTimeouts() {
 
 void homingController::goTo(homingState theNewState) {
  exitState(theHomingState);
- //Serial.printf("homingState from %s to %s\n",toString(theHomingState), toString(theNewState));
+ //Serial.printf("homingState from %s to %s\n", toString(theHomingState), toString(theNewState));
  theHomingState = theNewState;
  enterState(theNewState);
 }
 
 void homingController::enterState(homingState theNewState) {
- // Serial.printf("[%d, %d, %d]\n", theMotionController.machinePositionInSteps[0], theMotionController.machinePositionInSteps[1], theMotionController.machinePositionInSteps[2]);
+ //Serial.printf("[%d, %d, %d]\n", theMotionController.machinePositionInSteps[0], theMotionController.machinePositionInSteps[1], theMotionController.machinePositionInSteps[2]);
  switch (theHomingState) {
  case homingState::lost:
  break;
@@ -190,13 +197,15 @@ void homingController::enterState(homingState theNewState) {
 
  case homingState::closing:
  theMotionController.flushMotionBuffer();
- theMotionController.resetMachinePosition();
  {
+ point currentPosition;
  simplifiedMotion aMotion;
- aMotion.setForHoming(currentHomingAxis, theMachineProperties.motors.sMax[currentHomingAxisIndex], theMachineProperties.vHoming);
+ theMotionController.getMachinePosition(currentPosition);
+ aMotion.set(currentPosition, currentHomingAxis, theMachineProperties.motors.sMax[currentHomingAxisIndex], theMachineProperties.vHoming);
  theMotionController.append(aMotion);
  }
  theMotionController.start();
+ //Serial.printf("going to [%f]\n", theMachineProperties.motors.sMax[currentHomingAxisIndex] * theMachineProperties.motors.stepsPerMm[currentHomingAxisIndex]);
  break;
 
  case homingState::closedWaitForStop:
@@ -205,13 +214,15 @@ void homingController::enterState(homingState theNewState) {
 
  case homingState::opening:
  theMotionController.flushMotionBuffer();
- theMotionController.resetMachinePosition();
  {
+ point currentPosition;
  simplifiedMotion aMotion;
- aMotion.setForHoming(currentHomingAxis, -100.0, theMachineProperties.vHomingSlow);
+ theMotionController.getMachinePosition(currentPosition);
+ aMotion.set(currentPosition, currentHomingAxis, -1 * theMachineProperties.motors.sMax[currentHomingAxisIndex], theMachineProperties.vHoming);
  theMotionController.append(aMotion);
  }
  theMotionController.start();
+ //Serial.printf("going to [%f]\n", -1 * theMachineProperties.motors.sMax[currentHomingAxisIndex] * theMachineProperties.motors.stepsPerMm[currentHomingAxisIndex]);
  break;
 
  case homingState::openedWaitForStop:
@@ -220,18 +231,19 @@ void homingController::enterState(homingState theNewState) {
 
  case homingState::offsettingWaitForStop:
  theMotionController.flushMotionBuffer();
- theMotionController.resetMachinePosition();
  {
+ point currentPosition;
  simplifiedMotion aMotion;
- aMotion.setForHoming(currentHomingAxis, theMachineProperties.homingOffset[currentHomingAxisIndex], theMachineProperties.vHoming);
+ theMotionController.getMachinePosition(currentPosition);
+ aMotion.set(currentPosition, currentHomingAxis, theMachineProperties.homingOffset[currentHomingAxisIndex], theMachineProperties.vHoming);
  theMotionController.append(aMotion);
  }
  theMotionController.start();
- //Serial.printf("[%d, %d, %d]\n", theMotionController.machinePositionInSteps[0], theMotionController.machinePositionInSteps[1], theMotionController.machinePositionInSteps[2]);
  //Serial.printf("going to [%f]\n", theMachineProperties.homingOffset[currentHomingAxisIndex] * theMachineProperties.motors.stepsPerMm[currentHomingAxisIndex]);
  break;
 
  case homingState::found:
+ theMotionController.resetMachinePosition();
  // TODO put all actions here when homing is complete
  break;
 
@@ -243,7 +255,6 @@ void homingController::enterState(homingState theNewState) {
 void homingController::exitState(homingState theOldState) {
  switch (theOldState) {
  case homingState::offsettingWaitForStop:
- //Serial.printf("[%d, %d, %d]\n", theMotionController.machinePositionInSteps[0], theMotionController.machinePositionInSteps[1], theMotionController.machinePositionInSteps[2]);
  break;
 
  default:

lib/maincontroller/homing.h

@@ -29,7 +29,7 @@ class homingController {
  bool selectAxis(); // true if homeable axis is selected, false if no homeable axis found
 
  homingState theHomingState{homingState::lost}; // current state of the homing process for selected axis
- uint32_t homingAxisIndex{0}; // index into machineProperties.homingSequence[] : 0 for 1st axis homing (eg. Z), 1 for second axis homing...
+ uint32_t homingSequenceIndex{0}; // index into machineProperties.homingSequence[] : 0 for 1st axis homing (eg. Z), 1 for second axis homing...
  axis currentHomingAxis{axis::nmbrAxis}; // value of machineProperties.homingSequence[homingAxisIndex]
  uint32_t currentHomingAxisIndex{nmbrAxis}; // same as currentHomingAxis but as integer value (io enum) so we can use it to index arrays
  uint32_t theLimitSwitch{0}; // index into myInputs[] telling which input is the matching limit switch

lib/motion/motionctrl.cpp

@@ -253,4 +253,11 @@ void motionCtrl::resetMachinePosition() {
  for (uint32_t anAxis = 0; anAxis < nmbrAxis; ++anAxis) {
  machinePositionInSteps[anAxis] = 0;
  }
+}
+
+void motionCtrl::getMachinePosition(point& aPosition) {
+ for (uint32_t anAxis = 0; anAxis < nmbrAxis; ++anAxis) {
+ aPosition.inSteps[anAxis] = machinePositionInSteps[anAxis];
+ aPosition.inMm[anAxis] = machinePositionInSteps[anAxis] * theMachineProperties.motors.stepsPerMm[anAxis];
+ }
 }

lib/motion/motionctrl.h

@@ -32,10 +32,12 @@ class motionCtrl {
  motionState getState() const;
  bool isRunning() const;
  motionStrategy theStrategy() const;
- 
+
  step calcNextStepperMotorSignals();
 
- int32_t machinePositionInSteps[nmbrAxis]{0}; // TODO : this needs to be private, but made it public to make homing easier fttb
+ void getMachinePosition(point &aPosition);
+
+ int32_t machinePositionInSteps[nmbrAxis]{0}; // TODO make this private again, currently public so we can print it..
 
 #ifndef unitTesting
  private:
@@ -46,6 +48,7 @@ class motionCtrl {
  motionBuffer theMotionBuffer; // buffer with motion segments to be executed
  stepSignals theStepSignals; // signals to be sent to the motors with their timing
 
+
  float vJunction(uint32_t left, uint32_t right) const; // max speed at the boundary of two motion segments
  bool needStepForward(uint32_t anAxis, float positionInMm); //
  bool needStepBackward(uint32_t anAxis, float positionInMm); //

lib/motion/simplifiedmotion.cpp

@@ -10,6 +10,7 @@
 #include <stdio.h> // needed for sprintf()
 #include <limits> // needed for infinity
 #include <math.h> // needed for sqrt and others
+#include "point.h"
 
 uint32_t simplifiedMotion::toString(char* output) {
  uint32_t outputLenght{0};
@@ -43,17 +44,17 @@ uint32_t simplifiedMotion::toString(char* output) {
  return outputLenght;
 }
 
-void simplifiedMotion::setForHoming(axis anAxis, double offset, double speed) {
- // only changing the fields which are different from the initialization values
- type = motionType::feedLinear;
- trajectory.length = fabs(offset);
- speedProfile.vFeed = speed;
- trajectory.delta[static_cast<uint32_t>(anAxis)] = offset;
-}
-
-
-
+void simplifiedMotion::set(point& aPosition, axis anAxis, double offset, double speed) {
+ uint32_t axisIndex = static_cast<uint32_t>(anAxis);
+ type = motionType::feedLinear;
+ trajectory.length = fabs(offset);
+ speedProfile.vFeed = speed;
+ trajectory.delta[axisIndex] = offset;
 
+ for (axisIndex = 0; axisIndex < nmbrAxis; ++axisIndex) {
+ trajectory.startPosition[axisIndex] = aPosition.inMm[axisIndex];
+ }
+}
 
 void simplifiedMotion::setForTest(uint32_t aSet) {
  static constexpr double pi{3.141592653589793238463}; // constant for calculations in radians
@@ -141,21 +142,3 @@ void simplifiedMotion::setForTest(uint32_t aSet) {
  break;
  }
 }
-
-// void simplifiedMotion::setForTest(motionType theType, uint32_t trajectoryIndex, uint32_t speedprofileIndex) {
-// type = theType;
-// switch (trajectoryIndex) {
-// case 0U:
-// break;
-
-// default:
-// break;
-// }
-// switch (speedprofileIndex) {
-// case 0U:
-// break;
-
-// default:
-// break;
-// }
-// }

lib/motion/simplifiedmotion.h

@@ -11,7 +11,7 @@
 #include "axis.h"
 #include "motiontype.h"
 #include "rotationdirection.h"
-#include "position.h"
+#include "point.h"
 
 class simplifiedMotion {
  public:
@@ -43,5 +43,9 @@ class simplifiedMotion {
  uint32_t toString(char* destBuffer);
  void setForTest(uint32_t aSet);
  void setForTest(motionType theType, uint32_t trajectoryIndex, uint32_t speedprofileIndex);
- void setForHoming(axis anAxis, double offset, double speed);
+ void set(point &aPosition, axis anAxis, double offset, double speed);
+
+ //void setRelative(point &aPosition, axis anAxis, double offset, double speed);
+ //void setAbsolute(point &aPosition, axis anAxis, double offset, double speed);
+
 };

lib_target/cnc3axis/general/machineproperties.h

@@ -68,7 +68,7 @@ class machineProperties {
 
  double minLengthSProfile{0.0F}; // [mm] all motions with a length smaller will be 2nd order T-profile - larger will be 3rd order S-profile
 
- float vHoming{15}; // faster homing speed, towards switch closing
+ float vHoming{5}; // faster homing speed, towards switch closing
  float vHomingSlow{1}; // slower homing, towards opening limitswitch
 
  axis homingSequence[nmbrAxis]{axis::Z, axis::X, axis::nmbrAxis}; // in which sequence do we want to home axis.

platformio.ini

@@ -36,9 +36,9 @@ build_src_filter =
 
 test_framework = unity
 test_filter = 
- native/test_motion_sim*
- native/test_maincontrol*
-debug_test = native/test_motion_sim*
+ native/test_*
+
+debug_test = native/test_motion_control
 
 check_patterns =
  lib/*