Guillaume Chauvon
asser1
Diff: hardware.cpp
- Revision:
- 3:1dba6eca01ad
- Parent:
- 2:5764f89a27f6
- Child:
- 4:deef042e9c02
--- a/hardware.cpp Wed Apr 17 18:55:22 2019 +0000
+++ b/hardware.cpp Mon May 06 13:48:45 2019 +0000
@@ -4,36 +4,37 @@
#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;
-
+//mot G
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. */
{
- 10, /* Motor supply voltage in V. */
+ 12, /* Motor supply voltage in V. */
200, /* Min number of steps per revolution for the motor. */
4, /* Max motor phase voltage in A. */
- 7.06, /* Max motor phase voltage in V. */
+ 7, /* Max motor phase voltage in V. */
300, /* Motor initial speed [step/s]. */
- 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 247.4, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
1500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
992.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
602.7, /* Motor full-step speed threshold [step/s]. */
- 4.5, /* Holding kval [V]. */
- 4.5, /* Constant speed kval [V]. */
- 4.5, /* Acceleration starting kval [V]. */
- 4.5, /* Deceleration starting kval [V]. */
+ 5, /* Holding kval [V]. */
+ 5, /* Constant speed kval [V]. */
+ 5, /* Acceleration starting kval [V]. */
+ 5, /* Deceleration starting kval [V]. */
61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
392.1569e-6, /* Start slope [s/step]. */
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
- 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
- 32, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ 5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 5*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. */
@@ -41,27 +42,27 @@
/* Second Motor. */
{
- 10, /* Motor supply voltage in V. */
+ 12, /* Motor supply voltage in V. */
200, /* Min number of steps per revolution for the motor. */
4, /* Max motor phase voltage in A. */
- 7.06, /* Max motor phase voltage in V. */
+ 7, /* Max motor phase voltage in V. */
300, /* Motor initial speed [step/s]. */
- 500.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ 251.0, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
1500.0, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
992.0, /* Motor maximum speed [step/s]. */
0.0, /* Motor minimum speed [step/s]. */
602.7, /* Motor full-step speed threshold [step/s]. */
- 4.5, /* Holding kval [V]. */
- 4.5, /* Constant speed kval [V]. */
- 4.5, /* Acceleration starting kval [V]. */
- 4.5, /* Deceleration starting kval [V]. */
+ 5, /* Holding kval [V]. */
+ 5, /* Constant speed kval [V]. */
+ 5, /* Acceleration starting kval [V]. */
+ 5, /* Deceleration starting kval [V]. */
61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
392.1569e-6, /* Start slope [s/step]. */
643.1372e-6, /* Acceleration final slope [s/step]. */
643.1372e-6, /* Deceleration final slope [s/step]. */
0, /* Thermal compensation factor (range [0, 15]). */
- 4.5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
- 32, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ 5*1000*1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 5*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. */
@@ -92,7 +93,7 @@
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);
@@ -295,7 +296,73 @@
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("codeuse droite : %d, codeuse gauche : %d\n", lastEncodedB, lastEncodedA);
-}
\ No newline at end of file
+ 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;
+
+}