Merge pull request #530 from blurfl/B99-FAKE_SERVO-control

add B99 command to control FAKE_SERVO mode

cnc_ctrl_v1/Axis.cpp

@@ -17,6 +17,7 @@
 
 
 #include "Maslow.h"
+// #include <EEPROM.h>
 
 void Axis::setup(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2, const char& axisName, const unsigned long& loopInterval)
 {
@@ -84,14 +85,14 @@ void   Axis::setSteps(const long& steps){
 
 void   Axis::computePID(){
 
-    #ifdef FAKE_SERVO
-      if (motorGearboxEncoder.motor.attached()){
-        // Adds up to 10% error just to simulate servo noise
-        double rpm = (-1 * _pidOutput) * random(90, 110) / 100;
-        unsigned long steps = motorGearboxEncoder.encoder.read() + round( rpm * *_encoderSteps * LOOPINTERVAL)/(60 * 1000000);
-        motorGearboxEncoder.encoder.write(steps);
+      if (FAKE_SERVO_STATE != 0) {
+          if (motorGearboxEncoder.motor.attached()){
+            // Adds up to 10% error just to simulate servo noise
+            double rpm = (-1 * _pidOutput) * random(90, 110) / 100;
+            unsigned long steps = motorGearboxEncoder.encoder.read() + round( rpm * *_encoderSteps * LOOPINTERVAL)/(60 * 1000000);
+            motorGearboxEncoder.encoder.write(steps);
+          }
       }
-    #endif
 
     if (_disableAxisForTesting || !motorGearboxEncoder.motor.attached()){
         return;

cnc_ctrl_v1/Config.h

@@ -25,9 +25,12 @@
                            // LOOPINTERVAL tuning
 #define KINEMATICSDBG 0    // set to 1 for additional kinematics debug messaging
 
-// #define FAKE_SERVO      // Uncomment this line to cause the Firmware to mimic
-                           // a servo updating the encoder steps even if no servo
-                           // is connected.  Useful for testing on an arduino only
+#define FAKE_SERVO 4095    // store the state of FAKE_SERVO in EEPROM[ 4095 ] to preserve
+                           // the state of FAKE_SERVO mode over resets.
+                           // Use 'B99 ON' to turn FAKE_SERVO mode on and set EEPROM[ 4095 ] to '1',
+                           // 'B99' with no parameter, or any parameter other than 'ON' 
+                           // puts a '0' in that location and turns FAKE_SERVO mode off.
+                           // Useful for testing on an arduino only (e.g. without motors).
 
 // #define SIMAVR          // Uncomment this if you plan to run the Firmware in the simavr
                            // simulator. Normally, you would not define this directly, but

cnc_ctrl_v1/GCode.cpp

@@ -19,6 +19,7 @@ Copyright 2014-2017 Bar Smith*/
 // commands
 
 #include "Maslow.h"
+#include <EEPROM.h>
 
 maslowRingBuffer incSerialBuffer;
 String readyCommandString = "";  //KRK why is this a global?
@@ -367,7 +368,35 @@ byte  executeBcodeLine(const String& gcodeLine){
 
         return STATUS_OK;
     }
-    return STATUS_INVALID_STATEMENT;
+
+        // Use 'B99 ON' to set FAKE_SERVO mode on,
+        // 'B99' with no parameter, or any parameter other than 'ON' 
+        // turns FAKE_SERVO mode off.
+        // FAKE_SERVO mode causes the Firmware to mimic a servo,
+        // updating the encoder steps even if no servo is connected.
+        // Useful for testing on an arduino only (e.g. without motors).
+        // The status of FAKE_SERVO mode is stored in EEPROM[ 4095 ] 
+        // to persist between resets. Tthat byte is set to '1' when FAKE_SERVO
+        // is on, '0' when off. settingsWipe(SETTINGS_RESTORE_ALL) clears the
+        // EEPROM to '0', sothat stores '0' at EEPROM[ 4095 ] as well.
+        if(gcodeLine.substring(0, 3) == "B99") {
+        int letterO = gcodeLine.indexOf('O');
+        int letterN = gcodeLine.indexOf('N');
+        if ((letterO != -1) && (letterN != -1)) {
+          EEPROM[ FAKE_SERVO ] = 1;
+          FAKE_SERVO_STATE = 1;
+        } else {
+          EEPROM[ FAKE_SERVO ] = 0;
+          FAKE_SERVO_STATE = 0;
+        }
+        if (FAKE_SERVO_STATE == 0) {
+          Serial.println(F("FAKE_SERVO off"));
+        } else {
+          Serial.println(F("FAKE_SERVO on"));
+        }
+        return(STATUS_OK);
+     }
+   return STATUS_INVALID_STATEMENT;
 }
 
 void  executeGcodeLine(const String& gcodeLine){

cnc_ctrl_v1/GCode.h

@@ -46,5 +46,6 @@ void  G38(const String&);
 void  setInchesToMillimetersConversion(float);
 extern int SpindlePowerControlPin;
 extern int ProbePin;
+extern int FAKE_SERVO_STATE;
 
 #endif

cnc_ctrl_v1/Motor.cpp

@@ -113,7 +113,7 @@ void Motor::write(int speed, bool force){
     /*
     Sets motor speed from input. Speed = 0 is stopped, -255 is full reverse, 255 is full ahead. If force is true the motor attached state will be ignored
     */
-    if (_attachedState == 1 or force){
+    if ((_attachedState == 1 or force) and (FAKE_SERVO_STATE == 0)){
         speed = constrain(speed, -255, 255);
         _lastSpeed = speed; //saves speed for use in additive write
         bool forward = (speed > 0);

cnc_ctrl_v1/cnc_ctrl_v1.ino

@@ -35,6 +35,7 @@
 // TLE5206 version
 
 #include "Maslow.h"
+#include <EEPROM.h>
 
 // Define system global state structure
 system_t sys;
@@ -45,6 +46,9 @@ settings_t sysSettings;
 // Global realtime executor bitflag variable for setting various alarms.
 byte systemRtExecAlarm;  
 
+// Define global flag for FAKE_SERVO state
+int FAKE_SERVO_STATE = 0;
+
 // Define axes, it might be tighter to define these within the sys struct
 Axis leftAxis;
 Axis rightAxis;
@@ -63,6 +67,12 @@ void setup(){
     Serial.println(F(" Detected"));
     sys.inchesToMMConversion = 1;
     sys.writeStepsToEEPROM = false;
+    FAKE_SERVO_STATE = EEPROM[ FAKE_SERVO ] & B00000001;
+   if (FAKE_SERVO_STATE == 0) {
+       Serial.println(F("FAKE_SERVO off"));
+   } else {
+       Serial.println(F("FAKE_SERVO on"));
+   }
     settingsLoadFromEEprom();
     sys.feedrate = sysSettings.maxFeed / 2.0;
     setupAxes();