![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 171:bfc1fd2629d8
- Parent:
- 170:42c938a40313
- Child:
- 172:63af34265fe9
--- a/main.cpp Wed Nov 18 12:03:39 2020 +0000 +++ b/main.cpp Sat Nov 21 07:15:47 2020 +0000 @@ -1,4 +1,4 @@ -//201118_1 +//201121_1 #include "mbed.h" #include "FastPWM.h" #include "INIT_HW.h" @@ -112,7 +112,7 @@ MODE_VALVE_OPEN_LOOP, //3 MODE_JOINT_ADAPTIVE_BACKSTEPPING, //4 - MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5 + MODE_RL, //5 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6 MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7 @@ -124,7 +124,7 @@ MODE_CURRENT_CONTROL, //11 MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12 MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13 - MODE_RL, //14 + MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //14 //utility MODE_TORQUE_SENSOR_NULLING = 20, //20 @@ -268,27 +268,27 @@ float input_RL[num_input_RL] = { 0.0f }; //Critic Networks -float hc1[num_input_RL][10] = {0.0f}; -float bc1[10] = {0.0f}; -float hc2[10] = {0.0f}; +float hc1[num_input_RL][num_hidden_unit] = {0.0f}; +float bc1[num_hidden_unit] = {0.0f}; +float hc2[num_hidden_unit] = {0.0f}; float bc2 = 0.0f; //Critic Networks Temporary -float hc1_temp[num_input_RL][10] = {0.0f}; -float bc1_temp[10] = {0.0f}; -float hc2_temp[10] = {0.0f}; +float hc1_temp[num_input_RL][num_hidden_unit] = {0.0f}; +float bc1_temp[num_hidden_unit] = {0.0f}; +float hc2_temp[num_hidden_unit] = {0.0f}; float bc2_temp = 0.0f; //Actor Networks -float ha1[num_input_RL][10] = {0.0f}; -float ba1[10] = {0.0f}; -float ha2[10][2] = {0.0f}; +float ha1[num_input_RL][num_hidden_unit] = {0.0f}; +float ba1[num_hidden_unit] = {0.0f}; +float ha2[num_hidden_unit][2] = {0.0f}; float ba2[2] = {0.0f}; //Actor Networks Temporary -float ha1_temp[num_input_RL][10] = {0.0f}; -float ba1_temp[10] = {0.0f}; -float ha2_temp[10][2] = {0.0f}; +float ha1_temp[num_input_RL][num_hidden_unit] = {0.0f}; +float ba1_temp[num_hidden_unit] = {0.0f}; +float ha2_temp[num_hidden_unit][2] = {0.0f}; float ba2_temp[2] = {0.0f}; float VALVE_POS_RAW_NN = 0.0f; @@ -297,16 +297,16 @@ float Critic_Network(float *arr) { - float output1[10] = { 0.0f }; + float output1[num_hidden_unit] = { 0.0f }; float output = 0.0f; - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { output1[index2] = output1[index2] + hc1[index1][index2] * arr[index1]; } output1[index2] = tanh(output1[index2] + bc1[index2]); } for (int index2 = 0; index2 < 1; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { output = output + hc2[index1] * output1[index1]; } output = output + bc2; @@ -316,16 +316,16 @@ float Critic_Network_Temp(float *arr) { - float output1[10] = { 0.0f }; + float output1[num_hidden_unit] = { 0.0f }; float output = 0.0f; - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { output1[index2] = output1[index2] + hc1_temp[index1][index2] * arr[index1]; } output1[index2] = tanh(output1[index2] + bc1_temp[index2]); } for (int index2 = 0; index2 < 1; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { output = output + hc2_temp[index1] * output1[index1]; } output = output + bc2_temp; @@ -336,10 +336,10 @@ void Actor_Network(float *arr) { - float output1[10] = {0.0f}; + float output1[num_hidden_unit] = {0.0f}; float output[2] = {0.0f}; - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { output1[index2] = output1[index2] + ha1[index1][index2] * arr[index1]; } @@ -349,49 +349,34 @@ } } for (int index2 = 0; index2 < 2; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { output[index2] = output[index2] + ha2[index1][index2] * output1[index1]; } } - mean_before_SP = output[0] + ba2[0]; + mean_before_SP = output[0] + ba2[0]; //SP = softplus deviation_before_SP = output[1] + ba2[1]; - mean = log(1.0f+exp(mean_before_SP)); - deviation = log(1.0f+exp(deviation_before_SP)); -} - - -//void Actor_Network_Temp(float *arr) -//{ -// float output1[10] = {0.0f}; -// float output[2] = {0.0f}; -// -// for (int index2 = 0; index2 < 10; index2++) { -// for (int index1 = 0; index1 < num_input_RL; index1++) { -// output1[index2] = output1[index2] + ha1_temp[index1][index2] * arr[index1]; -// } -// output1[index2] = output1[index2] + ba1_temp[index2]; -// if (output1[index2] < 0) { -// output1[index2] = 0; -// } -// } -// for (int index2 = 0; index2 < 2; index2++) { -// for (int index1 = 0; index1 < 10; index1++) { -// output[index2] = output[index2] + ha2_temp[index1][index2] * output1[index1]; -// } -// } -// mean_before_SP = output[0] + ba2_temp[0]; -// deviation_before_SP = output[1] + ba2_temp[1]; // mean = log(1.0f+exp(mean_before_SP)); // deviation = log(1.0f+exp(deviation_before_SP)); -//} + if (mean_before_SP >=0) { + mean = mean_before_SP; + } else { + mean = 0.0f; + } + if (deviation_before_SP >=0) { + deviation = deviation_before_SP; + } else { + deviation = 0.0f; + } + +} void Actor_Network_Old(float *arr) { - float output1[10] = {0.0f}; + float output1[num_hidden_unit] = {0.0f}; float output[2] = {0.0f}; - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { output1[index2] = output1[index2] + ha1[index1][index2] * arr[index1]; } @@ -401,14 +386,24 @@ } } for (int index2 = 0; index2 < 2; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { output[index2] = output[index2] + ha2[index1][index2] * output1[index1]; } } mean_old = output[0] + ba2[0]; deviation_old = output[1] + ba2[1]; - mean_old = log(1.0f+exp(mean_old)); - deviation_old = log(1.0f+exp(deviation_old)); +// mean_old = log(1.0f+exp(mean_old)); +// deviation_old = log(1.0f+exp(deviation_old)); + if (mean_before_SP >=0) { + mean = mean_before_SP; + } else { + mean = 0.0f; + } + if (deviation_before_SP >=0) { + deviation = deviation_before_SP; + } else { + deviation = 0.0f; + } } float Grad_Normal_Dist_Mean(float mean, float deviation, float action) @@ -429,9 +424,9 @@ { float gradient_rate = 0.01f; - float G_hc1[num_input_RL][10] = {0.0f}; - float G_bc1[10] = {0.0f}; - for (int index2 = 0; index2 < 10; index2++) { + float G_hc1[num_input_RL][num_hidden_unit] = {0.0f}; + float G_bc1[num_hidden_unit] = {0.0f}; + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { for (int i=0; i<batch_size; i++) { float hx_sum = 0.0f; @@ -462,10 +457,10 @@ bc1_temp[index2] = bc1_temp[index2] - gradient_rate * G_bc1[index2]; } - float G_hc2[10] = {0.0f}; + float G_hc2[num_hidden_unit] = {0.0f}; float G_bc2 = 0.0f; for (int index2 = 0; index2 < 1; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { for (int i=0; i<batch_size; i++) { float hx_sum = 0.0f; float hx_sum_next = 0.0f; @@ -491,26 +486,41 @@ void update_Actor_Networks(float (*arr)[num_input_RL]) { - float gradient_rate = -0.01f; + float gradient_rate = 0.01f; //-0.01f - float G_ha1[num_input_RL][10] = {0.0f}; - float G_ba1[10] = {0.0f}; - for (int index2 = 0; index2 < 10; index2++) { + float G_ha1[num_input_RL][num_hidden_unit] = {0.0f}; + float G_ba1[num_hidden_unit] = {0.0f}; + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { for (int i=0; i<batch_size; i++) { if((advantage[i] >= 0.0f && ratio[i] >= 1.0f + epsilon) || (advantage[i] < 0.0f && ratio[i] < 1.0f - epsilon)) { G_ha1[index1][index2] = G_ha1[index1][index2]; } else { - float hx_sum = 0.0f; - for(int j=0; j<num_input_RL; j++) { - hx_sum = hx_sum + ha1_temp[j][index2]*arr[i][j]; + + float hx_sum_total = 0.0f; + for(int m = 0; m < num_hidden_unit; m++) { + for(int n = 0; n < num_input_RL; n++) { + hx_sum_total = hx_sum_total + ha1_temp[n][m]*arr[i][n]; + } } - hx_sum = hx_sum + bc1_temp[index2]; + hx_sum_total = hx_sum_total + bc1_temp[index2]; float d_mean_d_ha1 = 0.0f; float d_dev_d_ha1 = 0.0f; - if (hx_sum >= 0) { - d_mean_d_ha1 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i]))*ha2_temp[index2][0]*arr[i][index1]; - d_dev_d_ha1 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i]))*ha2_temp[index2][1]*arr[i][index1]; + if (hx_sum_total >=0) { + float hx_sum = 0.0f; + for(int j=0; j<num_input_RL; j++) { + hx_sum = hx_sum + ha1_temp[j][index2]*arr[i][j]; + } + hx_sum = hx_sum + bc1_temp[index2]; + if (hx_sum >= 0) { +// d_mean_d_ha1 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i]))*ha2_temp[index2][0]*arr[i][index1]; +// d_dev_d_ha1 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i]))*ha2_temp[index2][1]*arr[i][index1]; + d_mean_d_ha1 = ha2_temp[index2][0]*arr[i][index1]; + d_dev_d_ha1 = ha2_temp[index2][1]*arr[i][index1]; + } else { + d_mean_d_ha1 = 0.0f; + d_dev_d_ha1 = 0.0f; + } } else { d_mean_d_ha1 = 0.0f; d_dev_d_ha1 = 0.0f; @@ -525,16 +535,33 @@ if((advantage[i] >= 0.0f && ratio[i] >= 1.0f + epsilon) || (advantage[i] < 0.0f && ratio[i] < 1.0f - epsilon)) { G_ba1[index2] = G_ba1[index2]; } else { - float hx_sum = 0.0f; - for(int j=0; j<num_input_RL; j++) { - hx_sum = hx_sum + ha1_temp[j][index2]*arr[i][j]; + + float hx_sum_total = 0.0f; + for(int m = 0; m < num_hidden_unit; m++) { + for(int n = 0; n < num_input_RL; n++) { + hx_sum_total = hx_sum_total + ha1_temp[n][m]*arr[i][n]; + } } - hx_sum = hx_sum + bc1_temp[index2]; + hx_sum_total = hx_sum_total + bc1_temp[index2]; float d_mean_d_ba1 = 0.0f; float d_dev_d_ba1 = 0.0f; - if(hx_sum >=0) { - d_mean_d_ba1 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i]))*ha2_temp[index2][0]; - d_dev_d_ba1 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i]))*ha2_temp[index2][1]; + if (hx_sum_total >=0) { + + float hx_sum = 0.0f; + for(int j=0; j<num_input_RL; j++) { + hx_sum = hx_sum + ha1_temp[j][index2]*arr[i][j]; + } + hx_sum = hx_sum + bc1_temp[index2]; + + if(hx_sum >=0) { +// d_mean_d_ba1 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i]))*ha2_temp[index2][0]; +// d_dev_d_ba1 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i]))*ha2_temp[index2][1]; + d_mean_d_ba1 = ha2_temp[index2][0]; + d_dev_d_ba1 = ha2_temp[index2][1]; + } else { + d_mean_d_ba1 = 0.0f; + d_dev_d_ba1 = 0.0f; + } } else { d_mean_d_ba1 = 0.0f; d_dev_d_ba1 = 0.0f; @@ -546,27 +573,42 @@ ba1_temp[index2] = ba1_temp[index2] - gradient_rate * G_ba1[index2]; } - float G_ha2[10][2] = {0.0f}; + float G_ha2[num_hidden_unit][2] = {0.0f}; float G_ba2[2] = {0.0f}; for (int index2 = 0; index2 < 2; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { for (int i=0; i<batch_size; i++) { if((advantage[i] >= 0.0f && ratio[i] >= 1.0f + epsilon) || (advantage[i] < 0.0f && ratio[i] < 1.0f - epsilon)) { G_ha2[index1][index2] = G_ha2[index1][index2]; } else { - float hx_sum = 0.0f; - for(int j=0; j<num_input_RL; j++) { - hx_sum = hx_sum + ha1_temp[j][index1]*arr[i][j]; + + float hx_sum_total = 0.0f; + for(int m = 0; m < num_hidden_unit; m++) { + for(int n = 0; n < num_input_RL; n++) { + hx_sum_total = hx_sum_total + ha1_temp[n][m]*arr[i][n]; + } } - hx_sum = hx_sum + bc1_temp[index1]; + hx_sum_total = hx_sum_total + bc1_temp[index2]; float d_mean_d_ha2 = 0.0f; float d_dev_d_ha2 = 0.0f; - if (hx_sum >= 0) { - d_mean_d_ha2 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i]))*hx_sum; - d_dev_d_ha2 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i]))*hx_sum; + if (hx_sum_total >=0) { + float hx_sum = 0.0f; + for(int j=0; j<num_input_RL; j++) { + hx_sum = hx_sum + ha1_temp[j][index1]*arr[i][j]; + } + hx_sum = hx_sum + bc1_temp[index1]; + if (hx_sum >= 0) { +// d_mean_d_ha2 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i]))*hx_sum; +// d_dev_d_ha2 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i]))*hx_sum; + d_mean_d_ha2 = hx_sum; + d_dev_d_ha2 = hx_sum; + } else { + d_mean_d_ha2 = 0.0f; + d_mean_d_ha2 = 0.0f; + } } else { d_mean_d_ha2 = 0.0f; - d_dev_d_ha2 = 0.0f; + d_mean_d_ha2 = 0.0f; } G_ha2[index1][index2] = G_ha2[index1][index2] + advantage[i]/pi_old[i]*(d_mean_d_ha2*Grad_Normal_Dist_Mean(mean_array[i],deviation_array[i],action_array[i])+d_dev_d_ha2*Grad_Normal_Dist_Deviation(mean_array[i],deviation_array[i],action_array[i])); } @@ -581,8 +623,10 @@ float d_mean_d_ba2 = 0.0f; float d_dev_d_ba2 = 0.0f; - d_mean_d_ba2 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i])); - d_dev_d_ba2 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i])); +// d_mean_d_ba2 = exp(mean_before_SP_array[i])/(1.0f+exp(mean_before_SP_array[i])); +// d_dev_d_ba2 = exp(deviation_before_SP_array[i])/(1.0f+exp(deviation_before_SP_array[i])); + d_mean_d_ba2 = 1.0f; + d_dev_d_ba2 = 1.0f; G_ba1[index2] = G_ba1[index2] + advantage[i]/pi_old[i]*(d_mean_d_ba2*Grad_Normal_Dist_Mean(mean_array[i],deviation_array[i],action_array[i])+d_dev_d_ba2*Grad_Normal_Dist_Deviation(mean_array[i],deviation_array[i],action_array[i])); } } @@ -622,7 +666,7 @@ void Overwirte_Critic_Networks() { - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { hc1[index1][index2] = hc1_temp[index1][index2]; } @@ -633,14 +677,14 @@ } void Overwirte_Actor_Networks() { - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { ha1[index1][index2] = ha1_temp[index1][index2]; } ba1[index2] = ba1_temp[index2]; } for (int index2 = 0; index2 < 2; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { ha2[index1][index2] = ha2_temp[index1][index2]; } ba2[index2] = ba2_temp[index2]; @@ -731,7 +775,7 @@ ID_index_array[i] = (i+1) * 0.5f; } - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { hc1_temp[index1][index2] = (float) (rand()%100) * 0.01f ; } @@ -739,19 +783,19 @@ hc2_temp[index2] = (float) (rand()%100) * 0.01f; } bc2_temp = (float) (rand()%100) * 0.01f; - for (int index2 = 0; index2 < 10; index2++) { + for (int index2 = 0; index2 < num_hidden_unit; index2++) { for (int index1 = 0; index1 < num_input_RL; index1++) { ha1_temp[index1][index2] = (float) (rand()%100) * 0.01f; } ba1_temp[index2] = (float) (rand()%100) * 0.01f; } for (int index2 = 0; index2 < 2; index2++) { - for (int index1 = 0; index1 < 10; index1++) { + for (int index1 = 0; index1 < num_hidden_unit; index1++) { ha2_temp[index1][index2] = (float) (rand()%100) * 0.01f; } ba2_temp[index2] = (float) (rand()%100) * 0.01f; } - + Overwirte_Critic_Networks(); Overwirte_Actor_Networks(); @@ -759,20 +803,21 @@ *** Program is operating! *************************************/ while(1) { -// if(timer_while==1000 && OPERATING_MODE==5) { - //if(timer_while==1000) { - //i2c - read_field(i2c_slave_addr1); - if(DIR_VALVE_ENC < 0) value = 1023 - value; - // if(LED==1) { - // LED=0; - // } else - // LED = 1; - timer_while = 0; - //} +// if(timer_while==27491) { +// timer_while = 0; +// pc.printf("ref : %f virt_pos : %f mean : %f deviation : %f Last_pos_of_batch : %f reward_sum : %f\n", pos.sen/(float)(ENC_PULSE_PER_POSITION), logging3, logging2, logging4, logging1, logging5); +// //pc.printf("%f\n", virt_pos); +// //pc.printf("%f\n", pos.sen/(float)(ENC_PULSE_PER_POSITION)); +// //pc.printf("ref : %f virt_pos : %f\n", pos.sen/(float)(ENC_PULSE_PER_POSITION), virt_pos); +// } - timer_while ++; + + //i2c + //read_field(i2c_slave_addr1); + //if(DIR_VALVE_ENC < 0) value = 1023 - value; + +// timer_while ++; ///////////////////////////////////////////////////////Neural Network @@ -874,8 +919,8 @@ LED = 1; } - - + + /////////////////////////////////////////////////////////////////////RL switch (Update_Case) { case 0: { @@ -886,7 +931,6 @@ for (int epoch = 0; epoch < num_epoch; epoch++) { float loss_sum = 0.0f; for (int i=batch_size-1; i>=0; i--) { - V[batch_size] = 0.0f; //Calculate Estimated V //float temp_array[3] = {state_array[i][0], state_array[i][1], state_array[i][2]}; float temp_array[2] = {state_array[i][0], state_array[i][1]}; @@ -894,7 +938,7 @@ pi[i] = exp(-(action_array[i]-mean_array[i])*(action_array[i]-mean_array[i])/(2.0f*deviation_array[i]*deviation_array[i]))/(sqrt(2.0f*PI)*deviation_array[i]); Actor_Network_Old(temp_array); pi_old[i] = exp(-(action_array[i]-mean_old)*(action_array[i]-mean_old)/(2.0f*deviation_old*deviation_old))/(sqrt(2.0f*PI)*deviation_old); - r[i] = exp(-0.01f * state_array[i][1] * 70.0f * state_array[i][1] * 70.0f); + r[i] = exp(-0.00005f * state_array[i][1] * 70.0f * state_array[i][1] * 70.0f); if(i == batch_size-1) td_target[i] = r[i]; else td_target[i] = r[i] + gamma * V[i+1]; delta[i] = td_target[i] - V[i]; @@ -917,8 +961,8 @@ reward_sum = reward_sum + r[i]; } logging5 = reward_sum; - - + + //loss_batch = loss_sum / (float) batch_size; loss_batch = loss_sum; //Update Networks @@ -929,7 +973,7 @@ Update_Done_Flag = 1; Update_Case = 0; //logging1 = V[0]; - + break; } case 2: { @@ -2311,7 +2355,7 @@ gamma_hat = gamma_hat + gamma_hat_dot / (float) TMR_FREQ_5k; break; } - + case MODE_RL: { //t.reset(); //t.start(); @@ -2344,7 +2388,7 @@ } else if(virt_pos < -70.0f) { virt_pos = -70.0f; } - + RL_timer++; @@ -2563,9 +2607,9 @@ } - if (flag_data_request[2] == LOW) { + if (flag_data_request[2] == HIGH) { double t_value = 0.0f; - if(valve_pos.ref>=(float) VALVE_CENTER) { + if(value>=(float) VALVE_CENTER) { t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); } else { t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); @@ -2596,7 +2640,7 @@ // } if (flag_data_request[4] == HIGH) { //valve position - CAN_TX_VALVE_POSITION((int16_t) (input_NN[1] * 100.0f), (int16_t) (input_NN[2]* 100.0f), (int16_t) (input_NN[3]* 100.0f), (int16_t) (input_NN[4]* 100.0f)); //1600 + CAN_TX_VALVE_POSITION((int16_t) pos.sen/(float)(ENC_PULSE_PER_POSITION), (int16_t) virt_pos, (int16_t) 0, (int16_t) 0); //1600 } // Others : Reference position, Reference FT, PWM, Current (ID:1300)