Guillaume Chauvon
l
Diff: hardware.cpp
- Revision:
- 2:3066e614372f
- Parent:
- 1:0d76bc4e1aea
- Child:
- 3:d38aa400d5e7
diff -r 0d76bc4e1aea -r 3066e614372f hardware.cpp
--- a/hardware.cpp Wed Apr 17 15:49:42 2019 +0000
+++ b/hardware.cpp Wed May 08 21:19:10 2019 +0000
@@ -4,6 +4,7 @@
#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;
@@ -11,14 +12,14 @@
XNucleoIHM02A1 *x_nucleo_ihm02a1; //Création d'une entité pour la carte de contôle des pas à pas
L6470_init_t init[L6470DAISYCHAINSIZE] = {
- /* First Motor. */
+/* First Motor. */
{
4.08, /* Motor supply voltage in V. */
200, /* Min number of steps per revolution for the motor. */
7.5, /* Max motor phase voltage in A. */
- 7.06, /* Max motor phase voltage in V. */
+ 7, /* Max motor phase voltage in V. */
0, /* Motor initial speed [step/s]. */
- 478, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 500, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
1500.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
@@ -32,21 +33,21 @@
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
- 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
- 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 4.9*1000*1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
0xFF, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
},
-
+
/* Second Motor. */
{
4.08, /* Motor supply voltage in V. */
200, /* Min number of steps per revolution for the motor. */
7.5, /* Max motor phase voltage in A. */
- 7.06, /* Max motor phase voltage in V. */
+ 7, /* Max motor phase voltage in V. */
0, /* Motor initial speed [step/s]. */
- 500, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 490, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
1500.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
@@ -60,8 +61,8 @@
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
- 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
- 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 4.9*1000*1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
0xFF, /* Alarm conditions enable. */
0x2E88 /* Ic configuration. */
@@ -75,41 +76,38 @@
DevSPI dev_spi(D11, D12, D3);
-
-InterruptIn ENCBL(D5);
-InterruptIn ENCBJ(D6);
-
+//Connections codeuses
+//Nucleo 401re
+InterruptIn ENCAL(D9);
+InterruptIn ENCAJ(D8);
+InterruptIn ENCBL(D6);
+InterruptIn ENCBJ(D5);
+/*//Nucelo 746zg
+InterruptIn ENCAL(D8);
+InterruptIn ENCAJ(D7);
+InterruptIn ENCBL(D4);
+InterruptIn ENCBJ(D0);*/
-/*void init_hardware()
-{
- pc.baud(115200); //Initialisation de l'USART pc
-
- // Initializing Motor Control Expansion Board.
- x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
- motors = x_nucleo_ihm02a1->get_components();
-
-
-}*/
-
+volatile long encoderValueA = 0; //nombre de tics sur l'encodeur A
volatile long encoderValueB = 0; //nombre de tics sur l'encodeur B
void init_hardware()
{
- pc.baud(115200); //Initialisation de l'USART pc
+ pc.baud(2000000); //Initialisation de l'USART pc
/* Initializing Motor Control Expansion Board. */
x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
motors = x_nucleo_ihm02a1->get_components();
- /*ENCAL.mode(PullUp); //Initialisation des codeuses
- ENCAJ.mode(PullUp);*/
+ ENCAL.mode(PullUp); //Initialisation des codeuses
+ ENCAJ.mode(PullUp);
ENCBL.mode(PullUp);
ENCBJ.mode(PullUp);
- //ENCAL.rise(&updateEncoderA);
- //ENCAL.fall(&updateEncoderA);
- //ENCAJ.rise(&updateEncoderA);
- //ENCAJ.fall(&updateEncoderA);
+ ENCAL.rise(&updateEncoderA);
+ ENCAL.fall(&updateEncoderA);
+ ENCAJ.rise(&updateEncoderA);
+ ENCAJ.fall(&updateEncoderA);
ENCBL.rise(&updateEncoderB);
ENCBL.fall(&updateEncoderB);
@@ -159,79 +157,57 @@
x_nucleo_ihm02a1->perform_prepared_actions();
}
-void set_step_moteur_G(int steps)
+void set_step_moteur_D(int steps)
{
if (!moteurs_arret) {
- if (steps<0) {
- motors[1]->prepare_move(StepperMotor::BWD, steps); //BWD = backward , BWD = backward , la vitesse doit etre positive
- }
- else if(steps >0){
- motors[1]->prepare_move(StepperMotor::FWD, steps); //BWD = backward , FWD = forward , la vitesse doit etre positive
- }
- else {
- motors[1]->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_D(int steps)
-{
- int steps_modulo_128 = steps/128;
- if (!moteurs_arret) {
- if (steps<0) {
- motors[0]->prepare_move(StepperMotor::BWD, steps_modulo_128*128); //BWD = backward , BWD = backward , la vitesse doit etre positive
- }
- else if(steps >0){
- motors[0]->prepare_move(StepperMotor::FWD, steps_modulo_128*128); //BWD = backward , FWD = forward , la vitesse doit etre positive
- }
- else {
- motors[0]->prepare_hard_hiz(); //mode haute impédence pour pouvoir déplacer le robot à la main*/
- }
+ 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();
}
-long int get_position_G(){
- /* Getting the current position. */
- long int position = motors[1]->get_position();
- return position;
- /* Printing to the console. */
- //printf("--> Getting the current position: %d\r\n", position);
-
+/*
+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()
+{
+ return encoderValueA;
}
-long int get_position_D(){
- /* Getting the current position. */
- long int position = motors[0]->get_position();
-
- /* Printing to the console. */
- //printf("--> Getting the current position: %d\r\n", position);
- return position;
-
-}
+
long int get_nbr_tick_G()
{
return encoderValueB;
}
-volatile int lastEncodedB = 0;
-long lastencoderValueB = 0;
-int lastMSBB = 0;
-int lastLSBB = 0;
-
-void updateEncoderB()
-{
- int MSBB = ENCBL.read(); //MSB = most significant bit
- int LSBB = ENCBJ.read(); //LSB = least significant bit
-
- int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
- int sumB = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value
-
- if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
- if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;
-
- lastEncodedB = encodedB; //store this value for next time
-}
-
void attente_synchro()
{
//structute du temps d'attente de l'asservissement 10ms
@@ -281,3 +257,112 @@
}
+volatile int lastEncodedA = 0;
+long lastencoderValueA = 0;
+int lastMSBA = 0;
+int lastLSBA = 0;
+
+void updateEncoderA()
+{
+ int MSBA = ENCAL.read(); //MSB = most significant bit
+ int LSBA = ENCAJ.read(); //LSB = least significant bit
+
+ int encodedA = (MSBA << 1) |LSBA; //converting the 2 pin value to single number
+ int sumA = (lastEncodedA << 2) | encodedA; //adding it to the previous encoded value
+
+ if(sumA == 0b1101 || sumA == 0b0100 || sumA == 0b0010 || sumA == 0b1011) encoderValueA ++;
+ if(sumA == 0b1110 || sumA == 0b0111 || sumA == 0b0001 || sumA == 0b1000) encoderValueA --;
+
+ lastEncodedA = encodedA; //store this value for next time
+}
+
+
+volatile int lastEncodedB = 0;
+long lastencoderValueB = 0;
+int lastMSBB = 0;
+int lastLSBB = 0;
+
+void updateEncoderB()
+{
+ int MSBB = ENCBL.read(); //MSB = most significant bit
+ int LSBB = ENCBJ.read(); //LSB = least significant bit
+
+ int encodedB = (MSBB << 1) |LSBB; //converting the 2 pin value to single number
+ int sumB = (lastEncodedB << 2) | encodedB; //adding it to the previous encoded value
+
+ if(sumB == 0b1101 || sumB == 0b0100 || sumB == 0b0010 || sumB == 0b1011) encoderValueB ++;
+ if(sumB == 0b1110 || sumB == 0b0111 || sumB == 0b0001 || sumB == 0b1000) encoderValueB --;
+
+ lastEncodedB = encodedB; //store this value for next time
+}
+
+
+
+
+/*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());
+}
+long int get_position_G(){
+ /* Getting the current position. */
+ long int position = motors[1]->get_position();
+ return position;
+ /* Printing to the console. */
+ //printf("--> Getting the current position: %d\r\n", position);
+
+}
+long int get_position_D(){
+ /* Getting the current position. */
+ long int position = motors[0]->get_position();
+
+ /* Printing to the console. */
+ //printf("--> Getting the current position: %d\r\n", position);
+ return position;
+
+}