diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0e711ea..d944d19 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,7 +14,7 @@ pico_sdk_init()
add_executable(VL53L8X_Gradin
Geometrie.c
- i2c_maitre.c
+ i2c_slave.c
main.c
Temps.c
VL53L8CX_ULD_API/src/vl53l8cx_api.c
diff --git a/i2c_maitre.c b/i2c_maitre.c
deleted file mode 100644
index c24e0f1..0000000
--- a/i2c_maitre.c
+++ /dev/null
@@ -1,274 +0,0 @@
-#include "i2c_maitre.h"
-#include "hardware/gpio.h"
-#include "hardware/i2c.h"
-#include "pico/stdlib.h"
-#include <stdio.h>
-
-#define I2C1_SDA_PIN 14
-#define I2C1_SCL_PIN 15
-
-#define I2C_NB_MAX_TAMPON 20
-
-enum i2c_statu_t{
- I2C_STATU_LIBRE,
- I2C_STATU_OCCUPE
-} i2c_statu_i2c1;
-
-uint16_t I2C_tampon_envoi[I2C_NB_MAX_TAMPON];
-uint8_t I2C_tampon_reception[I2C_NB_MAX_TAMPON];
-uint16_t I2C_nb_a_envoyer, I2C_nb_a_recevoir;
-uint8_t adresse_7_bits;
-uint32_t i2c_error_code; // value of i2c->hw->tx_abrt_source if anything wrong happen, 0 if everything was fine.
-
-enum transaction_statu_t{
- TRANSACTION_EN_COURS,
- TRANSACTION_TERMINEE
-} statu_emission, statu_reception;
-
-void i2d_set_adresse_esclave(uint8_t _adresse_7bits);
-void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception);
-enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception);
-
-void i2c_maitre_init(void){
- //stdio_init_all();
- i2c_init(i2c1, 400 * 1000);
-
- printf("Initialisation des broches\n");
- printf("%d et %d en I2C\n", I2C1_SDA_PIN, I2C1_SCL_PIN);
- gpio_set_function(I2C1_SDA_PIN, GPIO_FUNC_I2C);
- gpio_set_function(I2C1_SCL_PIN, GPIO_FUNC_I2C);
- gpio_pull_up(I2C1_SDA_PIN);
- gpio_pull_up(I2C1_SCL_PIN);
-
- i2c_statu_i2c1 = I2C_STATU_LIBRE;
-}
-
-/// @brief Fonction à appeler régulièrement ou en interruption.
-/// @param i2c
-void i2c_gestion(i2c_inst_t *i2c){
- // on veut gérer l'i2c avec cette fonction.
- // 2 cas :
- // - Soit écriture simple (plusieurs octets (W))
- // - Soit écriture + lecture (Adresse (W), registre (W), données (R))
- // Pour écrire 1 octet, i2c->hw->data_cmd = xxx, (avec CMD:8 à 0, )
- // Pour lire 1 octet, i2c->hw->data_cmd = xxx (avec CMD:8 à 1)
- // Il faut mettre CMD:9 à 1 pour le dernier octet.
-
- // Envoi des données (ou des demandes de lecture)
- static uint16_t index_envoi=0, index_reception=0;
-
- // Acquitement des erreurs, pas 100% fonctionnel ! TODO !
- if(i2c->hw->tx_abrt_source !=0){
- // Seule solution trouvée pour réinitialiser l'I2C.
- char a;
- i2c_read_blocking(i2c, adresse_7_bits, &a, 1, false);
-
- I2C_nb_a_envoyer = 0;
- index_reception = 0;
- I2C_nb_a_recevoir = 0;
- statu_emission = TRANSACTION_TERMINEE;
- statu_reception = TRANSACTION_TERMINEE;
- i2c_statu_i2c1 = I2C_STATU_LIBRE;
- }
-
- while( (index_envoi < I2C_nb_a_envoyer) && (i2c_get_write_available(i2c)) ){
- bool restart = false;
- bool last = false;
-
- if (index_envoi == 0){
- // Début de l'envoi, assurons nous d'avoir la bonne adresse de l'esclave
- i2c->hw->enable = 0;
- i2c->hw->tar = adresse_7_bits;
- i2c->hw->enable = 1;
- }else{
- // Passage de l'écriture à la lecture, on envoie un bit de restart.
- if( !(I2C_tampon_envoi[index_envoi-1] & I2C_IC_DATA_CMD_CMD_BITS) &&
- (I2C_tampon_envoi[index_envoi] & I2C_IC_DATA_CMD_CMD_BITS)){
- restart = true;
- }
- }
-
- if(index_envoi + 1 == I2C_nb_a_envoyer){
- // Fin de la trame, nous devons envoyer un bit de stop.
- last = true;
- }
-
- i2c->hw->data_cmd =
- I2C_tampon_envoi[index_envoi] |
- bool_to_bit(restart) << I2C_IC_DATA_CMD_RESTART_LSB |
- bool_to_bit(last) << I2C_IC_DATA_CMD_STOP_LSB;
-
- if(last){
- statu_emission = TRANSACTION_TERMINEE;
- index_envoi = 0;
- I2C_nb_a_envoyer = 0;
- //printf("I2C emission terminee\n");
- }else{
- index_envoi++;
- }
-
- }
-
- // Réception des données - Lecture des données présentes dans le tampon
- while( (index_reception < I2C_nb_a_recevoir) && (i2c_get_read_available(i2c)) ){
- I2C_tampon_reception[index_reception] = (uint8_t) i2c->hw->data_cmd;
- index_reception++;
- }
- if(index_reception == I2C_nb_a_recevoir){
- statu_reception = TRANSACTION_TERMINEE;
- index_reception = 0;
- I2C_nb_a_recevoir = 0;
- }
-
- if(statu_reception == TRANSACTION_TERMINEE && statu_emission == TRANSACTION_TERMINEE){
- i2c_statu_i2c1 = I2C_STATU_LIBRE;
- }
-
-}
-
-/// @brief Charge le tampon d'émission pour pré-mâcher le travail à la fonction i2c_gestion
-/// @param emission
-/// @param nb_envoi
-/// @param nb_reception
-void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception){
- // Données à envoyer
- for(unsigned int index=0; index<nb_envoi; index++){
- I2C_tampon_envoi[index] = (uint16_t) emission[index];
- }
- // Données à lire
- for(unsigned int index=0; index<nb_reception; index++){
- I2C_tampon_envoi[nb_envoi + index] = (uint16_t) 0x0100;
- }
-}
-
-/// @brief Stock l'adresse de l'esclave avec lequel communiquer
-/// @param _adresse_7bits
-void i2d_set_adresse_esclave(uint8_t _adresse_7bits){
- adresse_7_bits =_adresse_7bits;
-}
-
-/// @brief Initialise la transmission I2, sur l'i2c1. Une transmission se compose de 2 trames I2C, une pour écrire (Adresse + données), une pour lire
-/// Si nb_reception = 0, alors la trame pour lire ne sera pas envoyée.
-/// @param emission : données à envoyer
-/// @param nb_envoi : nombre de données à envoyer
-/// @param nb_reception : nombre de données à recevoir
-/// @return I2C_EN_COURS, I2C_SUCCES ou I2C_ECHEC
-enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception){
- static enum m_statu_t{
- I2C_STATU_INIT,
- I2C_STATU_EN_COURS,
- }m_statu = I2C_STATU_INIT;
-
- switch(m_statu){
- case I2C_STATU_INIT:
- // I2C libre ?
- if(i2c_statu_i2c1 == I2C_STATU_OCCUPE){
- return I2C_EN_COURS;
- }
- // Alors il est à nous !
- i2c_statu_i2c1 = I2C_STATU_OCCUPE;
- statu_emission = TRANSACTION_EN_COURS;
- statu_reception = TRANSACTION_EN_COURS;
- i2c_error_code = 0;
-
- i2d_set_adresse_esclave(_adresse_7bits);
-
- i2c_charger_tampon_envoi(emission, nb_envoi, nb_reception);
- // Nous devons envoyer aussi une commande pour chaque octet à recevoir.
- I2C_nb_a_envoyer = nb_envoi + nb_reception;
- I2C_nb_a_recevoir = nb_reception;
-
- // On appelle la fonction gestion pour gagner du temps.
- i2c_gestion(i2c1);
- m_statu = I2C_STATU_EN_COURS;
- break;
-
- case I2C_STATU_EN_COURS:
- if(i2c_statu_i2c1 == I2C_STATU_LIBRE){
- m_statu = I2C_STATU_INIT;
- if(i2c_error_code){
- return I2C_ECHEC;
- }else{
- return I2C_SUCCES;
- }
-
- }
- break;
- }
- return I2C_EN_COURS;
-}
-
-/// @brief Lit le registre d'un composant se comportant comme une EPROM I2C.
-/// @return I2C_SUCCES, I2C_EN_COURS ou I2C_ECHEC
-enum i2c_resultat_t i2c_lire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * reception, uint8_t len){
- uint8_t emission[1];
- emission[0] = registre;
- enum i2c_resultat_t i2c_resultat;
- i2c_resultat = i2c_transmission(adresse_7_bits, emission, 1, len);
- if(i2c_resultat == I2C_SUCCES){
- for(uint32_t i = 0; i < len; i++){
- reception[i] = I2C_tampon_reception[i];
- }
- return I2C_SUCCES;
- }else if(i2c_resultat == I2C_ECHEC){
- return I2C_ECHEC;
- }
- return I2C_EN_COURS;
-}
-
-/// @brief Initialise une transaction I2C.
-/// Renvoie I2C_SUCCES si l'intégralité du message est chargé en envoi,
-/// Renvoie I2C_EN_COURS si la fonction doit encore être appelée pour finir d'envoyer le message
-/// Renvoie I2C_ECHEC en cas d'erreur I2C.
-enum i2c_resultat_t i2c_ecrire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * _emission, uint8_t len){
- uint8_t emission[I2C_NB_MAX_TAMPON];
- emission[0] = registre;
- for(uint32_t i = 0; i < len; i++){
- emission[i+1] = _emission[i];
- }
- enum i2c_resultat_t i2c_resultat;
- return i2c_transmission(adresse_7_bits, emission, 1 + len, 0);
-
-}
-
-
-/// @brief Pour l'instant bloquant, mais devrait passer en non bloquant bientôt => Non, voir i2c_lire_registre_nb
-/// @param adresse_7_bits
-/// @param
-/// @return 0: en cours,
-int i2c_lire_registre(char adresse_7_bits, char registre, char * reception, char len){
- int statu;
- char emission[1];
-
- emission[0] = registre;
- statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 1, 0);
- if(statu == PICO_ERROR_GENERIC){
- printf("I2C - Envoi registre Echec - adresse %x\n", adresse_7_bits);
- return I2C_ECHEC;
- }
-
- statu = i2c_read_blocking (i2c1, adresse_7_bits, reception, len, 0);
- if(statu == PICO_ERROR_GENERIC){
- printf("I2C - lecture registre Echec - adresse %x\n", adresse_7_bits);
- return I2C_ECHEC;
- }
-
- return I2C_SUCCES;
-}
-
-int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre){
- int statu;
- char emission[2];
-
- emission[0] = registre;
- emission[1] = valeur_registre;
- statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 2, 0);
- if(statu == PICO_ERROR_GENERIC){
- printf("Erreur ecrire registre\n");
- return I2C_ECHEC;
- }
-
- printf("i2c Registre %x, valeur %x\n", registre, valeur_registre);
-
- return I2C_SUCCES;
-}
\ No newline at end of file
diff --git a/i2c_maitre.h b/i2c_maitre.h
deleted file mode 100644
index 970b207..0000000
--- a/i2c_maitre.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#include "pico/stdlib.h"
-#include "hardware/i2c.h"
-
-enum i2c_resultat_t {
- I2C_EN_COURS,
- I2C_SUCCES,
- I2C_ECHEC
-};
-
-void i2c_maitre_init(void);
-void i2c_gestion(i2c_inst_t *i2c);
-enum i2c_resultat_t i2c_lire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * reception, uint8_t len);
-enum i2c_resultat_t i2c_ecrire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * _emission, uint8_t len);
-int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre);
-int i2c_lire_registre(char adresse_7_bits, char registre, char * reception, char len);
diff --git a/i2c_slave.c b/i2c_slave.c
new file mode 100644
index 0000000..1eb21dc
--- /dev/null
+++ b/i2c_slave.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#include "i2c_slave.h"
+#include "hardware/irq.h"
+
+typedef struct i2c_slave_t
+{
+ i2c_inst_t *i2c;
+ i2c_slave_handler_t handler;
+ bool transfer_in_progress;
+} i2c_slave_t;
+
+static i2c_slave_t i2c_slaves[2];
+
+static inline void finish_transfer(i2c_slave_t *slave) {
+ if (slave->transfer_in_progress) {
+ slave->handler(slave->i2c, I2C_SLAVE_FINISH);
+ slave->transfer_in_progress = false;
+ }
+}
+
+static void __not_in_flash_func(i2c_slave_irq_handler)(i2c_slave_t *slave) {
+ i2c_inst_t *i2c = slave->i2c;
+ i2c_hw_t *hw = i2c_get_hw(i2c);
+
+ uint32_t intr_stat = hw->intr_stat;
+ if (intr_stat == 0) {
+ return;
+ }
+ if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) {
+ hw->clr_tx_abrt;
+ finish_transfer(slave);
+ }
+ if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) {
+ hw->clr_start_det;
+ finish_transfer(slave);
+ }
+ if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) {
+ hw->clr_stop_det;
+ finish_transfer(slave);
+ }
+ if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) {
+ slave->transfer_in_progress = true;
+ slave->handler(i2c, I2C_SLAVE_RECEIVE);
+ }
+ if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) {
+ hw->clr_rd_req;
+ slave->transfer_in_progress = true;
+ slave->handler(i2c, I2C_SLAVE_REQUEST);
+ }
+}
+
+static void __not_in_flash_func(i2c0_slave_irq_handler)() {
+ i2c_slave_irq_handler(&i2c_slaves[0]);
+}
+
+static void __not_in_flash_func(i2c1_slave_irq_handler)() {
+ i2c_slave_irq_handler(&i2c_slaves[1]);
+}
+
+void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler) {
+ assert(i2c == i2c0 || i2c == i2c1);
+ assert(handler != NULL);
+
+ uint i2c_index = i2c_hw_index(i2c);
+ i2c_slave_t *slave = &i2c_slaves[i2c_index];
+ slave->i2c = i2c;
+ slave->handler = handler;
+
+ // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
+ // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
+ // disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow).
+ i2c_set_slave_mode(i2c, true, address);
+
+ i2c_hw_t *hw = i2c_get_hw(i2c);
+ // unmask necessary interrupts
+ hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS | I2C_IC_INTR_MASK_M_RD_REQ_BITS | I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS | I2C_IC_INTR_MASK_M_STOP_DET_BITS | I2C_IC_INTR_MASK_M_START_DET_BITS;
+
+ // enable interrupt for current core
+ uint num = I2C0_IRQ + i2c_index;
+ irq_set_exclusive_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
+ irq_set_enabled(num, true);
+}
+
+void i2c_slave_deinit(i2c_inst_t *i2c) {
+ assert(i2c == i2c0 || i2c == i2c1);
+
+ uint i2c_index = i2c_hw_index(i2c);
+ i2c_slave_t *slave = &i2c_slaves[i2c_index];
+ assert(slave->i2c == i2c); // should be called after i2c_slave_init()
+
+ slave->i2c = NULL;
+ slave->handler = NULL;
+ slave->transfer_in_progress = false;
+
+ uint num = I2C0_IRQ + i2c_index;
+ irq_set_enabled(num, false);
+ irq_remove_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
+
+ i2c_hw_t *hw = i2c_get_hw(i2c);
+ hw->intr_mask = I2C_IC_INTR_MASK_RESET;
+
+ i2c_set_slave_mode(i2c, false, 0);
+}
diff --git a/i2c_slave.h b/i2c_slave.h
new file mode 100644
index 0000000..4505e66
--- /dev/null
+++ b/i2c_slave.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#ifndef _I2C_SLAVE_H_
+#define _I2C_SLAVE_H_
+
+#include <hardware/i2c.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file i2c_slave.h
+ *
+ * \brief I2C slave setup.
+ */
+
+/**
+ * \brief I2C slave event types.
+ */
+typedef enum i2c_slave_event_t
+{
+ I2C_SLAVE_RECEIVE, /**< Data from master is available for reading. Slave must read from Rx FIFO. */
+ I2C_SLAVE_REQUEST, /**< Master is requesting data. Slave must write into Tx FIFO. */
+ I2C_SLAVE_FINISH, /**< Master has sent a Stop or Restart signal. Slave may prepare for the next transfer. */
+} i2c_slave_event_t;
+
+/**
+ * \brief I2C slave event handler
+ *
+ * The event handler will run from the I2C ISR, so it should return quickly (under 25 us at 400 kb/s).
+ * Avoid blocking inside the handler and split large data transfers across multiple calls for best results.
+ * When sending data to master, up to `i2c_get_write_available()` bytes can be written without blocking.
+ * When receiving data from master, up to `i2c_get_read_available()` bytes can be read without blocking.
+ *
+ * \param i2c Slave I2C instance.
+ * \param event Event type.
+ */
+typedef void (*i2c_slave_handler_t)(i2c_inst_t *i2c, i2c_slave_event_t event);
+
+/**
+ * \brief Configure I2C instance for slave mode.
+ *
+ * \param i2c I2C instance.
+ * \param address 7-bit slave address.
+ * \param handler Called on events from I2C master. It will run from the I2C ISR, on the CPU core
+ * where the slave was initialized.
+ */
+void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler);
+
+/**
+ * \brief Restore I2C instance to master mode.
+ *
+ * \param i2c I2C instance.
+ */
+void i2c_slave_deinit(i2c_inst_t *i2c);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _I2C_SLAVE_H_
diff --git a/main.c b/main.c
index efddd1e..c4c136a 100644
--- a/main.c
+++ b/main.c
@@ -7,7 +7,8 @@
#include "pico/multicore.h"
#include "hardware/adc.h"
#include "hardware/spi.h"
-#include "i2c_maitre.h"
+#include "hardware/i2c.h"
+#include "i2c_slave.h"
#include "Temps.h"
#include "VL53L8_2024.h"
#include "vl53l8cx_api.h"
@@ -33,7 +34,12 @@
#define D1 27
#define D2 28
#define D3 29
+#define SDA 6
+#define SCL 7
+#define I2C_SLAVE_SDA_PIN SDA
+#define I2C_SLAVE_SCL_PIN SCL
+#define I2C_SLAVE_ADDRESS 0x19
void affichage(void);
void gestion_affichage(void);
@@ -58,6 +64,65 @@ VL53L8CX_Configuration gauche, droit;
uint8_t capteur_init(VL53L8CX_Configuration * capteur);
+
+// The slave implements a 256 byte memory. To write a series of bytes, the master first
+// writes the memory address, followed by the data. The address is automatically incremented
+// for each byte transferred, looping back to 0 upon reaching the end. Reading is done
+// sequentially from the current memory address.
+static struct
+{
+ uint8_t mem[256];
+ uint8_t mem_address;
+ bool mem_address_written;
+} context;
+
+// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
+// printing to stdio may interfere with interrupt handling.
+static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
+ switch (event) {
+ case I2C_SLAVE_RECEIVE: // master has written some data
+ if (!context.mem_address_written) {
+ // writes always start with the memory address
+ context.mem_address = i2c_read_byte_raw(i2c);
+ context.mem_address_written = true;
+ } else {
+ // save into memory
+ context.mem[context.mem_address] = i2c_read_byte_raw(i2c);
+ context.mem_address++;
+ }
+ break;
+ case I2C_SLAVE_REQUEST: // master is requesting data
+ // load from memory
+ i2c_write_byte_raw(i2c, context.mem[context.mem_address]);
+ context.mem_address++;
+ break;
+ case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
+ context.mem_address_written = false;
+ break;
+ default:
+ break;
+ }
+}
+
+void i2c_envoi_32bits(int32_t value, char adresse){
+ context.mem[adresse] = value >> 24;
+ context.mem[adresse+1] = (value >> 16) & 0xFF;
+ context.mem[adresse+2] = (value >> 8) & 0xFF;
+ context.mem[adresse+3] = value & 0xFF;
+}
+
+static void i2c_setup_slave() {
+ gpio_init(I2C_SLAVE_SDA_PIN);
+ gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
+ gpio_pull_up(I2C_SLAVE_SDA_PIN);
+
+ gpio_init(I2C_SLAVE_SCL_PIN);
+ gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
+ gpio_pull_up(I2C_SLAVE_SCL_PIN);
+
+ i2c_slave_init(i2c1, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
+}
+
void main(void)
{
int ledpower = 500;
@@ -123,6 +188,8 @@ void main(void)
tampon[0] = 0x55;
tampon[1] = 0x55;
+ i2c_setup_slave();
+
sleep_ms(5000);
printf("Demarrage...\n");
@@ -236,11 +303,13 @@ void gestion_VL53L8CX(void){
if(echec == 2){
// Aucun capteur valide
ws2812_set(0x0F,0,0);
+ context.mem[0] = 0;
}else if(echec == 1){
// Un seul capteur valide
if(echec_gauche == 0 && ! isnan(gauche_planche_angle)){
// capteur gauche permet de déterminer la position de la planche
ws2812_set(0x0F,0x8,0);
+ context.mem[0] = 1;
planche_centre_x = gauche_planche_pos_x + 200 * cos(gauche_planche_angle);
planche_centre_y = gauche_planche_pos_y + 200 * sin(gauche_planche_angle);
planche_angle_rad = gauche_planche_angle;
@@ -248,6 +317,7 @@ void gestion_VL53L8CX(void){
}else if(echec_droit == 0 && ! isnan(droit_planche_angle)){
// capteur droit permet de déterminer la position de la planche
ws2812_set(0x0F,0x8,0);
+ context.mem[0] = 1;
planche_centre_x = droit_planche_pos_x - 200 * cos(droit_planche_angle);
planche_centre_y = droit_planche_pos_y - 200 * sin(droit_planche_angle);
planche_angle_rad = droit_planche_angle;
@@ -256,6 +326,7 @@ void gestion_VL53L8CX(void){
// On a un bout de la planche mais pas d'angle, c'est un echec
echec = 2;
ws2812_set(0x0F,0,0);
+ context.mem[0] = 0;
}
}else{
@@ -264,6 +335,10 @@ void gestion_VL53L8CX(void){
planche_centre_x = (droit_planche_pos_x + gauche_planche_pos_x)/2;
planche_centre_y = (droit_planche_pos_y + gauche_planche_pos_y)/2;
planche_angle_rad = atan2f(droit_planche_pos_y - gauche_planche_pos_y, droit_planche_pos_x - gauche_planche_pos_x);
+ i2c_envoi_32bits(planche_centre_x, 1);
+ i2c_envoi_32bits(planche_centre_y, 5);
+ i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
+ context.mem[0] = 2;
}