Résolution des probèmes de fausse détection de butées avec le translateur -> pb de PWM
This commit is contained in:
parent
a650bacca9
commit
b956e3a563
@ -36,43 +36,10 @@ struct position_t{
|
||||
|
||||
|
||||
|
||||
enum translateur_action_t{
|
||||
TRANSLATEUR_INIT,
|
||||
TRANSLATEUR_AVANCE,
|
||||
TRANSLATEUR_RECULE,
|
||||
}translateur_action = TRANSLATEUR_INIT;
|
||||
|
||||
enum etat_action_t Actionneur_empile();
|
||||
|
||||
|
||||
/// @brief pilote la vitesse des moteurs
|
||||
/// @param vitesse vitesse signée, sera saturée à 255
|
||||
void Moteur_set(int vitesse){
|
||||
ledcWrite(3, 0);
|
||||
ledcWrite(4, 0);
|
||||
if(vitesse < 0){
|
||||
vitesse = -vitesse;
|
||||
if(vitesse > 255){
|
||||
vitesse = 255;
|
||||
}
|
||||
ledcWrite(3, vitesse);
|
||||
}else{
|
||||
if(vitesse > 255){
|
||||
vitesse = 255;
|
||||
}
|
||||
ledcWrite(4, vitesse);
|
||||
}
|
||||
}
|
||||
|
||||
void Translateur_avance(){
|
||||
Moteur_set(-255);
|
||||
translateur_action = TRANSLATEUR_AVANCE;
|
||||
}
|
||||
|
||||
void Translateur_recule(){
|
||||
Moteur_set(255);
|
||||
translateur_action = TRANSLATEUR_RECULE;
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
@ -179,44 +146,6 @@ void setup()
|
||||
Ascenseur_init();
|
||||
}
|
||||
|
||||
|
||||
void Translateur_init(void){
|
||||
// Moteur
|
||||
ledcAttach(3, 500, 8);
|
||||
ledcWrite(3, 0);
|
||||
ledcAttach(4, 500, 8);
|
||||
ledcWrite(4, 0);
|
||||
// Etat des contacteurs de fin de course
|
||||
// relecture de 3 sur 2 (Avant)
|
||||
// relecture de 4 sur 10 (Arrière)
|
||||
pinMode(2, INPUT);
|
||||
pinMode(10, INPUT);
|
||||
}
|
||||
|
||||
enum etat_action_t Translateur_etat(void){
|
||||
if(translateur_action == TRANSLATEUR_AVANCE){
|
||||
if(digitalRead(2) == 0){
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}
|
||||
if(translateur_action == TRANSLATEUR_RECULE){
|
||||
if(digitalRead(10) == 0){
|
||||
return ACTION_TERMINEE;
|
||||
}
|
||||
}
|
||||
return ACTION_EN_COURS;
|
||||
}
|
||||
|
||||
void Translateur_cycle(void){
|
||||
if(Translateur_etat() == ACTION_TERMINEE){
|
||||
if(translateur_action == TRANSLATEUR_RECULE){
|
||||
Translateur_avance();
|
||||
}else{
|
||||
Translateur_recule();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void reglage_servo(int pin_servo){
|
||||
static int valeur=307; // Neutre
|
||||
printf("reglage_servo %d\n", pin_servo);
|
||||
@ -324,6 +253,7 @@ void loop()
|
||||
while(Actionneur_empile() == ACTION_EN_COURS);
|
||||
}
|
||||
if(inByte == 'c'){
|
||||
Serial.println("Cycle translateur");
|
||||
while(Serial.available() > 0){
|
||||
inByte = Serial.read();
|
||||
}
|
||||
@ -332,8 +262,11 @@ void loop()
|
||||
if(Serial.available() > 0){
|
||||
break;
|
||||
}
|
||||
printf(">GPIO2:%d\n",digitalRead(2));
|
||||
printf(">GPIO10:%d\n",digitalRead(10));
|
||||
Serial.printf(">GPIO2:%d\n",digitalRead(2));
|
||||
Serial.printf(">GPIO10:%d\n",digitalRead(10));
|
||||
Serial.printf(">OUT3:%d\n",digitalRead(3));
|
||||
Serial.printf(">OUT4:%d\n",digitalRead(4));
|
||||
delay(25);
|
||||
}
|
||||
}
|
||||
|
||||
@ -346,6 +279,8 @@ void loop()
|
||||
Serial.printf(">Servo4:%d\n", sms_sts.ReadPos(4));
|
||||
Serial.printf(">GPIO2:%d\n",digitalRead(2));
|
||||
Serial.printf(">GPIO10:%d\n",digitalRead(10));
|
||||
Serial.printf(">OUT3:%d\n",digitalRead(3));
|
||||
Serial.printf(">OUT4:%d\n",digitalRead(4));
|
||||
|
||||
Ascenseur_gestion();
|
||||
/*
|
||||
|
Loading…
Reference in New Issue
Block a user