diff --git a/Core/Inc/actio.h b/Core/Inc/actio.h index 5317b76..5de9a17 100644 --- a/Core/Inc/actio.h +++ b/Core/Inc/actio.h @@ -22,8 +22,8 @@ typedef struct Actionneurs { bool etatLedArmement; bool enJeu; uint16_t armementTirette; - uint16_t positionServo[NOMBRE_SERVOS]; - uint16_t valeursPositionsServos[NOMBRE_SERVOS][NOMBRE_POSITIONS_SERVOS]; + uint16_t angleServo[NOMBRE_SERVOS]; + // uint16_t valeursPositionsServos[NOMBRE_SERVOS][NOMBRE_POSITIONS_SERVOS]; uint16_t positionAscenseur; uint16_t valeursPositionsAscenseur[NOMBRE_POSITIONS_ASC]; GPIO_TypeDef * relayPorts[NOMBRE_RELAIS]; @@ -31,16 +31,15 @@ typedef struct Actionneurs { } Actionneurs; void initActionneurs(); -void setServoPosValue(uint16_t servoNum, uint16_t posNum, uint16_t val); void setAscPosValue(uint16_t posNum, uint16_t val); void PCA9685_ContinuousServoRun(uint8_t Channel); void PCA9685_ContinuousServoStop(uint8_t Channel); void PCA9685_LEDOn(uint8_t Channel); void PCA9685_LEDOff(uint8_t Channel); -uint16_t getServoPos(uint16_t servoNum); +float getServoAngle(uint16_t servoNum); uint16_t getRelayState(uint16_t relayNum); uint16_t getAscPos(); -void moveServo(uint16_t servoNum, uint16_t posNum); +void moveServo(uint16_t servoNum, float angle); void moveRelay(uint16_t relayNum, uint16_t state); void moveAsc(uint16_t posNum); void armTirette(); diff --git a/Core/Src/actio.c b/Core/Src/actio.c index 0a436cb..5c98328 100644 --- a/Core/Src/actio.c +++ b/Core/Src/actio.c @@ -182,10 +182,7 @@ void initActionneurs(){ act.armementTirette = 0; act.enJeu = 0; for(uint16_t servoId = 0; servoId < NOMBRE_SERVOS; servoId++){ - act.positionServo[servoId] = 0; - for(uint16_t posId = 0; posId < NOMBRE_POSITIONS_SERVOS; posId++){ - act.valeursPositionsServos[servoId][posId] = 0; - } + act.angleServo[servoId] = 0; } act.positionAscenseur = 0; for(uint16_t posId = 0; posId < NOMBRE_POSITIONS_ASC; posId++){ @@ -199,16 +196,16 @@ void initActionneurs(){ act.relayPortsNumbers[2] = RELAY_3_PIN; } -void setServoPosValue(uint16_t servoNum, uint16_t posNum, uint16_t val){ +/*void setServoPosValue(uint16_t servoNum, uint16_t posNum, uint16_t val){ act.valeursPositionsServos[servoNum][posNum] = val; -} +}*/ void setAscPosValue(uint16_t posNum, uint16_t val){ act.valeursPositionsAscenseur[posNum] = val; } -uint16_t getServoPos(uint16_t servoNum){ - return act.positionServo[servoNum]; +float getServoAngle(uint16_t servoNum){ + return act.angleServo[servoNum]; } uint16_t getRelayState(uint16_t relayNum) @@ -220,9 +217,9 @@ uint16_t getAscPos(){ return act.positionAscenseur; } -void moveServo(uint16_t servoNum, uint16_t posNum){ - PCA9685_SetServoAngle(servoNum, act.valeursPositionsServos[servoNum][posNum]); - act.positionServo[servoNum] = posNum; +void moveServo(uint16_t servoNum, float angle){ + PCA9685_SetServoAngle(servoNum, angle); + act.angleServo[servoNum] = angle; } void moveRelay(uint16_t relayNum, uint16_t state){ diff --git a/Core/Src/comm.c b/Core/Src/comm.c index bf84dd3..c297d6a 100644 --- a/Core/Src/comm.c +++ b/Core/Src/comm.c @@ -94,10 +94,10 @@ void USBProtocol_ProcessCommand(char *cmd) { break; } int servoId = atoi(servoIdStr); - int pos = getServoPos(servoId); + float angle = getServoAngle(servoId); /* append \";id;pos\" for each servo, check remaining buffer */ - int wrote = snprintf(response + off, sizeof(response) - off, ";%d;%d", servoId, pos); + int wrote = snprintf(response + off, sizeof(response) - off, ";%d;%.4f", servoId, angle); if (wrote < 0 || (size_t)wrote >= sizeof(response) - off) { success = 0; break; @@ -176,43 +176,6 @@ void USBProtocol_ProcessCommand(char *cmd) { } sprintf(response, "%s;%s;%s;%d\n", success ? "OK" : "KO", arg1, arg2, val); CDC_Transmit_FS((uint8_t*)response, strlen(response)); - } else if (strcmp(arg1, "SERVO") == 0 && strcmp(arg2, "POS") == 0){ - int n = atoi(arg3); - size_t off = 0; - off += snprintf(response + off, sizeof(response) - off, "%s;%s;%s;%d", "OK", arg1, arg2, n); - - for (int i = 0; i < n; ++i) { - char *servoIdStr = strtok(NULL, ";"); - char *posStr = strtok(NULL, ";"); - char *angleStr = strtok(NULL, ";"); - if (!servoIdStr || !posStr || !angleStr) { - success = 0; - break; - } - int servoId = atoi(servoIdStr); - int pos = atoi(posStr); - int angle = atoi(angleStr); - setServoPosValue(servoId, pos, angle); - - int wrote = snprintf(response + off, sizeof(response) - off, ";%d;%d", servoId, pos); - if (wrote < 0 || (size_t)wrote >= sizeof(response) - off) { - success = 0; - break; - } - off += wrote; - } - - if (!success) { - snprintf(response, sizeof(response), "KO;%s;%s\n", arg1, arg2); - } else { - if (off < sizeof(response) - 1) { - response[off++] = '\n'; - response[off] = '\0'; - } else { - response[sizeof(response) - 1] = '\0'; - } - } - CDC_Transmit_FS((uint8_t*)response, strlen(response)); } } else if (strcmp(type, "MOV") == 0) { int success = 1; @@ -223,16 +186,16 @@ void USBProtocol_ProcessCommand(char *cmd) { for (int i = 0; i < n; ++i) { char *servoIdStr = strtok(NULL, ";"); - char *posStr = strtok(NULL, ";"); - if (!servoIdStr || !posStr) { + char *angleStr = strtok(NULL, ";"); + if (!servoIdStr || !angleStr) { success = 0; break; } int servoId = atoi(servoIdStr); - int pos = atoi(posStr); - moveServo(servoId, pos); + float angle = atof(angleStr); + moveServo(servoId, angle); - int wrote = snprintf(response + off, sizeof(response) - off, ";%d;%d", servoId, pos); + int wrote = snprintf(response + off, sizeof(response) - off, ";%d;%.4f", servoId, angle); if (wrote < 0 || (size_t)wrote >= sizeof(response) - off) { success = 0; break;