Diff: main2.cpp

Revision:

2:e9cff52e8ee2

Parent:

1:d2ed8dcf965a

--- a/main2.cpp	Thu Dec 17 10:30:23 2020 +0000
+++ b/main2.cpp	Thu Dec 17 13:01:44 2020 +0000
@@ -2,7 +2,7 @@
 #include "dsp.h"
 #define maxX        0.5
 #define PER_ACQ     0.0001
-#define PER_STEP    0.001
+#define PER_STEP    0.00005
 #define PER_LED     2000
 #define MAX_CHAR    128
 
@@ -13,14 +13,23 @@
 AnalogOut   outX(PA_4);
 //AnalogOut   outY(PA_5);
 DigitalOut  out_led(D13);
+DigitalOut  debug_out(D4);
 arm_pid_instance_f32    pidX;
 arm_pid_instance_f32    pidY;
 /* Modules pour communication avec Matlab */
-Serial          pc(USBTX, USBRX); //TX - transmission, RX - reception
+Serial          pc(PC_10, PC_11); //TX - transmission, RX - reception
+Serial          realPC(USBTX, USBRX); //TX - transmission, RX - reception
 InterruptIn     button (USER_BUTTON);
 
 /* Variables globales pour Asservissement */
-double Kp = 1.0;
+char    mode = '0';
+double g_Kpx = 1;
+double g_Kix = 0;
+double g_Kdx = 0;
+double g_Kpy = 1;
+double g_Kiy = 0;
+double g_Kdy = 0;
+double g_sampling_frequency = 1000;
 /* Variables gloables pour Rampe / Step */
 double valX;
 int signeX;
@@ -31,14 +40,23 @@
 char g_ch;
 int g_t_temp=0;
 char data_ok = 0;
+int g_i = 0;
+int g_NotFullCommand = 0;
 int periode_led = PER_LED;
 
+bool IsAN(char A);
+int read_command();
+double k_coef(int i);
+
+void updatePID(void);
+
 /* Fonction d'interruption PID - X et Y */
 void controlLoop(void)
 {
-    out_led = 1;
+    debug_out = 1;
+    float inXdata = inX.read() - 0.5;
     //Process the PID controller
-    float outxx = arm_pid_f32(&pidX, (float) inX.read()-0.5);
+    float outxx = arm_pid_f32(&pidX, inXdata);
     float outyy = arm_pid_f32(&pidY, (float) inY.read()-0.5);
     //Range limit the output
     if (outxx < -0.5)
@@ -50,25 +68,23 @@
     else if (outyy > 0.5)
         outyy = 0.5;  
     //Set the new output duty cycle
+    //outxx = inXdata;
     outX.write(outxx+0.5);
     //outY.write(outyy+0.5);
-    out_led = 0;
+    debug_out = 0;
     g_t_temp++;
 }
 /* Fonction d'interruption de test - Rampe */
 void stepLoop(void){
-    out_led = 1;
+    debug_out = 1;
     valX = valX + signeX * pasX;
     if(valX > maxX){ signeX = -1; }
     if(valX < 0.0){ signeX = 1; }
     outX.write(valX);
-    out_led = 0;
+    debug_out = 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();
@@ -93,6 +109,7 @@
         k++;   
     } while (g_value[k]!='\0');    // loop until the '\n' character    
 }
+
 /* Fonction principale */
 int main()
 {
@@ -104,14 +121,7 @@
     g_value[6] = '\r';
     g_value[7] = '\n';
     //Initialize the PID instance structure
-    pidX.Kp = 1.0;
-    pidX.Ki = 0.001;
-    pidX.Kd = 0.0;
-    arm_pid_init_f32(&pidX, 1);
-    pidY.Kp = 1.0;
-    pidY.Ki = 0.001;
-    pidY.Kd = 0.0;
-    arm_pid_init_f32(&pidY, 1);
+    updatePID();
     //Run the PID control loop every 1ms
     // tik_asst.attach(&stepLoop, PER_STEP);
     button.fall(&IT_button_pressed);
@@ -129,16 +139,165 @@
         }
         
         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!");
+            tik_asst.detach();
+            int comm_ok = read_command();//recognizing coefficients
+            if(comm_ok){
+                pc.printf("%c_OK! gX = %lf gY = %lf \r\n", mode, g_Kpx, g_Kpy);
+                if((mode == 'P') or (mode == 'I') or (mode == 'D')){
+                    updatePID();
+                }     
             }
             data_ok = 0;
             g_index = 0;
+            tik_asst.attach(&controlLoop, PER_ACQ);
         }     
     } 
