Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
Diff: hardware.cpp
- Revision:
- 3:d38aa400d5e7
- Parent:
- 2:3066e614372f
- Child:
- 4:eac6746544fb
diff -r 3066e614372f -r d38aa400d5e7 hardware.cpp --- a/hardware.cpp Wed May 08 21:19:10 2019 +0000 +++ b/hardware.cpp Thu May 09 07:09:54 2019 +0000 @@ -4,7 +4,6 @@ #include "hardware.h" #include "DevSPI.h" #include "XNucleoIHM02A1.h" -#include "commande_moteurs.h" // PWM_MAX est définit dans réglage; bool moteurs_arret = false; @@ -82,11 +81,7 @@ InterruptIn ENCAJ(D8); InterruptIn ENCBL(D6); InterruptIn ENCBJ(D5); -/*//Nucelo 746zg -InterruptIn ENCAL(D8); -InterruptIn ENCAJ(D7); -InterruptIn ENCBL(D4); -InterruptIn ENCBJ(D0);*/ + volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B @@ -157,46 +152,6 @@ x_nucleo_ihm02a1->perform_prepared_actions(); } -void set_step_moteur_D(int steps) -{ - if (!moteurs_arret) { - if (1) { - motors[0]->prepare_move(StepperMotor::BWD, steps); //BWD = backward , FWD = forward , la vitesse doit etre positive - /*} else if (PWM <-PWM_MAX) { - motors[0]->prepare_run(StepperMotor::FWD, PWM_MAX); - } else if (PWM > 0) { - motors[0]->prepare_run(StepperMotor::BWD, PWM); - } else if (PWM < 0) { - motors[0]->prepare_run(StepperMotor::FWD, -PWM); - } else if (PWM == 0) { - motors[0]->prepare_run(StepperMotor::BWD, 0); - */} - /*} else { - motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main*/ - } - x_nucleo_ihm02a1->perform_prepared_actions(); -} -/* -void set_step_moteur_G(int PWM) -{ - - if (!moteurs_arret) { - if (PWM > PWM_MAX) { - motors[1]->prepare_run(StepperMotor::FWD, PWM_MAX); - } else if (PWM <-PWM_MAX) { - motors[1]->prepare_run(StepperMotor::BWD, PWM_MAX); - } else if (PWM > 0) { - motors[1]->prepare_run(StepperMotor::FWD, PWM); - } else if (PWM < 0) { - motors[1]->prepare_run(StepperMotor::BWD, -PWM); - } else if (PWM == 0) { - motors[1]->prepare_run(StepperMotor::BWD, 0); - } - } else { - motors[1]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main - } - x_nucleo_ihm02a1->perform_prepared_actions(); -}*/ long int get_nbr_tick_D() { @@ -298,53 +253,6 @@ - -/*void asservissement(){ - long int tick_D = get_nbr_tick_D(); - long int tick_G = get_nbr_tick_G(); - - long int tick_D_passe = tick_D-tick_prec_D; - long int tick_G_passe = tick_G-tick_prec_G; - - tick_prec_D=tick_D; - tick_prec_G=tick_G; - - float vitesse_codeuse_D = tick_D_passe; - float vitesse_codeuse_G = tick_G_passe; - - float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D; - float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G; - - somme_erreur_D += erreur_D; - somme_erreur_G += erreur_G; - - float delta_erreur_D = erreur_D-erreur_precedente_D; - float delta_erreur_G = erreur_G-erreur_precedente_G; - - erreur_precedente_G = erreur_G; - erreur_precedente_D = erreur_D; - - float cmd_D = Kp*erreur_D+Ki*somme_erreur_D + Kd*delta_erreur_D; - float cmd_G = Kp*erreur_G+Ki*somme_erreur_G + Kd*delta_erreur_G; - - if (cmd_G <0){ - cmd_G = 0; - } - if (cmd_G > 500){ - cmd_G = 500; - } - if (cmd_D <0){ - cmd_D = 0; - } - if (cmd_D > 500){ - cmd_D = 500; - } - commande_vitesse(cmd_G,cmd_D); - //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D); - //printf("%f\n",vitesse_codeuse_G); - //printf("oui"); -}*/ - void debugEncoder() { printf("tick_D : %ld, tick_G : %ld\n", get_nbr_tick_D(),get_nbr_tick_G()); @@ -366,3 +274,8 @@ return position; } + +void bouton(){ + DigitalIn depart(USER_BUTTON); + while (depart){} +}