Diff: main2.cpp

Revision:

1:d2ed8dcf965a

Parent:

0:44e787b4e1d0

Child:

2:e9cff52e8ee2

--- a/main2.cpp	Sat Dec 12 12:19:03 2020 +0000
+++ b/main2.cpp	Thu Dec 17 10:30:23 2020 +0000
@@ -1,37 +1,45 @@
 #include "mbed.h"
 #include "dsp.h"
- 
-Ticker ticker;
+#define maxX        0.5
+#define PER_ACQ     0.0001
+#define PER_STEP    0.001
+#define PER_LED     2000
+#define MAX_CHAR    128
+
+/* Modules pour Asservissement */
+Ticker      tik_asst;
 AnalogIn    inX(PC_2);
 AnalogIn    inY(PC_0);
 AnalogOut   outX(PA_4);
-AnalogOut   outY(PA_5);
-DigitalOut  out_led(D4);
-arm_pid_instance_f32 pidX;
-arm_pid_instance_f32 pidY;
+//AnalogOut   outY(PA_5);
+DigitalOut  out_led(D13);
+arm_pid_instance_f32    pidX;
+arm_pid_instance_f32    pidY;
+/* Modules pour communication avec Matlab */
+Serial          pc(USBTX, USBRX); //TX - transmission, RX - reception
+InterruptIn     button (USER_BUTTON);
 
+/* Variables globales pour Asservissement */
 double Kp = 1.0;
-//
-void controlLop(void){
-    out_led = 1;
-    double outxx, outyy;
-    
-    outxx = inX.read()-0.5;
-    outyy = inY.read()-0.5;
-    
-    outX.write(Kp*outxx);
-    outY.write(Kp*outyy);
-    out_led = 0;
-}
-//
+/* Variables gloables pour Rampe / Step */
+double valX;
+int signeX;
+double pasX = 0.01;
+/* Variables globales pour communication avec Matlab */
+char g_value[MAX_CHAR];
+int g_index=0;
+char g_ch;
+int g_t_temp=0;
+char data_ok = 0;
+int periode_led = PER_LED;
+
+/* Fonction d'interruption PID - X et Y */
 void controlLoop(void)
 {
     out_led = 1;
-    
     //Process the PID controller
-    double outxx = arm_pid_f32(&pidX, inX.read()-0.5);
-    double outyy = arm_pid_f32(&pidY, inY.read()-0.5);
-    
+    float outxx = arm_pid_f32(&pidX, (float) inX.read()-0.5);
+    float outyy = arm_pid_f32(&pidY, (float) inY.read()-0.5);
     //Range limit the output
     if (outxx < -0.5)
         outxx = -0.5;
@@ -43,16 +51,58 @@
         outyy = 0.5;  
     //Set the new output duty cycle
     outX.write(outxx+0.5);
-    outY.write(outyy+0.5);
-    /*
-    outX.write(inX.read());
-    outY.write(inY.read());
-    */
+    //outY.write(outyy+0.5);
+    out_led = 0;
+    g_t_temp++;
+}
+/* Fonction d'interruption de test - Rampe */
+void stepLoop(void){
+    out_led = 1;
+    valX = valX + signeX * pasX;
+    if(valX > maxX){ signeX = -1; }
+    if(valX < 0.0){ signeX = 1; }
+    outX.write(valX);
     out_led = 0;
+    g_t_temp++;
+}  
+/* Fonction d'interruption de test - NOTHING */
+void noLoop(void){
+    g_t_temp++;
 }
- 
+/* Fonction d'interruption pour la communication RS232 avec Matlab */
+void IT_Rx_Matlab(){
+    tik_asst.detach();
+    g_ch = pc.getc();   // read it
+    if ((g_index<MAX_CHAR-1) and (data_ok == 0)){           
+        g_value[g_index]=g_ch;  // put it into the value array and increment the index
+        g_index++;
+    }
+    if(g_ch == '\n'){
+        g_value[g_index] = '\0';
+        g_index=0;
+        data_ok = 1;
+    }
+    tik_asst.attach(&controlLoop, PER_ACQ);
+}
+
+/* Fonction d'interruption pour bouton */
+void IT_button_pressed(void){
+    int k=0;
+    do{
+        pc.putc(g_value[k]);
+        k++;   
+    } while (g_value[k]!='\0');    // loop until the '\n' character    
+}
+/* Fonction principale */
 int main()
 {
+    valX = 0;
+    signeX = 1;
+    pc.baud(9600);
+    pc.attach(&IT_Rx_Matlab);
+    strcpy(g_value, "Coucou");
+    g_value[6] = '\r';
+    g_value[7] = '\n';
     //Initialize the PID instance structure
     pidX.Kp = 1.0;
     pidX.Ki = 0.001;
@@ -62,12 +112,33 @@
     pidY.Ki = 0.001;
     pidY.Kd = 0.0;
     arm_pid_init_f32(&pidY, 1);
- 
     //Run the PID control loop every 1ms
-    ticker.attach(&controlLoop, 0.0001);
-    //ticker.attach(&controlLop, 0.0001);
- 
-    while(1) {
-        //Nothing to do here (except sleep maybe)
-    }
+    // tik_asst.attach(&stepLoop, PER_STEP);
+    button.fall(&IT_button_pressed);
+    tik_asst.attach(&controlLoop, PER_ACQ);
+    //tik_asst.attach(&noLoop, PER_ACQ);
+    
+    pc.printf("im here !!\r\n");
+    
+    while (true) {
+     
+        if(g_t_temp>=periode_led) // LED blinking
+        {    
+            g_t_temp=0;
+            out_led = !out_led;
+        }
+        
+        if(data_ok){
+            if(g_value[0] == 'P'){   // Mode PID
+                periode_led = 10000;
+                pc.printf("P_OK!");
+            }
+            if(g_value[0] == 'S'){   // Mode STEP
+                periode_led = 2000;    
+                pc.printf("S_OK!");
+            }
+            data_ok = 0;
+            g_index = 0;
+        }     
+    } 
 }
\ No newline at end of file