반응형
asdf
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 | #include "stdio.h" #include "stdlib.h" typedef struct Pid { int size; float *point; float *kp; float *ki; }Pid; float get_linear_interpolation(float lower_x, float upper_x, float lower_y, float upper_y, float x) { return (upper_y - lower_y) * (x - lower_x) / (upper_x - lower_x) + lower_y; } float get_gain(float *point, float *gain, int size, float op_point) { float out = 0.0f; if(size == 0) out = 0.0f; else if(size == 1) out = gain[0]; else if(size >= 2) { if (op_point <= point[0]) out = gain[0]; else if (op_point >= point[size-1]) out = gain[size-1]; else // Search Section { for (int i = 0; i < size - 1; i++) { // printf("Index %d\r\n",i); if(op_point < point[i+1]) { // float value = get_linear_interpolation(point[i], point[i+1], gain[i], gain[i+1], op_point); // printf("x %5.2f %5.2f y %5.2f %5.2f, y %5.2f @ x = %5.2f\r\n", point[i], point[i+1], gain[i], gain[i+1], value, op_point); out = get_linear_interpolation(point[i], point[i+1], gain[i], gain[i+1], op_point); break; } } } } else printf("size %d\r\n", size); return out; // else } int main() { int size = 2; Pid gains; gains.point = (float*)malloc(sizeof(float)*size); gains.kp = (float*)malloc(sizeof(float)*size); gains.ki = (float*)malloc(sizeof(float)*size); printf("Point kp ki \r\n"); for (int i = 0; i < size; i++) { gains.point[i] = i*10.0f +10.0; gains.kp[i] = (float)i +1.0; gains.ki[i] = (float)i*0.1f +0.1; } for (int i = 0; i < size; i++) { printf("%5.1f %5.2f %5.2f\r\n", gains.point[i], gains.kp[i], gains.ki[i]); } printf("Interpolate\r\n"); for (int i = 0; i < size*10; i++) { float point = i + 5.0; float kp = get_gain(gains.point, gains.kp, size, point); float ki = get_gain(gains.point, gains.ki, size, point); printf("%5.1f %5.2f %5.2f\r\n", point, kp, ki); } } | cs |
728x90