/***** * Copyright (c) 2023 - Poivron Robotique * * SPDX-License-Identifier: BSD-3-Clause */ #include "pico/stdlib.h" #include "pico/time.h" #include "pico/multicore.h" #include "hardware/pwm.h" #include #include #define PIN_STEP 0 #define PIN_DIR 1 #define PIN_ENABLE 2 #define PIN_CC_PWM 3 #define PIN_CC_DIR 4 #define PIN_CONTACTEUR_BAS 7 #define PIN_CONTACTEUR_OUVERT 5 #define PIN_CONTACTEUR_FERMÉ 6 #define SENS_BAS 1 #define SENS_HAUT 2 #define SENS_NONE 3 bool bouton_Presser = false; float v_actuelle_tr_s=0; float v_consigne_tr_s=0; float acc_tr_s = 50; const float pas_par_tour = 200; float temps_pas= 0.001; int pos_pas; float vitesse_nominale_tr_s = 10; uint sens_pas_a_pas=SENS_BAS; struct contacteur_t{ uint gpio; int pos_valide; int pos_actuelle; uint64_t time_new_pos; }; struct contacteur_t contacteur_bas, contacteur_fermé, contacteur_ouvert; uint64_t get_us_since_boot(){ return to_us_since_boot(get_absolute_time()); } void contacteur_update(struct contacteur_t * contacteur){ // Si la position du contacteur a changé, on lance le timer bool pos_lu = gpio_get(contacteur->gpio); if(pos_lu != contacteur->pos_actuelle){ contacteur->pos_actuelle = pos_lu; if(contacteur->pos_actuelle != contacteur->pos_valide){ contacteur->time_new_pos = get_us_since_boot(); } } if(contacteur->pos_actuelle != contacteur->pos_valide){ if(get_us_since_boot() - contacteur->time_new_pos > 100){ contacteur->pos_valide = contacteur->pos_actuelle; } } } int contacteur_init(uint gpio, struct contacteur_t * contacteur){ contacteur->gpio = gpio; gpio_init(contacteur->gpio ); gpio_pull_down(contacteur->gpio ); gpio_set_dir(contacteur->gpio, GPIO_IN); bool pos_lu = gpio_get(contacteur->gpio); contacteur->pos_actuelle = pos_lu; contacteur->pos_valide = pos_lu; } /// @brief calcule le pas de temps suivant /// @param /// @return float compute_time_step(float temps_pas_s){ if(v_actuelle_tr_s != v_consigne_tr_s){ // On ajoute l'accélération v_actuelle_tr_s += acc_tr_s * temps_pas_s; //printf(">vit:%f\n", v_actuelle_tr_s); // On vérifie qu'on a pas dépassé la vitesse consigne if(v_actuelle_tr_s > v_consigne_tr_s){ v_actuelle_tr_s = v_consigne_tr_s; } } //printf(">vit_2:%f\n", v_actuelle_tr_s); float temps_tr, temps_pas; if(v_actuelle_tr_s != 0){ temps_tr = 1 / v_actuelle_tr_s; // 100 temps_pas = temps_tr / pas_par_tour; // 5 if(temps_pas > 0.001) temps_pas = 0.001; return temps_pas; } return 0.001; } void affiche_pas_de_temps(){ while(1){ printf(">temps_pas:%f\n", temps_pas * 1000); printf(">v_actuelle_tr_s:%f\n", v_actuelle_tr_s); printf(">v_consigne_tr_s:%f\n", v_consigne_tr_s); printf(">contacteur_bas:%d\n", contacteur_bas.pos_valide); printf(">contacteur_ouvret:%d\n", contacteur_ouvert.pos_valide); printf(">contacteur_ferme:%d\n", contacteur_fermé.pos_valide); printf(">pos_pas:%d\n", pos_pas); sleep_ms(20); } } void pince_ouvre(){ gpio_put(PIN_CC_DIR, 0); pwm_set_chan_level(1, PWM_CHAN_B, 60000); while(contacteur_ouvert.pos_valide){ contacteur_update(&contacteur_ouvert); } pwm_set_chan_level(1, PWM_CHAN_B, 0); } void pince_ferme(){ gpio_put(PIN_CC_DIR, 1); pwm_set_chan_level(1, PWM_CHAN_B, 60000); while(contacteur_fermé.pos_valide){ contacteur_update(&contacteur_fermé); } //pwm_set_chan_level(1, PWM_CHAN_B, 30000); //sleep_ms(200); pwm_set_chan_level(1, PWM_CHAN_B, 0); } void pas_a_pas_stop(){ gpio_put(PIN_DIR, 0); gpio_put(PIN_ENABLE, 1); gpio_set_dir(PIN_STEP, GPIO_IN); v_consigne_tr_s = 0; sens_pas_a_pas=SENS_NONE; } void pas_a_pas_descend(){ gpio_put(PIN_DIR, 1); gpio_put(PIN_ENABLE, 0); gpio_set_dir(PIN_STEP, GPIO_OUT); v_consigne_tr_s = vitesse_nominale_tr_s; sens_pas_a_pas=SENS_BAS; } void pas_a_pas_monte(){ gpio_put(PIN_DIR, 0); gpio_put(PIN_ENABLE, 0); gpio_set_dir(PIN_STEP, GPIO_OUT); v_consigne_tr_s = vitesse_nominale_tr_s; sens_pas_a_pas=SENS_HAUT; } void main(void) { stdio_init_all(); // Moteur CC gpio_set_function(PIN_CC_PWM, GPIO_FUNC_PWM); pwm_set_wrap(1, (uint16_t)65535); pwm_set_chan_level(1, PWM_CHAN_B, 0); pwm_set_enabled(1, true); gpio_init(PIN_CC_DIR); gpio_set_dir(PIN_CC_DIR, GPIO_OUT); gpio_put(PIN_CC_DIR, 1); // Contacteurs contacteur_init(PIN_CONTACTEUR_BAS, &contacteur_bas); contacteur_init(PIN_CONTACTEUR_FERMÉ, &contacteur_fermé); contacteur_init(PIN_CONTACTEUR_OUVERT, &contacteur_ouvert); // Pas à pas gpio_init(PIN_STEP); gpio_init(PIN_DIR); gpio_init(PIN_ENABLE); gpio_set_dir(PIN_STEP, GPIO_IN); gpio_set_dir(PIN_DIR, GPIO_OUT); gpio_set_dir(PIN_ENABLE, GPIO_OUT); gpio_put(PIN_ENABLE, 1); multicore_launch_core1(affiche_pas_de_temps); pince_ouvre(); //sleep_ms(3000); //printf("kartoffen\n"); const uint32_t my_delay=500; bool consigne_pas_a_pas_active = false; int consigne_pas_a_pas = 0; while(1){ contacteur_update(&contacteur_ouvert); contacteur_update(&contacteur_fermé); contacteur_update(&contacteur_bas); if(sens_pas_a_pas==SENS_BAS && contacteur_bas.pos_valide == 1){ pas_a_pas_stop(); pos_pas = 0; } if(consigne_pas_a_pas_active == true){ if (abs(pos_pas - consigne_pas_a_pas) < 10){ pas_a_pas_stop(); consigne_pas_a_pas_active = false; } } temps_pas = compute_time_step(temps_pas); sleep_us(temps_pas * 1000000); gpio_put(PIN_STEP, 1); sleep_us(temps_pas * 1000000); gpio_put(PIN_STEP, 0); if(sens_pas_a_pas == SENS_HAUT){ pos_pas+= 2; }else if(sens_pas_a_pas == SENS_BAS){ pos_pas-= 2; } int key = getchar_timeout_us(0); // get any pending key press but don't wait if (key != PICO_ERROR_TIMEOUT) { if(key == 'u' || key == 'U'){ pas_a_pas_monte(); } if(key == 'd' || key == 'D'){ pas_a_pas_descend(); } if(key == 's' || key == 'S'){ pas_a_pas_stop(); } if(key == 'o' || key == 'O'){ pince_ouvre(); } if(key == 'f' || key == 'F'){ pince_ferme(); } if(key == '1'){ consigne_pas_a_pas=5000; consigne_pas_a_pas_active=true; if(pos_pas