Olivier Nastorg
mbed-os github
Dependencies: ADS1015 Faulhaber HTU21D_mod MS5837_potless Sensor_Head_RevB_3 USBDevice_dfu Utilsdfu beep
Fork of
ARNSRS_testDFU
by 
POTLESS
Diff: main.cpp
- Revision:
- 17:bef8abc445f2
- Parent:
- 16:917656586772
- Child:
- 18:bfd78c05b589
--- a/main.cpp Mon Nov 27 16:33:13 2017 +0000
+++ b/main.cpp Mon Dec 04 16:14:19 2017 +0000
@@ -1,37 +1,112 @@
+/*
+
+Câblage complet STM32L476RG :
+
+- Port SPI carte SD :
+
+MOSI -> PA_7
+MISO -> PA_6
+SCK -> PA_5
+CS -> PB_6
+
+- Sensor Head :
+
+Alim en 3.3v
+
+SCL -> PB_8
+SDA -> PB_9
+TX -> PC_12
+RX -> PD_2
+AlertRdy -> PA_11
+
+
+- HC_06 :
+
+Alim en 3.3v ou 5v
+
+TX -> PC_10
+RX -> PC_11
+
+- STM32L476RG sur H-Bridge L293 :
+
+Reférence L293 : http://www.ti.com/lit/ds/symlink/l293.pdf
+
+Volet Poumon :
+PWM -> PB_5 -> 1
+FWD -> PB_4 -> 2
+REV -> PB_10 -> 7
+
+Volet Fuite :
+PWM -> PB_13 -> 9
+FWD -> PB_14 -> 10
+REV -> PB_15 -> 15
+
+Potentiomètres retour Position :
+
+Alim en 3.3v
+
+Feedback Volet Poumon -> PC_2
+Feedback Volet Fuite -> PC_3
+
+- Câblage complet du H-Bridge L293 :
+
+1 -> PB_5
+2 -> PB_4
+3 -> Borne moteur
+4 -> non connectée
+5 -> GND
+6 -> Borne moteur
+7 -> PB_10
+8 -> V+ Alimentation moteurs 4.5v - 36v
+
+9 -> PB_13
+10 -> PB_14
+11 -> Borne moteur
+12 -> GND
+13 -> non connectée
+14 -> borne moteur
+15 -> PB_15
+16 -> V+ Alimentation logic 5v (alim moteur si tension compatible....)
+
+*/
+
#include "mbed.h"
#include <string>
#include "Sensor_head_revB.h"
#include "HTU21D.h"
#include "PID.h"
+#include "Actuonix.h"
-//Commandes des servos
-#define PWM_SERVO_POUMON PB_15
-#define PWM_SERVO_FUITE PB_1
-//Retour des servos
+//Commandes des servos
+#define PWM_SERVO_POUMON PB_5
+#define FWD_SERVO_POUMON PB_4
+#define REV_SERVO_POUMON PB_10
#define FEED_BACK_SERVO_POUMON PC_2
+
+#define PWM_SERVO_FUITE PB_13
+#define FWD_SERVO_FUITE PB_14
+#define REV_SERVO_FUITE PB_15
#define FEED_BACK_SERVO_FUITE PC_3
-//Ecrit dans le moniteur série de la tablette à 9600 bds si sur 1, penser à mettre NEED_CONSOLE_OUTPUT à 0
+//Mapping des valeur.
+#define MAX_MAP 100//Mapping en pourcentage
+//#define MAX_MAP 90//Mapping en en degrés sur 90 degrés
+
+
+//Ecrit dans le moniteur série de la tablette à 115200 bds si sur 1, penser à mettre NEED_CONSOLE_OUTPUT à 0
#define NEED_ANDROID_OUTPUT 1
//Sortie en mode VT100, à commenter si on veut une sortie type Arduino
//#define VT100
-//Mode PID, STD et DELTA, à commenter / décommenter
-//#define STD_MODE
-#define PID_MODE
-//#define DELTA_MODE
-
-float delta = 0.0f;
+//Mode PID, STD à commenter / décommenter
+#define STD_MODE
+//#define PID_MODE
#ifdef STD_MODE
int MODE_FLAG = 0;
#endif
-#ifdef DELTA_MODE
-int MODE_FLAG = 1;
-#endif
-
#ifdef PID_MODE
int MODE_FLAG = 2;
#endif
@@ -42,11 +117,15 @@
#define ANDROID(...)
#endif
-//Moniteur série
-Serial serialMonit(USBTX,USBRX,9600);
+Actuonix Servo_Poumon(FEED_BACK_SERVO_POUMON, PWM_SERVO_POUMON, FWD_SERVO_POUMON, REV_SERVO_POUMON, 1, MAX_MAP);
+
+Actuonix Servo_Fuite(FEED_BACK_SERVO_FUITE, PWM_SERVO_FUITE, FWD_SERVO_FUITE, REV_SERVO_FUITE, 1, MAX_MAP);
+
+//Moniteur série, Serial 2
+Serial serialMonit(USBTX,USBRX,115200);
//COM Série vers Android, Serial 3
-Serial android(PC_10,PC_11,9600);
+Serial android(PC_10,PC_11,115200);
//Init de la lib ARNSRS;
SENSOR_HEAD_REV_B sensors;
@@ -72,13 +151,13 @@
int CellO2_1 = 0;
int CellO2_2 = 0;
-//Mesure du tempsd'éxecution du loop
+//Mesure du temps d'éxecution du loop
Timer REAL_RATE;
float RATE = 0;
float RATE_TRUE = 0;
float Ref_Time = 1.0; //La durée de la boucle désirée...
-//HTU21D
+//HTU21D sur l'I2C
HTU21D temphumid(PB_9, PB_8); //Temp humid sensor || SDA, SCL
float Temp2;
int Humid;
@@ -91,21 +170,15 @@
static const char CLS[] = "\x1B[2J";
static const char HOME[] = "\x1B[H";
-//Thread d'intérogation des capteurs
+//Thread d'intérogation des capteurs et de positionnement des volets
Thread thread;
//Contrôle des servos
-PwmOut servo_poumon(PWM_SERVO_POUMON);
-AnalogIn FeedBack_Servo_Poumon(FEED_BACK_SERVO_POUMON);
+float Consigne_poumon = 50;
float volet_poumon_Position;
-float Limit_min_Servo_Poumon, Limit_max_Servo_Poumon;
-float Delta_FB_1;
-PwmOut servo_fuite(PWM_SERVO_FUITE);
-AnalogIn FeedBack_Servo_Fuite(FEED_BACK_SERVO_FUITE);
+float Consigne_fuite = 50;
float volet_fuite_Position;
-float Limit_min_Servo_Fuite, Limit_max_Servo_Fuite;
-float Delta_FB_2;
#ifdef PID_MODE
//Paramètre du PID
@@ -117,21 +190,15 @@
float consigne = 210;
float Max_Input = 1000;
float Min_Input = 80;
-float Max_Output = 1;//Vérifier la valeur pour angle à laisser ouvert...
-float Min_Output = 0;
+float Max_Output = MAX_MAP - 10;//Vérifier la valeur pour angle à laisser ouvert...
+float Min_Output = 10;
//Init PID
-PID control_Servo(Kc, Ti, Td, RATE_PID);//Kc, Ti, Td, interval
+PID control_Servo(Kc, Ti, Td, RATE_PID);
#endif
-//Remap d'une valeur
-float remap(float x, float in_min, float in_max, float out_min, float out_max)
-{
- return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
-//Thread d'intérogation des capteurs, positions servo et constitution de la chaine
+//Thread d'intérogation des capteurs, positions servo
void Get_Info_thread()
{
while (true) {
@@ -152,9 +219,12 @@
Humid = temphumid.sample_humid();
//Retour position des servos
- volet_poumon_Position = 90 - remap(FeedBack_Servo_Poumon, Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90);
- volet_fuite_Position = remap(FeedBack_Servo_Fuite, Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90);
+ volet_poumon_Position = Servo_Poumon.Get_Position();
+ volet_fuite_Position = Servo_Fuite.Get_Position();
+ //Position des servo mise à jour en permanence dans le thread
+ Servo_Poumon.Go_To_Prop(Consigne_poumon);
+ Servo_Fuite.Go_To_Prop(Consigne_fuite);
}
}
@@ -173,11 +243,11 @@
printf(" Cell O2 n 2 = %d\r\n" , CellO2_2);
printf("\r\n");
printf("\n");
- printf(" Volet Poumon = %3.2f%\r\n" , volet_poumon_Position);
- printf(" Volet Fuite = %3.2f%\r\n" , volet_fuite_Position);
+ printf(" Volet Poumon = %f\r\n" , volet_poumon_Position);
+ printf(" Volet Fuite = %f\r\n" , volet_fuite_Position);
printf("\r\n");
printf("Temps d execution de la boucle = %f seconde(s)\n", (RATE + RATE_TRUE / 1000));
- printf("\r\n", "");
+ printf("\r\n");
printf("A enregistrer = %s\n", to_store);
printf("\r\n");
#endif
@@ -198,13 +268,13 @@
printf("\x1b[0m\r Cell O2 n 1 = \x1b[1m\x1b[K%d\n", CellO2_1);
printf("\x1b[0m\r Cell O2 n 2 = \x1b[1m\x1b[K%d\n", CellO2_2);
printf("\n");
- printf("\x1b[0m\r Volet Poumon = \x1b[1m\x1b[K%3.2f%\n", volet_poumon_Position);
- printf("\x1b[0m\r Volet Fuite = \x1b[1m\x1b[K%3.2f%\n", volet_fuite_Position);
+ printf("\x1b[0m\r Volet Poumon = \x1b[1m\x1b[K%.2f\n", volet_poumon_Position);
+ printf("\x1b[0m\r Volet Fuite = \x1b[1m\x1b[K%.2f\n", volet_fuite_Position);
printf("\n");
printf("\x1b[0m\r Temps d execution de la boucle = \x1b[1m\x1b[K%f seconde(s)\n", (RATE + RATE_TRUE / 1000));
- printf("\r\n", "");
+ printf("\r\n");
printf("\x1b[0m\r A enregistrer = \x1b[1m\x1b[K%s\n", to_store);
- printf("\r\n", "");
+ printf("\r\n");
#endif
}
@@ -248,23 +318,11 @@
if ((char)commande == 'T') {
set_time(valeur);
} else if ((char)commande == 'I') {
- servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
-#ifdef DELTA_MODE
- float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
- if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
- servo_fuite = Sf;
-#endif
- printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
+ Consigne_poumon = (float)valeur;
+ printf(" Servo Poumon = %f\r\n", Consigne_poumon);
} else if ((char)commande == 'O') {
- servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
- printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);
- } else if ((char)commande == 'D') {
- delta = valeur;
-#ifdef DELTA_MODE
- float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
- if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
- servo_fuite = Sf;
-#endif
+ Consigne_fuite = (float)valeur;
+ printf(" Servo Fuite = %f\r\n", Consigne_fuite);
} else if ((char)commande == 'R') {
NVIC_SystemReset();
/////////////////////////////////////////
@@ -277,7 +335,6 @@
sensors.cozirSend(message);
}
- //wait_ms(100);
strcpy(param," ");
indexParam = 0;
newParamFlag = false;
@@ -293,23 +350,11 @@
if ((char)commande == 'T') {
set_time(valeur);
} else if ((char)commande == 'I') {
- servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
-#ifdef DELTA_MODE
- float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
- if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
- servo_fuite = Sf;
-#endif
- printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
+ Consigne_poumon = (float)valeur;
+ printf(" Servo Poumon = %f\r\n", Consigne_poumon);
} else if ((char)commande == 'O') {
- servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
- printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);
- } else if ((char)commande == 'D') {
- delta = valeur;
-#ifdef DELTA_MODE
- float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
- if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
- servo_fuite = Sf;
-#endif
+ Consigne_fuite = (float)valeur;
+ printf(" Servo Fuite = %f\r\n", Consigne_fuite);
/////////////////////////////////////////
//Pour rajouter une commande
//} else if ((char)commande == 'X') {
@@ -341,72 +386,17 @@
printf(" UPDATE CONSIGNE PID --> Consigne = %d\r\n\n", consigne);
}
#endif
- //wait_ms(100);
+
strcpy(Android," ");
indexAndroid = 0;
newAndroidFlag = false;
}
-//Calibration des limites de feedback des servos
-void Calibration_servo()
-{
- //Servos, calibration du feedback
- printf(" Check Servos :\r\n");
- printf("\r\n");
- servo_poumon.period(0.001); // à mettre dans le setup si cette calib n'est pas appelée (je l ai trouvée commentée)
- servo_fuite.period(0.001);
- //Rentrer les servos à fond
- servo_poumon = 0;
- servo_fuite = 0;
- wait(4);
-
- Limit_min_Servo_Poumon = FeedBack_Servo_Poumon.read();
- wait_ms(100);
- Limit_min_Servo_Fuite = FeedBack_Servo_Fuite.read();
-
- printf(" FeedBack Servo POUMON min = %f\n", Limit_min_Servo_Poumon);
- printf(" FeedBack Servo FUITE min = %f\n", Limit_min_Servo_Fuite);
- printf("\r\n");
-
- //Sortir les servos à fond
- servo_poumon = 1;
- servo_fuite = 1;
- wait(4);
-
- Limit_max_Servo_Poumon = FeedBack_Servo_Poumon.read();
- wait_ms(100);
- Limit_max_Servo_Fuite = FeedBack_Servo_Fuite.read();
-
- printf(" FeedBack Servo POUMON Max = %f\n", Limit_max_Servo_Poumon);
- printf(" FeedBack Servo FUITE Max = %f\n", Limit_max_Servo_Fuite);
- printf("\r\n");
-
- //Position milieu
- servo_poumon = 0.5;
- servo_fuite = 0.5;
- wait(4);
-
- //Mesure du delta consigne / feedback
- Delta_FB_1 = abs(remap(FeedBack_Servo_Poumon.read(), Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90) - 45);
- Delta_FB_2 = abs(remap(FeedBack_Servo_Fuite.read(), Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90) - 45);
-
- printf(" Delta Servo 1 = %f\n", Delta_FB_1);
- printf(" Delta Servo 2 = %f\n", Delta_FB_2);
-
- if(Delta_FB_1 > 10 || Delta_FB_2 > 10) printf(" Delta Servos non satisfaisant...\r\n\r\n");
-
-#ifdef DELTA_MODE
- //Position initial delta
- servo_poumon = 0.5;
- servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f;
-#endif
-}
-
int main()
{
- //UNIX TIMESTAMP depuis le erminal MAC = date +%s + 7200 pour heure d'été.....
- Calibration_servo();
+ Servo_Poumon.Calibrate();
+ Servo_Fuite.Calibrate();
/*
Par défaut les valeur en cas de calibration sur true sont les suivant
@@ -464,7 +454,6 @@
control_Servo.setMode(AUTO_MODE);
//Consigne à x mb
control_Servo.setSetPoint(consigne);
-
#endif
#ifdef VT100
@@ -491,8 +480,9 @@
char Time_buf[32];
strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds));
+#ifdef PID_MODE
//Fabrication de la chaine à enregistrer
- sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d:%.3f:%.3f:%.3f",
+ sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%.2f:%.2f:%d:%.3f:%.3f:%.3f:%d",
Time_buf,
co2,
ppO2,
@@ -507,7 +497,31 @@
MODE_FLAG,
Kc,
Ti,
- Td);
+ Td,
+ (int)consigne
+ );
+#endif
+#ifndef PID_MODE
+ //Fabrication de la chaine à enregistrer sans les variables du PID
+ sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%.2f:%.2f:%d:%.3f:%.3f:%.3f:%d",
+ Time_buf,
+ co2,
+ ppO2,
+ pression,
+ Temp1,
+ Temp2,
+ Humid,
+ CellO2_1,
+ CellO2_2,
+ volet_poumon_Position,
+ volet_fuite_Position,
+ MODE_FLAG,
+ 0,
+ 0,
+ 0,
+ 0
+ );
+#endif
//Enregistrement de la chaine
sensors.Write_SD((string)to_store);
@@ -525,10 +539,8 @@
#ifdef PID_MODE
//Update du PID
control_Servo.setProcessValue(ppO2);
- //Nouvelle sortie servo
- Commande_PID = control_Servo.compute();
- //Appliquer la consigne
- servo_fuite = Commande_PID;
+ //Nouvelle sortie servo fuite
+ Consigne_fuite = control_Servo.compute();
#endif
//Arrêt du Timer mesurant le temps d'éxecution du code