Merge pull request #13 from adamb314/develop

Develop

.gitignore

@@ -3,8 +3,11 @@
 ArduinoSketch/enableAutoUpload
 ..*
 *.sublime-workspace
+*.sublime-project
 ArduinoSketch/config/*
 CadFiles/**/*.FCStd1
 PcbDesignes/**/*-cache*
 PcbDesignes/**/*.sch-bak
 .thankyou
+ArduinoSketch/last*.txt
+ArduinoSketch/*ToLoad.txt

ArduinoLibrary/ServoProjectController/ServoProjectController.cpp

@@ -222,17 +222,20 @@ void DCServoCommunicator::updateOffset()
     }
 }
 
-void DCServoCommunicator::setControlSpeed(unsigned char controlSpeed)
+void DCServoCommunicator::setControlSpeed(unsigned char controlSpeed, float inertiaMarg)
 {
-    setControlSpeed(controlSpeed, controlSpeed * 4, controlSpeed * 32);
+    setControlSpeed(controlSpeed, controlSpeed * 4, controlSpeed * 32, inertiaMarg);
 }
 
 void DCServoCommunicator::setControlSpeed(unsigned char controlSpeed,
-        unsigned short int velControlSpeed, unsigned short int filterSpeed)
+        unsigned short int velControlSpeed, unsigned short int filterSpeed,
+        float inertiaMarg)
 {
     this->controlSpeed = controlSpeed;
     this->velControlSpeed = velControlSpeed;
     this->filterSpeed = filterSpeed;
+    inertiaMarg = stdmin::min(stdmin::max(inertiaMarg, 1.0), 1.0 + 255.0 / 128);
+    this->inertiaMarg = static_cast<unsigned char>(stdmin::round((inertiaMarg - 1.0) * 128.0));
 }
 
 void DCServoCommunicator::setBacklashControlSpeed(unsigned char backlashCompensationSpeed,
@@ -271,7 +274,18 @@ void DCServoCommunicator::setReference(const float& pos, const float& vel, const
     newPositionReference = true;
     newOpenLoopControlSignal = false;
     refPos = (pos - offset) / scale * positionUpscaling;
-    refVel = vel / scale;
+
+    int sign = 0;
+    if (vel > 0.0f)
+    {
+        sign = 1;
+    }
+    else if (vel < 0.0f)
+    {
+        sign = -1;
+    }
+    refVel = stdmin::round(vel / scale);
+    refVel = stdmin::max(1, stdmin::abs(refVel)) * sign;
 
     if (refVel > 4)
     {
@@ -458,6 +472,7 @@ CommunicationError DCServoCommunicator::run()
         bus->write(3, static_cast<char>(controlSpeed));
         bus->write(4, static_cast<char>(stdmin::round(velControlSpeed / 4.0f)));
         bus->write(5, static_cast<char>(stdmin::round(filterSpeed / 32.0f)));
+        bus->write(10, static_cast<char>(inertiaMarg));
         bus->write(6, static_cast<char>(backlashCompensationSpeed));
         bus->write(7, static_cast<char>(backlashCompensationSpeedVelDecrease));
         bus->write(8, static_cast<char>(backlashSize));

ArduinoLibrary/ServoProjectController/ServoProjectController.h

@@ -249,7 +249,7 @@ class ContinuousValueUpCaster
 
 class DCServoCommunicator
 {
-  public:
+public:
     class OpticalEncoderChannelData
     {
     public:
@@ -265,9 +265,9 @@ class DCServoCommunicator
 
     void setOffsetAndScaling(float scale, float offset, float startPosition = 0);
 
-    void setControlSpeed(unsigned char controlSpeed);
+    void setControlSpeed(unsigned char controlSpeed, float inertiaMarg = 1.0);
     void setControlSpeed(unsigned char controlSpeed, unsigned short int velControlSpeed,
-            unsigned short int filterSpeed);
+            unsigned short int filterSpeed, float inertiaMarg = 1.0);
 
     void setBacklashControlSpeed(unsigned char backlashCompensationSpeed,
             float backlashCompensationCutOffSpeed, float backlashSize);
@@ -312,7 +312,7 @@ class DCServoCommunicator
 
     CommunicationError run();
 
-  private:
+private:
     void updateOffset();
 
     Communication* bus{nullptr};
@@ -329,6 +329,7 @@ class DCServoCommunicator
     unsigned char controlSpeed{50};
     unsigned short int velControlSpeed{50 * 4};
     unsigned short int filterSpeed{50 * 32};
+    unsigned char inertiaMarg{0};
     unsigned char backlashCompensationSpeed{10};
     unsigned char backlashCompensationSpeedVelDecrease{0};
     unsigned char backlashSize{0};

ArduinoSketch/ArduinoSketch.ino

@@ -1,5 +1,5 @@
-#include "ArduinoC++BugFixes.h"
-#include "ThreadHandler.h"
+#include "src/ArduinoC++BugFixes.h"
+#include "src/Hardware/ThreadHandler.h"
 
 #include "config/config.h"