2026-Actionneurs/main.c

/*****
 * 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 <stdio.h>
#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 0
#define SENS_HAUT 1


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;

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);
		sleep_ms(1);
	}
}

void pince_ouvre(){
	gpio_put(PIN_CC_DIR, 1);
	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, 0);
	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, 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;
}

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);

	pince_ouvre();


	// 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);

	float vitesse_nominale_tr_s = 10;
	
	//sleep_ms(3000);
	//printf("kartoffen\n");
	const uint32_t my_delay=500;
	uint sens_pas_a_pas=SENS_BAS;
	
	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();
		}

		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);
		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'){
				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_HAUT;

			}
			if(key == 'd' || key == 'D'){
				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_BAS;
			}
			if(key == 's' || key == 'S'){
				gpio_put(PIN_DIR, 0);
				gpio_put(PIN_ENABLE, 1);
				gpio_set_dir(PIN_STEP, GPIO_IN);
				v_consigne_tr_s = 0;
			}
			if(key == 'o' || key == 'O'){
				pince_ouvre();
			}
			if(key == 'f' || key == 'F'){
				pince_ferme();
			}
			//printf("%c %d\n", key,key);
		}

		
	}
}