Masato Taki / Mbed 2 deprecated InflatableSystem

a

Dependencies:   mbed

main.cpp@0:057b99c6fdad, 2018-12-07 (annotated)

Committer:
mtaki
Date:
Fri Dec 07 06:51:41 2018 +0000
Revision:
0:057b99c6fdad
gatipuro

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mtaki 0:057b99c6fdad 1 #include "mbed.h"
mtaki 0:057b99c6fdad 2 #include "stdio.h"
mtaki 0:057b99c6fdad 3
mtaki 0:057b99c6fdad 4 #if !DEVICE_ANALOGOUT
mtaki 0:057b99c6fdad 5 #error You cannot use this example as the AnalogOut is not supported on this device.
mtaki 0:057b99c6fdad 6 #else
mtaki 0:057b99c6fdad 7 AnalogOut my_output(PA_4);
mtaki 0:057b99c6fdad 8 #endif
mtaki 0:057b99c6fdad 9
mtaki 0:057b99c6fdad 10 AnalogIn analog0_value(D11); //変位
mtaki 0:057b99c6fdad 11 AnalogIn analog1_value(A0); //圧力
mtaki 0:057b99c6fdad 12
mtaki 0:057b99c6fdad 13 #define KP (0.000005)
mtaki 0:057b99c6fdad 14 #define KI (0.0)
mtaki 0:057b99c6fdad 15 #define Gravity (9.80665)
mtaki 0:057b99c6fdad 16 #define Beem_length (0.1125)
mtaki 0:057b99c6fdad 17 #define cog_position (0.0628) //展開軸からみた重心位置
mtaki 0:057b99c6fdad 18 #define Beem_mass (1.2923)
mtaki 0:057b99c6fdad 19 #define Area (0.003136)
mtaki 0:057b99c6fdad 20 #define Cylinder_mass (0.2527+0.3004)
mtaki 0:057b99c6fdad 21 #define kt (13.88)
mtaki 0:057b99c6fdad 22 #define Initial_length (0.141)
mtaki 0:057b99c6fdad 23
mtaki 0:057b99c6fdad 24 int main()
mtaki 0:057b99c6fdad 25 {
mtaki 0:057b99c6fdad 26 double Threshold_P = 1.5 * 1000;
mtaki 0:057b99c6fdad 27 double Process_value;
mtaki 0:057b99c6fdad 28
mtaki 0:057b99c6fdad 29 //printf("hello");
mtaki 0:057b99c6fdad 30
mtaki 0:057b99c6fdad 31 while(Process_value<Threshold_P) {
mtaki 0:057b99c6fdad 32 Process_value = (double)analog1_value.read() * 5.07 * 1000;
mtaki 0:057b99c6fdad 33 my_output.write_u16(65535);
mtaki 0:057b99c6fdad 34 printf("P=%f\n",Process_value);
mtaki 0:057b99c6fdad 35
mtaki 0:057b99c6fdad 36 }
mtaki 0:057b99c6fdad 37
mtaki 0:057b99c6fdad 38 double Pressure_target; //目標圧力
mtaki 0:057b99c6fdad 39 double Pressure_diff;
mtaki 0:057b99c6fdad 40 double Pressure_integral;
mtaki 0:057b99c6fdad 41 double Displacement; //インフレータブルチューブ初期長さからの変位
mtaki 0:057b99c6fdad 42 double voltage = 0;
mtaki 0:057b99c6fdad 43 double T;
mtaki 0:057b99c6fdad 44
mtaki 0:057b99c6fdad 45 double PtoBit = 65535 / 66*1000; //65535 bit / 66 kPa
mtaki 0:057b99c6fdad 46
mtaki 0:057b99c6fdad 47 Timer time;
mtaki 0:057b99c6fdad 48 time.start();
mtaki 0:057b99c6fdad 49
mtaki 0:057b99c6fdad 50 while(1) {
mtaki 0:057b99c6fdad 51 Displacement = (((double)analog0_value.read() / 4.0) - (1.0 / 20.0)); //型変換必要あり20181205
mtaki 0:057b99c6fdad 52 Pressure_target = ((Beem_mass * cog_position / Beem_length + Cylinder_mass ) * Gravity - kt * ( Initial_length - Displacement )) / Area ;
mtaki 0:057b99c6fdad 53
mtaki 0:057b99c6fdad 54 Process_value = (double)analog1_value.read() * 5.07 * 1000.0; //bit-圧力[Pa]変換:0-5.07 [kPaG],0-0.1 [割合]
mtaki 0:057b99c6fdad 55 //Pressure_diff = Pressure_target - Process_value;
mtaki 0:057b99c6fdad 56 Pressure_diff = 2500.0 - Process_value;
mtaki 0:057b99c6fdad 57
mtaki 0:057b99c6fdad 58 //voltage = (int)((KP * Pressure_diff + KI * Pressure_integral)*PtoBit);
mtaki 0:057b99c6fdad 59 voltage = ((KP * Pressure_diff + KI * Pressure_integral)*PtoBit);
mtaki 0:057b99c6fdad 60 voltage = (int)(voltage);
mtaki 0:057b99c6fdad 61
mtaki 0:057b99c6fdad 62 if(voltage < 0) {
mtaki 0:057b99c6fdad 63 my_output.write_u16(0);
mtaki 0:057b99c6fdad 64 } else if(voltage >= 0 && voltage <= 1689) {
mtaki 0:057b99c6fdad 65 my_output.write_u16(1689);
mtaki 0:057b99c6fdad 66 } else if(voltage > 1689 && voltage <= 65535) {
mtaki 0:057b99c6fdad 67 my_output.write_u16(voltage);
mtaki 0:057b99c6fdad 68 } else if(voltage > 65535) {
mtaki 0:057b99c6fdad 69 my_output.write_u16(65535);
mtaki 0:057b99c6fdad 70 }
mtaki 0:057b99c6fdad 71
mtaki 0:057b99c6fdad 72 Pressure_integral += Pressure_diff;
mtaki 0:057b99c6fdad 73
mtaki 0:057b99c6fdad 74 T = time.read();
mtaki 0:057b99c6fdad 75 printf("T=%f\t",T);
mtaki 0:057b99c6fdad 76
mtaki 0:057b99c6fdad 77 //printf("Vin=%f\t",analog1_value.read());
mtaki 0:057b99c6fdad 78 //printf("X=%f\t",Displacement);
mtaki 0:057b99c6fdad 79 printf("P=%f\t",Process_value);
mtaki 0:057b99c6fdad 80 //printf("Pt=%f\t",Pressure_target);
mtaki 0:057b99c6fdad 81 printf("Pd=%f\t",Pressure_diff);
mtaki 0:057b99c6fdad 82 printf("Vout=%f\n",voltage);
mtaki 0:057b99c6fdad 83
mtaki 0:057b99c6fdad 84
mtaki 0:057b99c6fdad 85
mtaki 0:057b99c6fdad 86 }
mtaki 0:057b99c6fdad 87 }