Diff: main.cpp

Revision:

0:057b99c6fdad

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 07 06:51:41 2018 +0000
@@ -0,0 +1,87 @@
+#include "mbed.h"
+#include "stdio.h"
+
+#if !DEVICE_ANALOGOUT
+#error You cannot use this example as the AnalogOut is not supported on this device.
+#else
+AnalogOut my_output(PA_4);
+#endif
+
+AnalogIn analog0_value(D11);        //変位
+AnalogIn analog1_value(A0);         //圧力
+
+#define KP              (0.000005)
+#define KI              (0.0)
+#define Gravity         (9.80665)
+#define Beem_length     (0.1125)
+#define cog_position    (0.0628)        //展開軸からみた重心位置
+#define Beem_mass       (1.2923)
+#define Area            (0.003136)
+#define Cylinder_mass   (0.2527+0.3004)
+#define kt              (13.88)
+#define Initial_length  (0.141)
+
+int main()
+{
+    double Threshold_P = 1.5 * 1000;
+    double Process_value;
+
+    //printf("hello");
+
+    while(Process_value<Threshold_P) {
+        Process_value = (double)analog1_value.read() * 5.07 * 1000;
+        my_output.write_u16(65535);
+        printf("P=%f\n",Process_value);
+
+    }
+
+    double Pressure_target;             //目標圧力
+    double Pressure_diff;
+    double Pressure_integral;
+    double Displacement;                //インフレータブルチューブ初期長さからの変位
+    double voltage = 0;
+    double T;
+
+    double PtoBit = 65535 / 66*1000;  //65535 bit / 66 kPa
+
+    Timer time;
+    time.start();
+
+    while(1) {
+        Displacement = (((double)analog0_value.read() / 4.0) - (1.0 / 20.0));        //型変換必要あり20181205
+        Pressure_target = ((Beem_mass * cog_position / Beem_length + Cylinder_mass ) * Gravity - kt * ( Initial_length - Displacement )) / Area ;
+
+        Process_value = (double)analog1_value.read() * 5.07 * 1000.0;        //bit-圧力[Pa]変換:0-5.07 [kPaG],0-0.1 [割合]
+        //Pressure_diff = Pressure_target - Process_value;
+        Pressure_diff = 2500.0 - Process_value;
+
+        //voltage = (int)((KP * Pressure_diff + KI * Pressure_integral)*PtoBit);
+        voltage = ((KP * Pressure_diff + KI * Pressure_integral)*PtoBit);
+        voltage = (int)(voltage);
+
+        if(voltage < 0) {
+            my_output.write_u16(0);
+        } else if(voltage >= 0 && voltage <= 1689) {
+            my_output.write_u16(1689);
+        } else if(voltage > 1689 && voltage <= 65535) {
+            my_output.write_u16(voltage);
+        } else if(voltage > 65535) {
+            my_output.write_u16(65535);
+        }
+
+        Pressure_integral += Pressure_diff;
+        
+        T = time.read();
+        printf("T=%f\t",T);
+                
+        //printf("Vin=%f\t",analog1_value.read());
+        //printf("X=%f\t",Displacement);
+        printf("P=%f\t",Process_value);
+        //printf("Pt=%f\t",Pressure_target);
+        printf("Pd=%f\t",Pressure_diff);
+        printf("Vout=%f\n",voltage);
+                
+                
+        
+    }
+}
\ No newline at end of file