270 lines
6.3 KiB
C
270 lines
6.3 KiB
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 <stdlib.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 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 <consigne_pas_a_pas ){
|
|
pas_a_pas_monte();
|
|
}else{
|
|
pas_a_pas_descend();
|
|
}
|
|
}
|
|
if(key == '2'){
|
|
consigne_pas_a_pas=15000;
|
|
consigne_pas_a_pas_active=true;
|
|
if(pos_pas <consigne_pas_a_pas ){
|
|
pas_a_pas_monte();
|
|
}else{
|
|
pas_a_pas_descend();
|
|
}
|
|
}
|
|
//printf("%c %d\n", key,key);
|
|
}
|
|
|
|
|
|
}
|
|
}
|