-}
\ No newline at end of file
+}
+
+/* Mise à jour des coefficients */
+void updatePID(void){
+    switch(mode){
+        case 'P' : g_Kix = 0; g_Kiy = 0; g_Kdx = 0; g_Kdy = 0; break;
+        case 'I' : g_Kdx = 0; g_Kdy = 0; break;
+        case 'D' : break;
+        default : g_Kpx = 1; g_Kpy = 1;
+    }
+    pidX.Kp = g_Kpx;
+    pidX.Ki = g_Kix;
+    pidX.Kd = g_Kdx;
+    arm_pid_init_f32(&pidX, 1);
+    pidY.Kp = g_Kpy;
+    pidY.Ki = g_Kiy;
+    pidY.Kd = g_Kdy;
+    arm_pid_init_f32(&pidY, 1);
+}
+/* Reconnaissance caractères */
+bool IsAN(char A)//check if the character if a number or not
+{
+    char numbers[] = "0123456789\0";
+    for (int i = 0; numbers[i] != '\0'; i++)
+        if (A == numbers[i])
+            return true;
+    return false;
+}
+
+double k_coef(int i)
+{
+    double K = 0;
+    int tempK[100];
+
+    int initial_I = i;
+    int point_j = 0;
+    int negative = 0;
+    
+    int exp_trigger = 0;
+    int exp_negative = 0;
+    int temp_exp[100];
+    int j = 0;
+    int j_exp = 0;
+    double power_of_ten = 0;
+    
+    for (; g_value[i] != '\0' && g_value[i] != '_'; i++)
+    {
+        if (exp_trigger == 0 && IsAN(g_value[i]))
+        {
+            tempK[j] = g_value[i] - 48;
+            j++;
+        }
+        else if (exp_trigger == 1 && IsAN(g_value[i]))
+        {
+            temp_exp[j_exp] = g_value[i] - 48;
+            j_exp++;
+        }
+        
+        else if (exp_trigger == 0 && g_value[i] == '-')
+        {
+            negative = 1;
+            initial_I++;
+        }
+        else if (g_value[i] == 'e' || g_value[i] == 'E')
+        {
+            exp_trigger = 1;
+        }
+        else if (exp_trigger == 1 && (g_value[i] == '+' || g_value[i] == '-'))
+        {
+            if (g_value[i] == '-')
+            {
+                exp_negative = 1;
+            }
+        }
+        else if (exp_trigger == 0 && g_value[i] == '.')
+        {
+            point_j = j;
+        }
+    }
+    g_i = i;
+    for (int k = 0; k < j; k++)
+    {
+        K+= pow(10.0, point_j - k - 1) * tempK[k];
+    }
+    
+    if (exp_trigger==1)
+    {
+        for (int k = 0; k < j_exp; k++)
+        {
+            power_of_ten += pow(10.0, j_exp-1 - k) * temp_exp[k];
+        }
+        power_of_ten = (exp_negative == 0) ? (power_of_ten) : (-power_of_ten);
+        K = K * pow(10.0, power_of_ten);
+    }
+    
+    return negative==0?K:(-K);
+}
+
+int read_command()
+{
+    int i;
+    switch(g_value[0]){
+        case 'P':
+            mode = 'P';
+            i = 2;
+            g_Kpx=k_coef(i);
+            g_Kpy=k_coef(g_i+1);
+            g_sampling_frequency = k_coef(g_i + 1);
+            if (g_value[g_i + 1] == '!')    g_NotFullCommand = 0;
+            else g_NotFullCommand = 1;
+            return 1;
+        case 'I':
+            mode = 'I';
+            i = 2;
+            g_Kpx =k_coef(i);
+            g_Kpy = k_coef(g_i+1);
+            g_Kix = k_coef(g_i + 1);
+            g_Kiy = k_coef(g_i + 1);
+            g_sampling_frequency = k_coef(g_i + 1);
+            if (g_value[g_i + 1] == '!')    g_NotFullCommand = 0;
+            else g_NotFullCommand = 1;
+            return 1;
+        case 'D' :
+            mode = 'D';
+            i = 2;
+            g_Kpx = k_coef(i);
+            g_Kpy = k_coef(g_i + 1);
+            g_Kix = k_coef(g_i + 1);
+            g_Kiy = k_coef(g_i + 1);
+            g_Kdx = k_coef(g_i + 1);
+            g_Kdy = k_coef(g_i + 1);
+            g_sampling_frequency = k_coef(g_i + 1);
+            if (g_value[g_i + 1] == '!')    g_NotFullCommand = 0;
+            else g_NotFullCommand = 1;
+            return 1;
+        case 'S':
+            mode = 'S';
+            g_Kpx = 1.0;
+            g_Kpy = 1.0;
+            g_Kix = 0;
+            g_Kiy = 0;
+            g_Kdx = 0;
+            g_Kdy = 0;
+            return 1;
+        default : 
+            mode = '0';
+            return 0;
+    }
+}