timur/LibSidServo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PID works

Browse Source
This commit is contained in:
Edward V. Emelianov
2026-03-23 17:21:33 +03:00
parent 9e7c732afd
commit 071e7fc189
6 changed files with 85 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
ssii.c
View File

@@ -26,7 +26,7 @@
#include "serial.h"
#include "ssii.h"
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0;
int X_ENC_ZERO = 0, Y_ENC_ZERO = 0; // will be filled later from config
// defaults until read from controller
double X_MOT_STEPSPERREV = 13312000.,
Y_MOT_STEPSPERREV = 17578668.,
@@ -45,12 +45,13 @@ uint16_t SScalcChecksum(uint8_t *buf, int len){
}
// Next three functions runs under locked mountdata_t mutex and shouldn't call locked it again!!
static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int *nstopped, axis_status_t stat){
static axis_status_t chkstopstat(int32_t *prev, int32_t cur, int32_t tag, int *nstopped, axis_status_t stat){
if(*prev == INT32_MAX){
stat = AXIS_STOPPED;
DBG("START");
}else if(stat == AXIS_GONNASTOP){ // check stop
}else if(stat == AXIS_GONNASTOP || (stat != AXIS_STOPPED && cur == tag)){ // got command "stop" or motor is on target
if(*prev == cur){
DBG("Test for stop, nstopped=%d", *nstopped);
if(++(*nstopped) > MOTOR_STOPPED_CNT){
stat = AXIS_STOPPED;
DBG("AXIS stopped");
@@ -68,8 +69,8 @@ static void ChkStopped(const SSstat *s, mountdata_t *m){
static int32_t Xmot_prev = INT32_MAX, Ymot_prev = INT32_MAX; // previous coordinates
static int Xnstopped = 0, Ynstopped = 0; // counters to get STOPPED state
axis_status_t Xstat, Ystat;
Xstat = chkstopstat(&Xmot_prev, s->Xmot, &Xnstopped, m->Xstate);
Ystat = chkstopstat(&Ymot_prev, s->Ymot, &Ynstopped, m->Ystate);
Xstat = chkstopstat(&Xmot_prev, s->Xmot, m->Xtarget, &Xnstopped, m->Xstate);
Ystat = chkstopstat(&Ymot_prev, s->Ymot, m->Ytarget, &Ynstopped, m->Ystate);
if(Xstat != m->Xstate || Ystat != m->Ystate){
DBG("Status changed");
setStat(Xstat, Ystat);