/* Correction du TP 10 (partie 1, sans OS) -- extraits de Core/Src/main.c. * * Configuration CubeMX : UART4 Asynchronous PA12 (TX) / PA11 (RX), 1 Mbit/s * 8N1 ; PA10 = GPIO_Output DXL_OE, niveau de depart HIGH (ecoute) ; UART5 * 115200 et printf comme au TP 5 ; SYS Debug = Serial Wire ; horloge HSI * (UART4 a 64 MHz : USARTDIV = 64, 1 Mbit/s exact). * Pieges, voir DYNAMIXEL.md du banc de mise en service : /OE actif bas, * echo local, bascule en ecoute apres TC (HAL_UART_Transmit attend TC). */ /* ===== USER CODE BEGIN Includes ============================================= */ #include /* USER CODE END Includes */ /* ===== USER CODE BEGIN PD =================================================== */ #define DXL_PING 0x01u #define DXL_READ 0x02u #define DXL_WRITE 0x03u #define DXL_ID 1u /* a adapter au servomoteur du poste */ /* USER CODE END PD */ /* ===== USER CODE BEGIN PV =================================================== */ volatile uint8_t g_found[8]; volatile uint32_t g_found_count; /* USER CODE END PV */ /* ===== USER CODE BEGIN 0 ==================================================== */ int __io_putchar(int ch) { uint8_t c = (uint8_t)ch; HAL_UART_Transmit(&huart5, &c, 1u, 10u); return ch; } /* Envoie une instruction, lit la reponse. Renvoie 1 si la reponse est * complete, correcte et sans erreur ; recopie ses m parametres dans resp. */ static int transaction(uint8_t id, uint8_t inst, const uint8_t *p, uint8_t n, uint8_t *resp, uint8_t m) { uint8_t tx[32]; uint8_t rx[32]; uint8_t cks; if ((n > 24u) || (m > 24u)) { return 0; } tx[0] = 0xFFu; tx[1] = 0xFFu; tx[2] = id; tx[3] = (uint8_t)(n + 2u); tx[4] = inst; cks = (uint8_t)(id + tx[3] + inst); for (uint8_t k = 0u; k < n; k++) { tx[5u + k] = p[k]; cks = (uint8_t)(cks + p[k]); } tx[5u + n] = (uint8_t)~cks; /* emission : HAL_UART_Transmit ne rend la main qu'au flag TC */ HAL_GPIO_WritePin(DXL_OE_GPIO_Port, DXL_OE_Pin, GPIO_PIN_RESET); HAL_UART_Transmit(&huart4, tx, (uint16_t)(6u + n), 5u); HAL_GPIO_WritePin(DXL_OE_GPIO_Port, DXL_OE_Pin, GPIO_PIN_SET); /* l'echo a deborde pendant l'emission : on efface et on vide */ __HAL_UART_CLEAR_FLAG(&huart4, UART_CLEAR_OREF | UART_CLEAR_FEF | UART_CLEAR_NEF); __HAL_UART_SEND_REQ(&huart4, UART_RXDATA_FLUSH_REQUEST); if (id == 0xFEu) { return 1; } /* diffusion : pas de reponse */ if (HAL_UART_Receive(&huart4, rx, (uint16_t)(6u + m), 5u) != HAL_OK) { return 0; } if ((rx[0] != 0xFFu) || (rx[1] != 0xFFu) || (rx[2] != id) || (rx[3] != (uint8_t)(m + 2u))) { return 0; } cks = (uint8_t)(rx[2] + rx[3] + rx[4]); for (uint8_t k = 0u; k < m; k++) { cks = (uint8_t)(cks + rx[5u + k]); } if ((uint8_t)~cks != rx[5u + m]) { return 0; } if (rx[4] != 0u) { return 0; } /* octet d'erreur */ for (uint8_t k = 0u; k < m; k++) { resp[k] = rx[5u + k]; } return 1; } static int dxl_ping(uint8_t id) { return transaction(id, DXL_PING, NULL, 0u, NULL, 0u); } static int dxl_read(uint8_t id, uint8_t addr, uint8_t *out, uint8_t len) { uint8_t p[2] = { addr, len }; return transaction(id, DXL_READ, p, 2u, out, len); } static int dxl_write8(uint8_t id, uint8_t addr, uint8_t v) { uint8_t p[2] = { addr, v }; return transaction(id, DXL_WRITE, p, 2u, NULL, 0u); } static int dxl_write16(uint8_t id, uint8_t addr, uint16_t v) { uint8_t p[3] = { addr, (uint8_t)(v & 0xFFu), (uint8_t)(v >> 8) }; return transaction(id, DXL_WRITE, p, 3u, NULL, 0u); } static int dxl_read16(uint8_t id, uint8_t addr, uint16_t *v) { uint8_t b[2]; if (!dxl_read(id, addr, b, 2u)) { return 0; } *v = (uint16_t)(b[0] | ((uint16_t)b[1] << 8)); return 1; } static void dxl_scan(void) { g_found_count = 0u; for (uint32_t id = 1u; id <= 253u; id++) { if (dxl_ping((uint8_t)id)) { if (g_found_count < 8u) { g_found[g_found_count] = (uint8_t)id; } g_found_count++; printf("servomoteur trouve : ID %lu\r\n", (unsigned long)id); } } } /* USER CODE END 0 */ /* ===== USER CODE BEGIN 2 ==================================================== dxl_scan(); USER CODE END 2 */ /* ===== Dans while (1), USER CODE BEGIN 3 ==================================== * Une seule des variantes ci-dessous a la fois. */ /* Etape 3 : modele, tension, temperature, une fois par seconde. */ #if 0 { uint16_t model = 0u; uint8_t vt[2] = { 0u, 0u }; if (dxl_read16(DXL_ID, 0u, &model) && dxl_read(DXL_ID, 42u, vt, 2u)) { printf("modele %u tension %u.%u V temperature %u C\r\n", model, vt[0] / 10u, vt[0] % 10u, vt[1]); } else { printf("pas de reponse de l'ID %u\r\n", DXL_ID); } HAL_Delay(1000); } #endif /* Etape 4 : LED, puis position alternee 300 / 700 (mode position : les * limites d'angle doivent etre 0 et 1023, valeurs d'usine). */ #if 0 { static uint32_t k = 0u; uint16_t pos; dxl_write8(DXL_ID, 25u, (uint8_t)(k & 1u)); /* LED */ dxl_write16(DXL_ID, 30u, (k & 1u) ? 700u : 300u); /* position voulue */ for (uint32_t t = 0u; t < 10u; t++) { if (dxl_read16(DXL_ID, 36u, &pos)) { printf("position %u\r\n", pos); } HAL_Delay(200); } k++; } #endif /* Etape 4 : mode roue, un sens, l'autre, arret. */ #if 1 { static int mode_roue = 0; if (!mode_roue) { dxl_write16(DXL_ID, 6u, 0u); /* limite CW = 0 */ dxl_write16(DXL_ID, 8u, 0u); /* limite CCW = 0 : mode roue */ dxl_write8(DXL_ID, 24u, 1u); /* couple active */ mode_roue = 1; } dxl_write16(DXL_ID, 32u, 300u); HAL_Delay(2000); dxl_write16(DXL_ID, 32u, 1024u + 300u); HAL_Delay(2000); dxl_write16(DXL_ID, 32u, 0u); HAL_Delay(2000); } #endif /* USER CODE END 3 */ /* Reponses attendues aux questions : * 1. 6 octets x 10 bits / 1 Mbit/s = 60 us. * 2. ID 1 : FF FF 01 02 01 FB (01 + 02 + 01 = 04, ~04 = FB). * 3. La carte tient le fil en permanence : le servomoteur ne peut jamais * repondre. L'emission parait normale, seule la reception est morte -- * c'est la panne vecue a la mise en service de la carte. * 4. La position reelle, differente de la position voulue : le servomoteur * pousse mais ne l'atteint pas (et chauffe si on insiste). * 5. Non : sur les 60 degres non couverts par le capteur, la position lue * n'est pas valable (le firmware RatBot doit traverser ce secteur aveugle * a l'estime). */