|
@@ -294,52 +294,56 @@ void LimiterSwitchTask (void* arg) {
|
|
|
sensorsInfo.limitXSwitchDown = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_12);
|
|
|
pinStates = (limitXSwitchDownPrevState << 1) | sensorsInfo.limitXSwitchDown;
|
|
|
if ((pinStates & 0x3) == 0x1) {
|
|
|
- limiterXTriggered = 1;
|
|
|
- sensorsInfo.currentXPosition = 0;
|
|
|
- sensorsInfo.positionXWeak = 0;
|
|
|
+ limiterXTriggered = 1;
|
|
|
+ sensorsInfo.positionXOffset = 0 - sensorsInfo.currentXPosition;
|
|
|
+ sensorsInfo.positionXWeak = 0;
|
|
|
}
|
|
|
limitXSwitchDownPrevState = sensorsInfo.limitXSwitchDown;
|
|
|
|
|
|
sensorsInfo.limitXSwitchUp = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_13);
|
|
|
pinStates = (limitXSwitchUpPrevState << 1) | sensorsInfo.limitXSwitchUp;
|
|
|
if ((pinStates & 0x3) == 0x1) {
|
|
|
- limiterXTriggered = 1;
|
|
|
- sensorsInfo.currentXPosition = 100;
|
|
|
- sensorsInfo.positionXWeak = 0;
|
|
|
+ limiterXTriggered = 1;
|
|
|
+ sensorsInfo.positionXOffset = 100 - sensorsInfo.currentXPosition;
|
|
|
+ sensorsInfo.positionXWeak = 0;
|
|
|
}
|
|
|
limitXSwitchUpPrevState = sensorsInfo.limitXSwitchUp;
|
|
|
|
|
|
sensorsInfo.limitXSwitchCenter = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_10);
|
|
|
pinStates = (limitXSwitchCenterPrevState << 1) | sensorsInfo.limitXSwitchCenter;
|
|
|
if ((pinStates & 0x3) == 0x1) {
|
|
|
- sensorsInfo.currentXPosition = AXE_X_MIDDLE_VALUE;
|
|
|
- sensorsInfo.positionXWeak = 0;
|
|
|
+ sensorsInfo.positionXOffset = AXE_X_MIDDLE_VALUE - sensorsInfo.currentXPosition;
|
|
|
+ ;
|
|
|
+ sensorsInfo.positionXWeak = 0;
|
|
|
}
|
|
|
limitXSwitchCenterPrevState = sensorsInfo.limitXSwitchCenter;
|
|
|
|
|
|
sensorsInfo.limitYSwitchDown = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_11);
|
|
|
pinStates = (limitYSwitchDownPrevState << 1) | sensorsInfo.limitYSwitchDown;
|
|
|
if ((pinStates & 0x3) == 0x1) {
|
|
|
- limiterYTriggered = 1;
|
|
|
- sensorsInfo.currentYPosition = 0;
|
|
|
- sensorsInfo.positionYWeak = 0;
|
|
|
+ limiterYTriggered = 1;
|
|
|
+ sensorsInfo.positionYOffset = 0 - sensorsInfo.currentYPosition;
|
|
|
+ ;
|
|
|
+ sensorsInfo.positionYWeak = 0;
|
|
|
}
|
|
|
limitYSwitchDownPrevState = sensorsInfo.limitYSwitchDown;
|
|
|
|
|
|
sensorsInfo.limitYSwitchUp = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_9);
|
|
|
pinStates = (limitYSwitchUpPrevState << 1) | sensorsInfo.limitYSwitchUp;
|
|
|
if ((pinStates & 0x3) == 0x1) {
|
|
|
- limiterYTriggered = 1;
|
|
|
- sensorsInfo.currentYPosition = 100;
|
|
|
- sensorsInfo.positionYWeak = 0;
|
|
|
+ limiterYTriggered = 1;
|
|
|
+ sensorsInfo.positionYOffset = 100 - sensorsInfo.currentYPosition;
|
|
|
+ ;
|
|
|
+ sensorsInfo.positionYWeak = 0;
|
|
|
}
|
|
|
limitYSwitchUpPrevState = sensorsInfo.limitYSwitchUp;
|
|
|
|
|
|
sensorsInfo.limitYSwitchCenter = HAL_GPIO_ReadPin (GPIOD, GPIO_PIN_8);
|
|
|
pinStates = (limitYSwitchCenterPrevState << 1) | sensorsInfo.limitYSwitchCenter;
|
|
|
if ((pinStates & 0x3) == 0x1) {
|
|
|
- sensorsInfo.currentYPosition = AXE_Y_MIDDLE_VALUE;
|
|
|
- sensorsInfo.positionYWeak = 0;
|
|
|
+ sensorsInfo.currentYPosition = AXE_Y_MIDDLE_VALUE - sensorsInfo.currentYPosition;
|
|
|
+ ;
|
|
|
+ sensorsInfo.positionYWeak = 0;
|
|
|
}
|
|
|
limitYSwitchCenterPrevState = sensorsInfo.limitYSwitchCenter;
|
|
|
if (((sensorsInfo.limitXSwitchDown == 1) || (sensorsInfo.limitXSwitchUp == 1)) && (limiterXTriggered == 1)) {
|