asdgas
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 61:8d36b4042096
- Parent:
- 60:64181f1d3e60
- Child:
- 62:456f87e3124e
--- a/main.cpp Tue Mar 31 05:12:37 2020 +0000 +++ b/main.cpp Mon Apr 13 11:55:09 2020 +0000 @@ -131,13 +131,15 @@ MODE_SYSTEM_ID, //33 }; + + int main() { /********************************* *** Initialization *********************************/ LED = 1; - //pc.baud(9600); + pc.baud(9600); // i2c init i2c.frequency(400 * 1000); // 0.4 mHz @@ -215,6 +217,7 @@ *************************************/ 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; @@ -225,6 +228,7 @@ timer_while = 0; } timer_while ++; + //pc.printf("%d\n", value); } } @@ -359,8 +363,8 @@ if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) { ENC_UPDATE(); } - - ADC1->CR2 |= 0x40000000; + + ADC1->CR2 |= 0x40000000; if (SENSING_MODE == 0) { // Torque Sensing (0~210)bar ============================================= float pres_A_new = (((float) ADC1->DR) - 2047.5f); @@ -384,7 +388,7 @@ torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N } } - + // //Pressure sensor A // ADC1->CR2 |= 0x40000000; // adc _ 12bit // //while((ADC1->SR & 0b10)); @@ -616,16 +620,16 @@ //REFERENCE_MODE = MODE_REF_NO_ACT; if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 2.0f; else pos.ref = pos.ref - 2.0f; - + // pos.err = pos.ref_home_pos - pos.sen; // float VALVE_POS_RAW_POS_FB = 0.0f; // VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f; // valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; // VALVE_POS_CONTROL(valve_pos.ref); - + CONTROL_MODE = MODE_JOINT_CONTROL; alpha_trans = 0.0f; - + } else { ENC_SET(HOMEPOS_OFFSET); @@ -645,7 +649,7 @@ vel.ref = 0.0f; // input for position control - + CONTROL_MODE = MODE_JOINT_CONTROL; alpha_trans = 0.0f; // pos.err = pos.ref - (float)pos.sen; @@ -921,7 +925,7 @@ } else { DZ_case = 0; } - + CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6)); first_check = 1; @@ -986,7 +990,7 @@ valve_pos.ref = VALVE_MAX_POS; } VALVE_POS_CONTROL(valve_pos.ref); - + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Vout.ref = 0.0f; } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { @@ -1031,7 +1035,7 @@ valve_pos.ref = VALVE_MAX_POS; } VALVE_POS_CONTROL(valve_pos.ref); - + } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; @@ -1074,7 +1078,7 @@ } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) { Ref_Valve_Pos_Old = valve_pos.ref; FINAL_POS = pos.sen; - + if((FINAL_POS - START_POS)>100) { DZ_DIRECTION = 1; } else if((FINAL_POS - START_POS)<-100) { @@ -1091,7 +1095,7 @@ first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; VALVE_DEADZONE_PLUS = (float) SECOND_DZ; - + ROM_RESET_DATA(); CONTROL_UTILITY_MODE = MODE_NO_ACT; @@ -1296,9 +1300,9 @@ torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; } } - + VALVE_POS_CONTROL(valve_pos.ref); - + Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err); V_out = (float) Vout.ref;