Kim GiJeong
/
HydraulicControlBoard_LIGHT_GJ
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 19:23b7c1ad8683, committed 2019-09-09
- Comitter:
- Lightvalve
- Date:
- Mon Sep 09 06:29:56 2019 +0000
- Parent:
- 18:b8adf1582ea3
- Child:
- 20:806196fda269
- Commit message:
- 190909
Changed in this revision
--- a/CAN/function_CAN.cpp Wed Sep 04 11:02:57 2019 +0000
+++ b/CAN/function_CAN.cpp Mon Sep 09 06:29:56 2019 +0000
@@ -1154,7 +1154,7 @@
int32_t valve_pos_vs_flowrate;
valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
- int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 50 + DDV_CENTER;
+ int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + DDV_CENTER;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
--- a/CAN/function_CAN.h Wed Sep 04 11:02:57 2019 +0000 +++ b/CAN/function_CAN.h Mon Sep 09 06:29:56 2019 +0000 @@ -224,6 +224,7 @@ void CAN_RX_HANDLER(); void CAN_ID_INIT(void); -extern int ID_index_array[100]; +extern int ID_index_array[50]; #endif +
--- a/function_utilities/function_utilities.cpp Wed Sep 04 11:02:57 2019 +0000
+++ b/function_utilities/function_utilities.cpp Mon Sep 09 06:29:56 2019 +0000
@@ -9,6 +9,21 @@
//FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
+//int CID_RX_CMD = 100;
+//int CID_RX_REF_POSITION = 200;
+//int CID_RX_REF_TORQUE = 300;
+//int CID_RX_REF_PRES_DIFF = 400;
+//int CID_RX_REF_VOUT = 500;
+//int CID_RX_REF_VALVE_POSITION = 600;
+//int CID_RX_REF_CURRENT = 700;
+//
+//int CID_TX_INFO = 1100;
+//int CID_TX_POSITION = 1200;
+//int CID_TX_TORQUE = 1300;
+//int CID_TX_PRES = 1400;
+//int CID_TX_VOUT = 1500;
+//int CID_TX_VALVE_POSITION = 1600;
+
/*******************************************************************************
* VARIABLE
******************************************************************************/
@@ -206,13 +221,12 @@
int VALVE_POS_TMP = 0;
int JOINT_VEL_TMP = 0;
int DDV_POS_AVG = 0;
-int VALVE_POS_AVG[100] = {0};
+int VALVE_POS_AVG[50] = {0};
int VALVE_POS_AVG_OLD = 0;
int data_num = 0;
int ID_index = 0;
int DZ_index = 1;
-int ID_index_array[100] = {0};
-int FR_index_array[100] = {0};
+int ID_index_array[50] = {0};
int first_check = 0;
double init_time = 0.0;
int VEL_POINT = 0;
@@ -226,7 +240,8 @@
int one_period_end = 0;
double Ref_Vel_Test = 0.0;
long TMR2_FOR_SLOW_LOGGING = 0;
-int velcount = 0;
+//int velcount = 0;
+int Ref_Valve_Pos_FF_CAN = 0;
char max_check = 0;
char min_check = 0;
@@ -284,6 +299,8 @@
double CUR_CURRENT = 0.0;
double u_CUR[3] = {0.0,0.0,0.0};
+//int Ref_Valve_Pos_FF_CAN = 0;
+
/*******************************************************************************
* General math functions
******************************************************************************/
@@ -324,12 +341,6 @@
/*******************************************************************************
* ROM functions
******************************************************************************/
-void ROM_INIT_DATA(void)
-{
- //ROM_RESET_DATA();
- ROM_CALL_DATA();
-}
-
void ROM_RESET_DATA(void) {
FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
if (!writer.ready()) writer.open();
@@ -395,8 +406,8 @@
}
for(int i=0; i<100; i++)
{
- writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[ID_index] & 0xFFFF));
- writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF));
+ writer.write(RID_VALVE_POS_VS_FLOWRATE_0 + i, (int) (JOINT_VEL[i] & 0xFFFF));
+ writer.write(RID_VALVE_POS_VS_FLOWRATE_0_1 + i, (int) ((JOINT_VEL[i] >> 16) & 0xFFFF));
}
writer.write(RID_VALVE_MAX_POS, (int) VALVE_MAX_POS);
writer.write(RID_VALVE_MIN_POS, (int) VALVE_MIN_POS);
@@ -550,3 +561,4 @@
}
+
--- a/function_utilities/function_utilities.h Wed Sep 04 11:02:57 2019 +0000 +++ b/function_utilities/function_utilities.h Mon Sep 09 06:29:56 2019 +0000 @@ -26,7 +26,6 @@ void ROM_RESET_DATA(void); void ROM_CALL_DATA(void); -void ROM_INIT_DATA(void); void ENC_UPDATE(void); void ENC_SET_ZERO(void);
--- a/main.cpp Wed Sep 04 11:02:57 2019 +0000
+++ b/main.cpp Mon Sep 09 06:29:56 2019 +0000
@@ -70,6 +70,22 @@
int timer_while = 0;
int while_index = 0;
+
+extern int CID_RX_CMD;
+extern int CID_RX_REF_POSITION;
+extern int CID_RX_REF_TORQUE;
+extern int CID_RX_REF_PRES_DIFF;
+extern int CID_RX_REF_VOUT;
+extern int CID_RX_REF_VALVE_POSITION;
+extern int CID_RX_REF_CURRENT;
+
+extern int CID_TX_INFO;
+extern int CID_TX_POSITION;
+extern int CID_TX_TORQUE;
+extern int CID_TX_PRES;
+extern int CID_TX_VOUT;
+extern int CID_TX_VALVE_POSITION;
+
// =============================================================================
// =============================================================================
// =============================================================================
@@ -129,8 +145,8 @@
/*********************************
*** Initialization
*********************************/
- LED = 1;
- pc.baud(9600);
+ //LED = 1;
+ //pc.baud(9600);
// i2c init
i2c.frequency(400 * 1000); // 0.4 mHz
@@ -147,8 +163,7 @@
make_delay();
//rom
-
- ROM_INIT_DATA();
+ ROM_CALL_DATA();
make_delay();
// ADC init
@@ -169,19 +184,18 @@
can.attach(&CAN_RX_HANDLER);
CAN_ID_INIT();
make_delay();
-
+ can.filter(msg.id, 0xFFFFF000, CANStandard);
+
// spi _ enc
spi_enc_set_init();
make_delay();
//DAC init
dac_1 = PRES_A_VREF / 3.3;
- //dac_1 = 0.0;
dac_2 = PRES_B_VREF / 3.3;
- //dac_2 = 0.0;
make_delay();
- for (int i=0; i<100; i++) {
+ for (int i=0; i<50; i++) {
if(i%2==0)
ID_index_array[i] = - i * 0.5;
else
@@ -192,15 +206,15 @@
*** Program is operating!
*************************************/
while(1) {
- if(timer_while==1){
+ 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;
+// if(LED==1) {
+// LED=0;
+// } else
+// LED = 1;
timer_while = 0;
}
@@ -216,9 +230,9 @@
for(i=0; i<VALVE_POS_NUM; i++) {
if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) {
if(i==0) {
- Ref_Valve_Pos_FF = ((int) 50.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+ Ref_Valve_Pos_FF = ((int) 10.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
} else {
- Ref_Valve_Pos_FF = ((int) 50.0*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 50*ID_index_array[i-1];
+ Ref_Valve_Pos_FF = ((int) 10.0*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 10*ID_index_array[i-1];
}
break;
}
@@ -228,6 +242,7 @@
} else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Ref_Valve_Pos_FF = VALVE_MIN_POS;
}
+ Ref_Valve_Pos_FF_CAN = Ref_Valve_Pos_FF;
return Ref_Valve_Pos_FF;
}
@@ -261,6 +276,9 @@
break;
}
}
+
+ //VALVE_PWM_RAW_FF = 0.0;
+
V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
}
@@ -308,201 +326,74 @@
TIMER INTERRUPT
*******************************************************************************/
-unsigned long CNT_TMR4 = 0;
+//unsigned long CNT_TMR4 = 0;
double FREQ_TMR4 = (double)FREQ_10k;
double DT_TMR4 = (double)DT_10k;
extern "C" void TIM4_IRQHandler(void)
{
- if ( TIM4->SR & TIM_SR_UIF ) {
- /*******************************************************
- *** Sensor Read & Data Handling
- ********************************************************/
+ if (TIM4->SR & TIM_SR_UIF ) {
+
+ /*******************************************************
+ *** Sensor Read & Data Handling
+ ********************************************************/
-
- //Using LoadCell
+
+ //Using LoadCell
// ADC1->CR2 |= 0x40000000; // adc _ 12bit
// //while((ADC1->SR & 0b10));
// double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
// double torque_new = ((double)ADC1->DR - PRES_A_NULL) / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0;
// torq.sen = torq.sen*(1.0-alpha_update_torque)+torque_new*(alpha_update_torque);
- //Pressure sensor A
- ADC1->CR2 |= 0x40000000; // adc _ 12bit
- //while((ADC1->SR & 0b10));
- double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
- double pres_A_new = ((double)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
- pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+
- //Pressure sensor 1B
- //ADC2->CR2 |= 0x40000000; // adc _ 12bit
- //while((ADC2->SR & 0b10));
- double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
- double pres_B_new = ((double)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR;
- pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
- torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
+ //Pressure sensor A
+ ADC1->CR2 |= 0x40000000; // adc _ 12bit
+ //while((ADC1->SR & 0b10));
+ double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+ double pres_A_new = ((double)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
+ pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+
+ //Pressure sensor 1B
+ //ADC2->CR2 |= 0x40000000; // adc _ 12bit
+ //while((ADC2->SR & 0b10));
+ double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+ double pres_B_new = ((double)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR;
+ pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+ torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
- //Current
- //ADC3->CR2 |= 0x40000000; // adc _ 12bit
+ //Current
+ //ADC3->CR2 |= 0x40000000; // adc _ 12bit
// a1=ADC2->DR;
- int raw_cur = ADC3->DR;
- //while((ADC3->SR & 0b10));
- double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
- double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
- cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
- cur.sen = raw_cur;
+ int raw_cur = ADC3->DR;
+ //while((ADC3->SR & 0b10));
+ double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
+ double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
+ cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
+ cur.sen = raw_cur;
-
-
- /*******************************************************
- *** Timer Counting & etc.
- ********************************************************/
- CNT_TMR4++;
- }
+ /*******************************************************
+ *** Timer Counting & etc.
+ ********************************************************/
+ //CNT_TMR4++;
+ }
TIM4->SR = 0x0; // reset the status register
}
+
+
int j =0;
-unsigned long CNT_TMR3 = 0;
+//unsigned long CNT_TMR3 = 0;
double FREQ_TMR3 = (double)FREQ_5k;
double DT_TMR3 = (double)DT_5k;
extern "C" void TIM3_IRQHandler(void)
-{
- if ( TIM3->SR & TIM_SR_UIF ) {
-
+{
+ if (TIM3->SR & TIM_SR_UIF ) {
ENC_UPDATE();
- /*
- // Reference Loop
- switch (REFERENCE_MODE) {
- case MODE_REF_NO_ACT: {
- break;
- }
- case MODE_REF_DIRECT: {
- if (FLAG_REFERENCE_VALVE_PWM) {
- Vout.ref = (double) Vout.ref;
- }
- if (FLAG_REFERENCE_VALVE_POSITION) {
- valve_pos.ref = (double) valve_pos.ref;
- }
- if (FLAG_REFERENCE_JOINT_POSITION) {
- pos.ref = (double) pos.ref;
- vel.ref = (double) vel.ref;
- }
- if (FLAG_REFERENCE_JOINT_TORQUE) {
- torq.ref = (double) torq.ref;
- }
- if (FLAG_REFERENCE_CURRENT) {
- cur.ref = ((double)cur.ref);
- }
- break;
- }
-
- case MODE_REF_COS_INC: {
- if (FLAG_REFERENCE_VALVE_PWM) {
- Vout.ref = ((double) Vout.ref - (double) INIT_Vout.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_Vout.ref;
- }
- if (FLAG_REFERENCE_VALVE_POSITION) {
- valve_pos.ref = ((double) valve_pos.ref - (double) INIT_Valve_Pos.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_Valve_Pos.ref;
- }
- if (FLAG_REFERENCE_JOINT_POSITION) {
- pos.ref = ((double) pos.ref - (double) INIT_Pos.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_Pos.ref;
- vel.ref = 0.0;
- }
- if (FLAG_REFERENCE_JOINT_TORQUE) {
- torq.ref = ((double) torq.ref - (double) INIT_torq.ref)*(0.5 - 0.5 * cos(3.14159 * (double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_torq.ref;
- }
- TMR3_COUNT_REFERENCE++;
- if (TMR3_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
- //REFERENCE_MODE = MODE_REF_DIRECT;
- TMR3_COUNT_REFERENCE = REF_MOVE_TIME_5k;
- //TMR3_COUNT_REFERENCE = 0;
- }
- break;
- }
-
+ // CONTROL LOOP ------------------------------------------------------------
- case MODE_REF_LINE_INC: {
- if (FLAG_REFERENCE_VALVE_PWM) {
- Vout.ref = ((double) Vout.ref - (double) INIT_Vout.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_Vout.ref;
- }
- if (FLAG_REFERENCE_VALVE_POSITION) {
- valve_pos.ref = ((double) valve_pos.ref - (double) INIT_valve_pos.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_valve_pos.ref;
- }
- if (FLAG_REFERENCE_JOINT_POSITION) {
- pos.ref = ((double) pos.ref - (double) INIT_REF_POS)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS;
- vel.ref = ((double) pos.ref - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k; // pulse/sec
- }
- if (FLAG_REFERENCE_JOINT_TORQUE) {
- torq.ref = ((double) torq.ref - (double) INIT_torq.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_torq.ref;
- }
- TMR3_COUNT_REFERENCE++;
- if (TMR3_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
- //REFERENCE_MODE = MODE_REF_DIRECT;
- TMR3_COUNT_REFERENCE = REF_MOVE_TIME_5k;
- //TMR3_COUNT_REFERENCE = 0;
- }
- break;
- }
-
- case MODE_REF_SIN_WAVE: {
- if (FLAG_REFERENCE_VALVE_PWM) {
- Vout.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) Vout.ref;
- }
- if (FLAG_REFERENCE_VALVE_POSITION) {
- valve_pos.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) valve_pos.ref;
- }
- if (FLAG_REFERENCE_JOINT_POSITION) {
- Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) pos.ref;
- }
- if (FLAG_REFERENCE_JOINT_TORQUE) {
- Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref;
- }
- TMR3_COUNT_REFERENCE++;
- if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) {
- REFERENCE_MODE = MODE_REF_DIRECT;
- TMR3_COUNT_REFERENCE = 0;
- }
- break;
- }
-
- case MODE_REF_SQUARE_WAVE: {
- if (FLAG_REFERENCE_VALVE_PWM) {
- Vout.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) Vout.ref;
- if (Vout.ref >= Vout.ref) Vout.ref = REF_MAG + Vout.ref;
- else Vout.ref = -REF_MAG + Vout.ref;
- }
- if (FLAG_REFERENCE_VALVE_POSITION) {
- valve_pos.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) valve_pos.ref;
- if (valve_pos.ref >= valve_pos.ref) valve_pos.ref = REF_MAG + valve_pos.ref;
- else valve_pos.ref = -REF_MAG + valve_pos.ref;
- }
- if (FLAG_REFERENCE_JOINT_POSITION) {
- Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) pos.ref;
- if (Ref_Joint_Pos >= pos.ref) valve_pos.ref = REF_MAG + pos.ref;
- else Ref_Joint_Pos = -REF_MAG + pos.ref;
- }
- if (FLAG_REFERENCE_JOINT_TORQUE) {
- Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref;
- if (Ref_Joint_Torq >= torq.ref) valve_pos.ref = REF_MAG + torq.ref;
- else Ref_Joint_Torq = -REF_MAG + torq.ref;
- }
- TMR3_COUNT_REFERENCE++;
- if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) {
- REFERENCE_MODE = MODE_REF_DIRECT;
- TMR3_COUNT_REFERENCE = 0;
- }
- break;
- }
-
- default:
- break;
- }
-
- */
-
- // CONTROL LOOP ------------------------------------------------------------
-
switch (CONTROL_MODE) {
case MODE_NO_ACT: {
V_out = 0;
@@ -551,7 +442,7 @@
// if (torq.err_sum<-1000) torq.err_sum = -1000;
// VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
-
+
PWM_RAW_FORCE_FB = 0.0;
V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
@@ -561,9 +452,9 @@
case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: {
double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback
- double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward
+ //double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward
double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback
- int DDV_JOINT_CAN = 0;
+ //int DDV_JOINT_CAN = 0;
// feedback input for position control
pos.err = pos.ref - (double) pos.sen;
pos.err_diff = pos.err - pos.err_old;
@@ -594,9 +485,8 @@
// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
VALVE_POS_RAW_FORCE_FB = 0.0;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
- //valve_pos.ref = DDV_JOINT_POS_FF(Ref_Joint_Vel);
-
+ //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
break;
}
@@ -686,7 +576,7 @@
valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
VALVE_POS_CONTROL(valve_pos.ref);
-
+
break;
}
@@ -695,66 +585,66 @@
break;
}
-
- case MODE_TEST_CURRENT_CONTROL:
- {
+
+ case MODE_TEST_CURRENT_CONTROL: {
if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
TMR3_COUNT_IREF = 0;
- } TMR3_COUNT_IREF++;
-
- // Set Current Reference
+ }
+ TMR3_COUNT_IREF++;
+
+ // Set Current Reference
double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0;
double I_REF_MID = 0.0;
if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
I_REF = I_REF_MID + 1.0;
} else {
I_REF = I_REF_MID - 1.0;
- }
+ }
// double T = 1.0; // wave period
// I_REF = (5. * sin(2. * 3.1415 * (double) TMR3_COUNT_IREF / (double)TMR_FREQ_5k/ T));
// I_REF = (2.0 * sin(2. * 2. * 3.14 * (double) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.)));
-
+
if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
- CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
+ CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
}
break;
}
-
- case MODE_TEST_PWM_CONTROL:
- {
+
+ case MODE_TEST_PWM_CONTROL: {
if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
TMR3_COUNT_IREF = 0;
- } TMR3_COUNT_IREF++;
-
+ }
+ TMR3_COUNT_IREF++;
+
// Set PWM reference
double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0;
- double I_REF_MID = 0.0;
+ //double I_REF_MID = 0.0;
if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
CUR_PWM = 1000;
} else {
CUR_PWM = -1000;
- }
-
+ }
+
if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
}
break;
}
-
-
+
+
case MODE_CURRENT_CONTROL: {
cur.ref = cur.ref; // Unit : mA
CurrentControl();
break;
}
-
+
case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback
double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward
-
+
// feedback input for position control
pos.err = pos.ref - pos.sen;
double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
@@ -762,7 +652,7 @@
pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
pos.err_old = pos.err;
I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
-
+
// feedforward input for position control
double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
double K_ff = 1.3;
@@ -770,51 +660,51 @@
if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
-
+
// feedback input for position control
I_REF_FORCE_FB = 0.0;
-
+
// feedforward input for position control
I_REF_FORCE_FF = 0.0;
-
+
cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
-
+
CurrentControl();
-
+
break;
}
-
+
case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
- double T_REF = 0.0; // Torque Reference
+ //double T_REF = 0.0; // Torque Reference
double I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
double I_REF_VC = 0.; // I_REF for velocity compensation
-
+
// feedback input for position control
- double Joint_Pos_Err = 34.0-(double) pos.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
- double Joint_Vel_Err = 0.0-(double) vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
- double K_spring = 0.7;
- double D_damper = 0.02;
+ //double Joint_Pos_Err = 34.0-(double) pos.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+ //double Joint_Vel_Err = 0.0-(double) vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+ //double K_spring = 0.7;
+ //double D_damper = 0.02;
// T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
-
+
// torque feedback
torq.err = torq.ref - torq.sen;
// torq.err_diff = torq.err - torq.err_old;
// torq.err_old = torq.err;
torq.err_sum = torq.err_sum + torq.err/(double)TMR_FREQ_5k;
- I_REF_FORCE_FB = 0.001*((double)P_GAIN_JOINT_TORQUE * torq.err + (double)I_GAIN_JOINT_TORQUE * torq.err_sum);
-
+ I_REF_FORCE_FB = 0.001*((double)P_GAIN_JOINT_TORQUE * torq.err + (double)I_GAIN_JOINT_TORQUE * torq.err_sum);
+
// velocity compensation for torque control
- double Joint_Vel_Act = vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
+ double Joint_Vel_Act = vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
double K_vc = 1.5; // Velocity comp. gain
- double K_v = 0.0; // Valve gain
+ double K_v = 0.0; // Valve gain
if(Joint_Vel_Act > 0) K_v = 1.0/100.0; // open, tuning
if(Joint_Vel_Act < 0) K_v = 1.0/100.0; // close, tuning
I_REF_VC = K_vc*K_v*Joint_Vel_Act;
-
+
cur.ref = I_REF_VC + I_REF_FORCE_FB;
- // cur.ref = I_REF_FORCE_FB;
-
- double I_MAX = 10.00; // Maximum Current : 10mV
+ // cur.ref = I_REF_FORCE_FB;
+
+ double I_MAX = 10.00; // Maximum Current : 10mV
double Ka = 1.0/I_GAIN_JOINT_TORQUE;
if(cur.ref > I_MAX) {
double I_rem = cur.ref-I_MAX;
@@ -823,20 +713,20 @@
torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k;
} else if(cur.ref < -I_MAX) {
double I_rem = cur.ref-(-I_MAX);
- I_rem = Ka*I_rem;
+ I_rem = Ka*I_rem;
cur.ref = -I_MAX;
torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k;
}
-
+
CurrentControl();
-
-
+
+
/*
double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback
double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward
-
+
// feedback input for position control
pos.err = pos.ref - pos.sen;
double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
@@ -844,7 +734,7 @@
pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
pos.err_old = pos.err;
I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
-
+
// feedforward input for position control
double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
double K_ff = 1.3;
@@ -852,18 +742,18 @@
if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
-
+
// feedback input for position control
I_REF_FORCE_FB = 0.0;
-
+
// feedforward input for position control
I_REF_FORCE_FF = 0.0;
-
+
cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
-
+
CurrentControl();
*/
-
+
break;
}
@@ -871,18 +761,18 @@
// DAC Voltage reference set
if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
CUR_TORQUE_sum += torq.sen;
-
+
if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0;
CUR_TORQUE_sum = 0;
TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean);
-
+
if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-
+
ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
dac_1 = TORQUE_VREF / 3.3;
}
@@ -891,18 +781,18 @@
TMR3_COUNT_TORQUE_NULL = 0;
CUR_TORQUE_sum = 0;
CUR_TORQUE_mean = 0;
-
+
ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
-
+
//pc.printf("%f\n", TORQUE_VREF);
dac_1 = TORQUE_VREF / 3.3;
}
TMR3_COUNT_TORQUE_NULL++;
break;
- }
+ }
case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
if (TMR3_COUNT_DEADZONE == 0) {
@@ -967,9 +857,9 @@
VALVE_DEADZONE_MINUS = VALVE_CENTER;
}
V_out = 0;
-
+
ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
//spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
@@ -982,8 +872,8 @@
}
case MODE_FIND_HOME: {
-
-
+
+
////////////////////////////////////////////FIND_HOME_PWM/////////////////////////////////////////////////////
if (FLAG_FIND_HOME == true) {
cnt_findhome = 0;
@@ -1032,7 +922,7 @@
cnt_findhome = 0;
cnt_vel_findhome = 0;
}
-
+
/*
////////////////////////////////////////////FIND_HOME_CURRENT/////////////////////////////////////////////////////
if (FLAG_FIND_HOME == true) {
@@ -1042,7 +932,7 @@
Ref_Joint_Pos = pos.sen;
FLAG_FIND_HOME = false;
}
-
+
int cnt_check_enc = (TMR_FREQ_5k/500);
if(cnt_findhome%cnt_check_enc == 0){
FINDHOME_POSITION = pos.sen;
@@ -1050,19 +940,19 @@
FINDHOME_POSITION_OLD = FINDHOME_POSITION;
} cnt_findhome++;
if(cnt_findhome == 10000) cnt_findhome = 0;
-
+
if (abs(FINDHOME_VELOCITY) <= 1) {
cnt_vel_findhome = cnt_vel_findhome + 1;
} else {
cnt_vel_findhome = 0;
}
-
+
if (cnt_vel_findhome < 3*TMR_FREQ_5k) { // wait for 3sec
REFERENCE_MODE = MODE_REF_NO_ACT;
if (HOMEPOS_OFFSET > 0) Ref_Joint_Pos = Ref_Joint_Pos + 1.0;
else Ref_Joint_Pos = Ref_Joint_Pos - 1.0;
pos.err = Ref_Joint_Pos - pos.sen;
- I_REF = 0.001*((double)pos.err);
+ I_REF = 0.001*((double)pos.err);
if(I_REF>5.0) I_REF = 5.0;
if(I_REF<-5.0) I_REF = -5.0;
cur.ref = I_REF;
@@ -1076,7 +966,7 @@
Ref_Joint_Vel = 0.0;
REFERENCE_MODE = MODE_REF_COS_INC;
CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION;
-
+
FINDHOME_POSITION = 0;
FINDHOME_POSITION_OLD = 0;
FINDHOME_VELOCITY = 0;
@@ -1084,7 +974,7 @@
cnt_vel_findhome = 0;
}
*/
-
+
break;
}
@@ -1135,9 +1025,9 @@
}
}
if (fl_temp_cnt2 == 100) {
-
+
ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0));
cur_vel_sum = 0;
fl_temp_cnt = 0;
@@ -1180,7 +1070,7 @@
if (PRES_A_VREF < 0) PRES_A_VREF = 0;
if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3;
if (PRES_B_VREF < 0) PRES_B_VREF = 0;
-
+
dac_1 = PRES_A_VREF / 3.3;
dac_2 = PRES_B_VREF / 3.3;
}
@@ -1193,7 +1083,7 @@
CUR_PRES_B_mean = 0;
ROM_RESET_DATA();
-
+
dac_1 = PRES_A_VREF / 3.3;
dac_2 = PRES_B_VREF / 3.3;
//pc.printf("nulling end");
@@ -1225,7 +1115,7 @@
CUR_PRES_B_sum = 0;
CUR_PRES_A_mean = 0;
CUR_PRES_B_mean = 0;
-
+
ROM_RESET_DATA();
//spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
@@ -1273,9 +1163,9 @@
VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
for(i=0; i<25; i++) {
VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
-
+
//ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i]));
if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
VALVE_MAX_POS = VALVE_POS_AVG[i];
@@ -1285,9 +1175,9 @@
VALVE_POS_AVG_OLD = VALVE_MIN_POS;
}
}
-
+
ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS));
//spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS));
CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
@@ -1374,7 +1264,7 @@
//CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM));
}
- }else {
+ } else {
if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
@@ -1468,9 +1358,11 @@
SECOND_DZ = valve_pos.ref;
DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
first_check = 0;
-
+ VALVE_DEADZONE_MINUS = FIRST_DZ;
+ VALVE_DEADZONE_PLUS = SECOND_DZ;
+
ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
//spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
//spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
@@ -1566,14 +1458,10 @@
DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
first_check = 0;
VALVE_DEADZONE_MINUS = FIRST_DZ;
-
- ROM_RESET_DATA();
-
//spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
VALVE_DEADZONE_PLUS = SECOND_DZ;
-
ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
//spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
@@ -1616,15 +1504,15 @@
V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
} else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
data_num = 0;
- valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER;
+ valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
START_POS = pos.sen;
} else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k)) {
- valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER;
+ valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
data_num = data_num + 1;
- if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen) > 20000) {
+ if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen) > 20000.0) {
FINAL_POS = pos.sen;
one_period_end = 1;
}
@@ -1640,23 +1528,23 @@
min_check = 1;
}
JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
-
- ROM_RESET_DATA();
-
+
+ //ROM_RESET_DATA();
+
//spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
//spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF));
VALVE_FR_timer = 0;
one_period_end = 0;
ID_index= ID_index +1;
// CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index));
+ V_out = 0.0;
}
if(max_check == 1 && min_check == 1) {
-
- ROM_RESET_DATA();
-
+
//spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
VALVE_POS_NUM = ID_index;
+ ROM_RESET_DATA();
ID_index = 0;
first_check = 0;
VALVE_FR_timer = 0;
@@ -1704,14 +1592,13 @@
//CAN ----------------------------------------------------------------------
//if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
- if (TMR3_COUNT_CAN_TX % 10 == 0) {
+ if (TMR3_COUNT_CAN_TX % 1000 == 0) {
if (flag_data_request[0] == HIGH) {
//position+velocity
CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
//CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
//CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
-
//pc.printf("can good");
// CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
}
@@ -1721,6 +1608,7 @@
//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
CAN_TX_TORQUE((int16_t) (cur.sen));
+ //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
// CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
}
@@ -1737,7 +1625,7 @@
if (flag_data_request[3] == HIGH) {
//PWM
CAN_TX_PWM((int16_t) CUR_PWM);
- // CAN_TX_PWM((int16_t) (CONTROL_MODE));
+ //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
// CAN_TX_PWM((int16_t) cnt_vel_findhome);
// CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
// CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
@@ -1762,9 +1650,9 @@
}
TMR3_COUNT_CAN_TX++;
-
-
-
+
+
+
/*******************************************************
*** Valve Control
@@ -1787,8 +1675,8 @@
// Saturation of output voltage to 5.0V
// if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
// else if (PWM_out < -0.41667) PWM_out=-0.41667;
-
- // Saturation of output voltage to 12.0V
+
+ // Saturation of output voltage to 12.0V
if(PWM_out > 1.0) PWM_out=1.0;
else if (PWM_out < -1.0) PWM_out=-1.0;
@@ -1819,7 +1707,7 @@
// can.write(msg);
// }
- if((CNT_TMR3%5000)==0) {
+ //if((CNT_TMR3%5000)==0) {
// if(LED==1) {
// LED=0;
// } else
@@ -1828,73 +1716,18 @@
// pc.printf("A %f\n", (double) pres_A.sen);
// CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[j]);
// j++;
- }
+ //}
/*******************************************************
*** Timer Counting & etc.
********************************************************/
- CNT_TMR3++;
+ //CNT_TMR3++;
+
}
-
TIM3->SR = 0x0; // reset the status register
}
-/*
-void ValveControl(unsigned int ControlMode)
-{
- switch (ControlMode) {
- case MODE_NO_ACT: // 0
- V_out = 0.0;
- break;
- case MODE_VALVE_OPEN_LOOP: // 1
- V_out = Vout.ref;
- break;
- case MODE_VALVE_POSITION_CONTROL: // 2
- CurrentControl();
- break;
- case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3
- V_out = 0.0;
- break;
- case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4
- double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
- double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
-
- // feedback input for position control
- pos.err = pos.ref - pos.sen;
- double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz
- double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
- pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff;
- pos.err_old = pos.err;
- I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
-
- // feedforward input for position control
- double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
- double K_ff = 1.3;
- double K_v = 0.0;
- if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
- if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
- I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref;
-
- cur.ref = I_REF_POS_FF + I_REF_POS_FB;
- break;
- case MODE_TEST_CURRENT_CONTROL: // 9
- V_out = 0.0;
- break;
- case MODE_TEST_PWM_CONTROL: // 10
- V_out = 0.0;
- break;
- case MODE_FIND_HOME: // 22
- V_out = 0.0;
- break;
- default:
- V_out = 0.0;
- break;
-
- }
-}
-*/
-
void CurrentControl()
{
cur.err = cur.ref - cur.sen;
--- a/setting.h Wed Sep 04 11:02:57 2019 +0000 +++ b/setting.h Mon Sep 09 06:29:56 2019 +0000 @@ -6,8 +6,8 @@ #define PIN_W PB_6 //#define PWM_ARR 0x465 // loop 80k, pwm 40k //#define PWM_ARR 0x8CA // loop 40k, pwm 20k -#define PWM_ARR 0x1194 // loop 20k, pwm 10k -//#define PWM_ARR 0x2328 // loop 10k, pwm 5k +//#define PWM_ARR 0x1194 // loop 20k, pwm 10k +#define PWM_ARR 0x2328 // loop 10k, pwm 5k #define TMR3_COUNT 0x4650 // loop 5k #define FREQ_5k 5000.0 @@ -19,6 +19,23 @@ #define DT_20k 0.00005 #define DT_40k 0.000025 +//extern int CID_RX_CMD; +//extern int CID_RX_REF_POSITION; +//extern int CID_RX_REF_TORQUE; +//extern int CID_RX_REF_PRES_DIFF; +//extern int CID_RX_REF_VOUT; +//extern int CID_RX_REF_VALVE_POSITION; +//extern int CID_RX_REF_CURRENT; +// +//extern int CID_TX_INFO; +//extern int CID_TX_POSITION; +//extern int CID_TX_TORQUE; +//extern int CID_TX_PRES; +//extern int CID_TX_VOUT; +//extern int CID_TX_VALVE_POSITION; + + + extern DigitalOut check; extern DigitalOut check_2; extern AnalogOut dac_1; @@ -286,13 +303,12 @@ extern int VALVE_POS_TMP; extern int JOINT_VEL_TMP; extern int DDV_POS_AVG ; -extern int VALVE_POS_AVG[100]; +extern int VALVE_POS_AVG[50]; extern int VALVE_POS_AVG_OLD; extern int data_num; extern int ID_index; extern int DZ_index; -extern int ID_index_array[100]; -extern int FR_index_array[100]; +extern int ID_index_array[50]; extern int first_check; extern double init_time; extern int VEL_POINT; @@ -306,7 +322,8 @@ extern int one_period_end; extern double Ref_Vel_Test; extern long TMR2_FOR_SLOW_LOGGING; -extern int velcount; +//extern int velcount; +extern int Ref_Valve_Pos_FF_CAN; extern char max_check; extern char min_check; @@ -363,3 +380,5 @@ extern int TMR3_COUNT_IREF; extern double CUR_CURRENT; extern double u_CUR[3]; + +//extern int Ref_Valve_Pos_FF_CAN; \ No newline at end of file