Skip to content

Commit

Permalink
Merge pull request #8 from ponoor/develop
Browse files Browse the repository at this point in the history 
Added minSpeed commands for the firmware and the config tool.
  • Loading branch information
@kanta
kanta authored Nov 25, 2021
2 parents 2b7f633 + 0c685c8 commit a46ec21
Show file tree
Hide file tree
Showing 23 changed files with 1,933 additions and 54 deletions.
6 changes: 3 additions & 3 deletions STEP800_firmware/STEP800_firmware.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,8 @@ const char *firmwareName = "STEP800_PROTO_BLACK";
#else
const char *firmwareName = "STEP800_R1";
#endif
const uint8_t firmwareVersion[3] = {1,0,0};
const uint8_t applicableConfigVersion[2] = {1,0};
const uint8_t firmwareVersion[3] = {1,0,1};
const uint8_t applicableConfigVersion[2] = {1,1};

// L6470vh
#ifdef PROTOTYPE_BLACK
Expand Down Expand Up @@ -357,7 +357,7 @@ void updateServo(uint32_t currentTimeMicros) {
integral[i] += ((error + eZ1[i]) / 2.0f);
if (integral[i] > 1500.0f) integral[i] = 1500.0f;
else if (integral[i] < -1500.0f) integral[i] = -1500.0f;
if (abs(error) > position_tolerance) {
if (fabsf(error) > position_tolerance) {
double diff = error - eZ1[i];

spd = error * kP[i] + integral[i] * kI[i] + diff * kD[i];
Expand Down
4 changes: 3 additions & 1 deletion STEP800_firmware/diagnosis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -283,6 +283,7 @@ void printConfigurations() {
printAllData(F("dec"), dec);
printAllData(F("maxSpeed"), maxSpeed);
printAllData(F("fullStepSpeed"), fullStepSpeed);
printAllData("minSpeed", minSpeed);

printHeader(F("Voltage mode"));
printAllData(F("kvalHold"), kvalHold);
Expand All @@ -294,7 +295,8 @@ void printConfigurations() {
printAllData(F("accFinalSlope"), accFinalSlope);
printAllData(F("decFinalSlope"), decFinalSlope);
printAllData(F("stallThreshold"), stallThreshold);
printAllData(F("lowSpeedOptimize"), lowSpeedOptimize);
showAllBools("lowSpeedOptimizeEnable", lowSpeedOptimizeEnable);
printAllData("lowSpeedOptimizeThreshold", lowSpeedOptimizeThreshold);

printHeader(F("Servo mode"));
printAllData(F("kP"), kP);
Expand Down
4 changes: 3 additions & 1 deletion STEP800_firmware/globals.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,14 +119,16 @@ uint8_t
microStepMode[NUM_OF_MOTOR]; // STEP_MODE
uint16_t slewRate[NUM_OF_MOTOR]; // GATECFG1
uint8_t slewRateNum[NUM_OF_MOTOR]; // [0]114, [1]220, [2]400, [3]520, [4]790, [5]980.
float lowSpeedOptimize[NUM_OF_MOTOR];
bool lowSpeedOptimizeEnable[NUM_OF_MOTOR];
float lowSpeedOptimizeThreshold[NUM_OF_MOTOR];
bool electromagnetBrakeEnable[NUM_OF_MOTOR];
uint16_t brakeTransitionDuration[NUM_OF_MOTOR];

float
acc[NUM_OF_MOTOR],
dec[NUM_OF_MOTOR],
maxSpeed[NUM_OF_MOTOR],
minSpeed[NUM_OF_MOTOR],
fullStepSpeed[NUM_OF_MOTOR];

// Servo mode
Expand Down
4 changes: 3 additions & 1 deletion STEP800_firmware/globals.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,13 +255,15 @@ extern uint8_t
microStepMode[NUM_OF_MOTOR]; // STEP_MODE
extern uint16_t slewRate[NUM_OF_MOTOR]; // GATECFG1
extern uint8_t slewRateNum[NUM_OF_MOTOR]; // [0]114, [1]220, [2]400, [3]520, [4]790, [5]980.
extern float lowSpeedOptimize[NUM_OF_MOTOR];
extern float lowSpeedOptimizeThreshold[NUM_OF_MOTOR];
extern bool lowSpeedOptimizeEnable[NUM_OF_MOTOR];
extern bool electromagnetBrakeEnable[NUM_OF_MOTOR];
extern uint16_t brakeTransitionDuration[NUM_OF_MOTOR];
extern float
acc[NUM_OF_MOTOR],
dec[NUM_OF_MOTOR],
maxSpeed[NUM_OF_MOTOR],
minSpeed[NUM_OF_MOTOR],
fullStepSpeed[NUM_OF_MOTOR];

// Servo mode
Expand Down
6 changes: 5 additions & 1 deletion STEP800_firmware/loadConfig.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,11 +147,13 @@ void loadConfig() {
JsonArray speedProfile_dec = speedProfile["dec"];
JsonArray speedProfile_maxSpeed = speedProfile["maxSpeed"];
JsonArray speedProfile_fullStepSpeed = speedProfile["fullStepSpeed"];
JsonArray speedProfile_minSpeed = speedProfile["minSpeed"];
for (i = 0; i < NUM_OF_MOTOR; i++) {
acc[i] = speedProfile_acc[i] | 1000.;
dec[i] = speedProfile_dec[i] | 1000.;
maxSpeed[i] = speedProfile_maxSpeed[i] | 650.;
fullStepSpeed[i] = speedProfile_fullStepSpeed[i] | 15610.;
minSpeed[i] = speedProfile_minSpeed[i] | 0.;
}

// Voltage mode
Expand All @@ -165,6 +167,7 @@ void loadConfig() {
JsonArray voltageMode_FN_SLP_ACC = voltageMode["FN_SLP_ACC"];
JsonArray voltageMode_FN_SLP_DEC = voltageMode["FN_SLP_DEC"];
JsonArray voltageMode_STALL_TH = voltageMode["STALL_TH"];
JsonArray voltageMode_lowSpeedOptimizeEnable = voltageMode["lowSpeedOptimizeEnable"];
JsonArray voltageMode_lowSpeedOptimize = voltageMode["lowSpeedOptimize"];
for (i = 0; i < NUM_OF_MOTOR; i++) {
kvalHold[i] = voltageMode_KVAL_HOLD[i] | 0;
Expand All @@ -177,7 +180,8 @@ void loadConfig() {
decFinalSlope[i] = voltageMode_FN_SLP_DEC[i] | 0x29;
// stallThreshold[i] = voltageMode_STALL_TH[i] | 0x1F;
stallThreshold[i] = voltageMode_STALL_TH[i] | 0x7F;
lowSpeedOptimize[i] = voltageMode_lowSpeedOptimize[i] | 20.;
lowSpeedOptimizeEnable[i] = voltageMode_lowSpeedOptimizeEnable[i] | false;
lowSpeedOptimizeThreshold[i] = voltageMode_lowSpeedOptimize[i] | 20.;
}

// Current mode
Expand Down
63 changes: 56 additions & 7 deletions STEP800_firmware/oscListeners.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ void OSCMsgReceive() {
// speed
bMsgRouted |= msgIN.route("/setSpeedProfile", setSpeedProfile);
bMsgRouted |= msgIN.route("/setMaxSpeed", setMaxSpeed);
bMsgRouted |= msgIN.route("/setMinSpeed", setMinSpeed);
bMsgRouted |= msgIN.route("/setFullstepSpeed", setFullstepSpeed);
bMsgRouted |= msgIN.route("/getFullstepSpeed", getFullstepSpeed);
bMsgRouted |= msgIN.route("/setAcc", setAcc);
Expand Down Expand Up @@ -116,6 +117,7 @@ void OSCMsgReceive() {
bMsgRouted |= msgIN.route("/getStallThreshold", getStallThreshold);
bMsgRouted |= msgIN.route("/setOverCurrentThreshold", setOverCurrentThreshold);
bMsgRouted |= msgIN.route("/getOverCurrentThreshold", getOverCurrentThreshold);
bMsgRouted |= msgIN.route("/enableLowSpeedOptimize", enableLowSpeedOptimize);
bMsgRouted |= msgIN.route("/setLowSpeedOptimizeThreshold", setLowSpeedOptimizeThreshold);
bMsgRouted |= msgIN.route("/getLowSpeedOptimizeThreshold", getLowSpeedOptimizeThreshold);

Expand Down Expand Up @@ -732,16 +734,18 @@ void getOverCurrentThreshold(OSCMessage& msg, int addrOffset) {

void setLowSpeedOptimizeThreshold(OSCMessage& msg, int addrOffset) {
uint8_t motorID = getInt(msg, 0);
float _minSpeed = getFloat(msg,1);
float _lowSpdOptThreshold = getFloat(msg,1);
if(isCorrectMotorId(motorID)) {
motorID -= MOTOR_ID_FIRST;
stepper[motorID].setMinSpeed(_minSpeed);
lowSpeedOptimize[motorID] = _minSpeed;
lowSpeedOptimizeThreshold[motorID] = _lowSpdOptThreshold;
if (lowSpeedOptimizeEnable[motorID])
stepper[motorID].setMinSpeed(_lowSpdOptThreshold);
}
else if (motorID == MOTOR_ID_ALL) {
for (uint8_t i = 0; i < NUM_OF_MOTOR; i++) {
stepper[i].setMinSpeed(_minSpeed);
lowSpeedOptimize[i] = _minSpeed;
lowSpeedOptimizeThreshold[i] = _lowSpdOptThreshold;
if (lowSpeedOptimizeEnable[i])
stepper[i].setMinSpeed(_lowSpdOptThreshold);
}
}
}
Expand All @@ -762,7 +766,7 @@ void getLowSpeedOptimizeThreshold(uint8_t motorId) {
bool optimizationEnabled = (stepper[motorId].getParam(MIN_SPEED) & (1 << 12)) > 0;
OSCMessage newMes("/lowSpeedOptimizeThreshold");
newMes.add((int32_t)motorId + MOTOR_ID_FIRST);
newMes.add(stepper[motorId].getMinSpeed());
newMes.add(lowSpeedOptimizeEnable[motorId]);
newMes.add(optimizationEnabled);
Udp.beginPacket(destIp, outPort);
newMes.send(Udp);
Expand All @@ -771,6 +775,32 @@ void getLowSpeedOptimizeThreshold(uint8_t motorId) {
turnOnTXL();
}

void enableLowSpeedOptimize(OSCMessage& msg, int addrOffset) {
uint8_t motorID = getInt(msg, 0);
bool state = getBool(msg, 1);
if(isCorrectMotorId(motorID)) {
motorID -= MOTOR_ID_FIRST;
stepper[motorID].setLoSpdOpt(state);
lowSpeedOptimizeEnable[motorID] = state;
if (state) {
stepper[motorID].setMinSpeed(lowSpeedOptimizeThreshold[motorID]);
} else {
stepper[motorID].setMinSpeed(minSpeed[motorID]);
}
}
else if (motorID == MOTOR_ID_ALL) {
for (uint8_t i = 0; i < NUM_OF_MOTOR; i++) {
stepper[i].setLoSpdOpt(state);
lowSpeedOptimizeEnable[i] = state;
if (state) {
stepper[i].setMinSpeed(lowSpeedOptimizeThreshold[i]);
} else {
stepper[i].setMinSpeed(minSpeed[i]);
}
}
}
}

void setBemfParam(OSCMessage& msg, int addrOffset) {
uint8_t motorID = getInt(msg, 0);
uint16_t intSpeed = constrain(getInt(msg, 1), 0, 0x3FFF);
Expand Down Expand Up @@ -1429,7 +1459,26 @@ void setMaxSpeed(OSCMessage& msg, int addrOffset) {
}
}
}
// MIN_SPEED register is set by setLowSpeedOptimizeThreshold function.

void setMinSpeed(OSCMessage& msg, int addrOffset) {
uint8_t motorID = getInt(msg, 0);
float _minSpeed = getFloat(msg, 1);
if(isCorrectMotorId(motorID)) {
motorID -= MOTOR_ID_FIRST;
minSpeed[motorID] = _minSpeed;
if (!lowSpeedOptimizeEnable[motorID]) {
stepper[motorID].setMinSpeed(_minSpeed);
}
}
else if (motorID == MOTOR_ID_ALL) {
for (uint8_t i = 0; i < NUM_OF_MOTOR; i++) {
minSpeed[i] = _minSpeed;
if (!lowSpeedOptimizeEnable[motorID]) {
stepper[i].setMinSpeed(_minSpeed);
}
}
}
}

void setFullstepSpeed(OSCMessage& msg, int addrOffset) {
uint8_t motorID = getInt(msg, 0);
Expand Down
3 changes: 3 additions & 0 deletions STEP800_firmware/oscListeners.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ void getOverCurrentThreshold(OSCMessage& msg, int addrOffset);
void setLowSpeedOptimizeThreshold(OSCMessage& msg, int addrOffset);
void getLowSpeedOptimizeThreshold(OSCMessage& msg, int addrOffset);
void getLowSpeedOptimizeThreshold(uint8_t motorId);
void enableLowSpeedOptimize(OSCMessage& msg, int addrOffset);
void setBemfParam(OSCMessage& msg, int addrOffset);
void getBemfParam(OSCMessage& msg, int addrOffset);
void getBemfParam(uint8_t motorId);
Expand Down Expand Up @@ -119,6 +120,8 @@ void getKval(uint8_t motorId);
// speed_commands_osc_listener
void setSpeedProfile(OSCMessage& msg, int addrOffset);
void setMaxSpeed(OSCMessage& msg, int addrOffset);
void setMinSpeed(OSCMessage& msg, int addrOffset);
void getMinSpeed(OSCMessage& msg, int addrOffset);
void setFullstepSpeed(OSCMessage& msg, int addrOffset);
void getFullstepSpeed(OSCMessage& msg, int addrOffset);
void setAcc(OSCMessage& msg, int addrOffset);
Expand Down
8 changes: 6 additions & 2 deletions STEP800_firmware/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,12 @@ void resetMotorDriver(uint8_t deviceID) {
stepper[deviceID].resetDev();
stepper[deviceID].configStepMode(microStepMode[deviceID]);
stepper[deviceID].setMaxSpeed(maxSpeed[deviceID]);
stepper[deviceID].setLoSpdOpt(true);
stepper[deviceID].setMinSpeed(lowSpeedOptimize[deviceID]); // Low speed optimization threshold
stepper[deviceID].setLoSpdOpt(lowSpeedOptimizeEnable[deviceID]);
if (lowSpeedOptimizeEnable[deviceID]) {
stepper[deviceID].setMinSpeed(lowSpeedOptimizeThreshold[deviceID]); // Low speed optimization threshold
} else {
stepper[deviceID].setMinSpeed(minSpeed[deviceID]);
}
stepper[deviceID].setFullSpeed(fullStepSpeed[deviceID]);
stepper[deviceID].setAcc(acc[deviceID]);
stepper[deviceID].setDec(dec[deviceID]);
Expand Down
10 changes: 0 additions & 10 deletions configTool/css/temp.css
View file

This file was deleted.

59 changes: 48 additions & 11 deletions configTool/index.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
<div class="container">


<h1>STEP800 configuration tool <span>rev 1.0</span></h1>
<h1>STEP800 configuration tool <span>rev 1.1</span></h1>

<div class="file-input">
<input type='file' name="load" class="file" id="test">
Expand Down Expand Up @@ -41,7 +41,7 @@ <h2>Information</h2>
</div>
<div class="input-items -sm">
<input type="number" name="configVersion" size="10" min="0" max="255" value="1" readonly="readonly">
<input type="number" name="configVersion" size="10" min="0" max="255" value="0" readonly="readonly">
<input type="number" name="configVersion" size="10" min="0" max="255" value="1" readonly="readonly">
</div>
</li>
<li>
Expand All @@ -62,7 +62,7 @@ <h2>Network</h2>
<li>
<div class="input-label">
<label class="hint">My IP address</label>
<p class="tooltip">IP address of STEP400.</p>
<p class="tooltip">IP address of STEP800.</p>
</div>
<div class="input-items -sm">
<input type="number" name="myIp" size="10" min="0" max="255" value="10">
Expand All @@ -83,7 +83,7 @@ <h2>Network</h2>
<li>
<div class="input-label">
<label class="hint">Destination IP</label>
<p class="tooltip">Specify where STEP400 responses have to be sent.</p>
<p class="tooltip">Specify where STEP800 responses have to be sent.</p>
</div>
<div class="input-items -sm">
<input type="number" name="destIp" size="10" min="0" max="255" value="10">
Expand Down Expand Up @@ -131,14 +131,14 @@ <h2>Network</h2>
<li>
<div class="input-label">
<label class="hint">Incoming port</label>
<p class="tooltip">Port number for the communication from PC to STEP400.</p>
<p class="tooltip">Port number for the communication from PC to STEP800.</p>
</div>
<div class="input-items"><input type="number" name="inPort" size="30" value="50000"></div>
</li>
<li>
<div class="input-label">
<label class="hint">Outgoing port</label>
<p class="tooltip">Port number for the communication from STEP400 to PC.</p>
<p class="tooltip">Port number for the communication from STEP800 to PC.</p>
</div>
<div class="input-items"><input type="number" name="outPort" size="30" value="50100"></div>
</li>
Expand All @@ -152,7 +152,7 @@ <h2>Network</h2>
<li>
<div class="input-label">
<label class="hint">MAC address</label>
<p class="tooltip">MAC address of STEP400.</p>
<p class="tooltip">MAC address of STEP800.</p>
</div>
<div class="input-items -sm">
<input type="text" name="mac" size="10" min="0" max="255" value="60">
Expand All @@ -174,21 +174,21 @@ <h2>Network</h2>
<li>
<div class="input-label">
<label class="hint">Booted message enable</label>
<p class="tooltip">Send /booted message when STEP400 is booted or restarted.</p>
<p class="tooltip">Send /booted message when STEP800 is booted or restarted.</p>
</div>
<div class="input-items"><input type="checkbox" name="bootedMsgEnable" checked></div>
</li>
<li>
<div class="input-label">
<label class="hint">Allow OSC reply before /setDestIp</label>
<p class="tooltip">If this box is checked, STEP400 can send response messages even before receiving the /setDestIp message.</p>
<p class="tooltip">If this box is checked, STEP800 can send response messages even before receiving the /setDestIp message.</p>
</div>
<div class="input-items"><input type="checkbox" name="canSendMsgBeforeDestIp"></div>
</li>
<li>
<div class="input-label">
<label class="hint">Report errors</label>
<p class="tooltip">Enable error messages from STEP400.</p>
<p class="tooltip">Enable error messages from STEP800.</p>
</div>
<div class="input-items"><input type="checkbox" name="reportError" checked></div>
</li>
Expand Down Expand Up @@ -1033,6 +1033,25 @@ <h2>Speed Profile</h2>
<span class="unit">step/s/s</span>
</div>
</li>
<li>
<div class="input-label">
<label class="hint">Min speed</label>
<p class="tooltip">This value defines a minimum speed of the motor, and also releaseSw speed which is a part of the homing procedure.</p>
</div>
<div class="input-items">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<input type="number" name="minSpeed" min="0.0" max="976.3" value="0" step="0.01">
<span class="unit">step/s</span>
</div>
</li>
</ul>
</ul>

<div class="heading-wrap">
Expand Down Expand Up @@ -1221,7 +1240,25 @@ <h2>Voltage Mode</h2>
</li>
<li>
<div class="input-label">
<label class="hint">lowSpeedOptimize</label>
<label class="hint">Low speed optimization enable</label>
<p class="tooltip">Reduce the phase current distortion at a very low speed using a small driving voltage. When this optimization is enabled, speed profile minimum speed is forced to zero.</p>
</div>
<div class="input-items">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<input type="checkbox" name="lowSpeedOptimizeEnable">
<span class="unit"></span>
</div>
</li>
<li>
<div class="input-label">
<label class="hint">Low speed optimization threshold</label>
<p class="tooltip">The speed threshold below which the low speed compensation works.</p>
</div>
<div class="input-items">
Expand Down
Loading

0 comments on commit a46ec21

Please sign in to comment.