Sungwoo Kim
/
HydraulicControlBoard_Learning
for learning
Revision 256:7c8cc8b56b88, committed 2021-03-03
- Comitter:
- Lightvalve
- Date:
- Wed Mar 03 12:23:13 2021 +0000
- Parent:
- 255:aa424a9ca332
- Commit message:
- 210303
Changed in this revision
diff -r aa424a9ca332 -r 7c8cc8b56b88 CAN/function_CAN.cpp --- a/CAN/function_CAN.cpp Mon Feb 22 14:09:13 2021 +0000 +++ b/CAN/function_CAN.cpp Wed Mar 03 12:23:13 2021 +0000 @@ -137,12 +137,6 @@ case CRX_SET_DATA_REQUEST: { int request_type = msg.data[2]; flag_data_request[request_type] = msg.data[1]; - //pc.printf("can middle %d\n", request_type); - -// if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC - //if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3; -// if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC - //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3; // Position, Velocity, and Torque (ID:1200) if (flag_data_request[0] == HIGH) { @@ -154,8 +148,7 @@ } } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator if (SENSING_MODE == 0) { -// CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); - CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (pos.ref*0.001f)); + CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); } else if (SENSING_MODE == 1) { CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); } @@ -173,8 +166,8 @@ double t_value_ref = 0.0f; if (CURRENT_CONTROL_MODE) { - t_value = cur.sen; - t_value_ref = I_REF_fil; + t_value = cur.sen * 1000.0f; //Pulse + t_value_ref = I_REF_fil * 1000.0f; //Pulse } else { if(value>=(float) VALVE_CENTER) { t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); @@ -189,8 +182,7 @@ } } -// CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 - CAN_TX_PRES((int16_t) (VALVE_DEADZONE_PLUS), (int16_t) (t_value_ref)); // 1400 + CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 } // //If it doesn't rest, below can can not work. @@ -851,7 +843,7 @@ pos.ref = (double)temp_pos * 1.0f; vel.ref = (double)temp_vel * 10.0f; } else { //Linear Actuator - pos.ref = (double)temp_pos * 10.0f; + pos.ref = (double)temp_pos * 16.0f; vel.ref = (double)temp_vel * 256.0f; }
diff -r aa424a9ca332 -r 7c8cc8b56b88 SPI_EEP_ENC/SPI_EEP_ENC.cpp --- a/SPI_EEP_ENC/SPI_EEP_ENC.cpp Mon Feb 22 14:09:13 2021 +0000 +++ b/SPI_EEP_ENC/SPI_EEP_ENC.cpp Wed Mar 03 12:23:13 2021 +0000 @@ -82,7 +82,7 @@ // ENCODER void spi_enc_set_clear(void){ unsigned int temp; - enc_cs = 1; //sw add +// enc_cs = 1; //sw add enc_cs = 0; temp = enc.write(0b00100000); enc_cs = 1; @@ -92,14 +92,14 @@ unsigned int temp, i, temp1, temp2; // write MDR0 -> 0b11 -> x4 quadrature count mode - enc_cs = 1; //sw add +// enc_cs = 1; //sw add enc_cs = 0; temp = enc.write(0b10001000); // WR + MDR0 temp = enc.write(0b00000011); // quadratue mode enc_cs = 1; // write MDR1 -> 0b10 -> 2-byte counter mode - for(i=0;i<100;i++); + for(i=0;i<10000;i++); enc_cs = 0; temp = enc.write(0b10010000); // WR + MDR1 //temp = enc.write(0b00000010); // 2 byte mode @@ -114,7 +114,7 @@ int spi_enc_read(void){ //for(t_i=0;t_i<100;t_i++); unsigned int t_dummy, t_b1, t_b2, t_b3, t_b4, t_i; - enc_cs = 1; //sw add +// enc_cs = 1; //sw add enc_cs = 0; t_dummy = enc.write(0b01100000); // Read Commend t_b1 = enc.write(0x00); // Dummy data for clock
diff -r aa424a9ca332 -r 7c8cc8b56b88 function_utilities/function_utilities.cpp --- a/function_utilities/function_utilities.cpp Mon Feb 22 14:09:13 2021 +0000 +++ b/function_utilities/function_utilities.cpp Wed Mar 03 12:23:13 2021 +0000 @@ -625,8 +625,8 @@ //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f; VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM); - K_SPRING = spi_eeprom_read(RID_K_SPRING); - D_DAMPER = spi_eeprom_read(RID_D_DAMPER); +// K_SPRING = spi_eeprom_read(RID_K_SPRING); +// D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
diff -r aa424a9ca332 -r 7c8cc8b56b88 main.cpp --- a/main.cpp Mon Feb 22 14:09:13 2021 +0000 +++ b/main.cpp Wed Mar 03 12:23:13 2021 +0000 @@ -891,12 +891,17 @@ enc_cs = 1; //sw add enc.format(8,0); - enc.frequency(10000000); //10M + enc.frequency(5000000); //10M enc_cs = 0; //sw add make_delay(); - -// spi_eeprom_write(RID_BNO, (int16_t) 0); + + // spi _ enc + spi_enc_set_init(); + make_delay(); + + //Inital BNO setting +// spi_eeprom_write(RID_BNO, (int16_t) 1); // make_delay(); //rom @@ -911,6 +916,14 @@ Init_PWM(); TIM4->CR1 ^= TIM_CR1_UDIS; make_delay(); + + // CAN + can.attach(&CAN_RX_HANDLER); + CAN_ID_INIT(); + make_delay(); + + //can.reset(); + can.filter(msg.id, 0xFFFFF000, CANStandard); // TMR3 init Init_TMR3(); @@ -922,22 +935,24 @@ // TIM2->CR1 ^= TIM_CR1_UDIS; // make_delay(); - // CAN - can.attach(&CAN_RX_HANDLER); - CAN_ID_INIT(); - make_delay(); +// // CAN +// can.attach(&CAN_RX_HANDLER); +// CAN_ID_INIT(); +// make_delay(); + + //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); //NVIC_SetPriority(TIM2_IRQn, 3); NVIC_SetPriority(TIM4_IRQn, 3); - //can.reset(); - can.filter(msg.id, 0xFFFFF000, CANStandard); - - // spi _ enc - spi_enc_set_init(); - make_delay(); +// //can.reset(); +// can.filter(msg.id, 0xFFFFF000, CANStandard); + + // // spi _ enc +// spi_enc_set_init(); +// make_delay(); //DAC init if (SENSING_MODE == 0) { @@ -2380,16 +2395,15 @@ double torq_ref = 0.0f; pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] - vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] + vel.err = (vel.ref - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] //K & D Low Pass Filter float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D); D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D); - -// torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N] - torq_ref = torq.ref; + torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N] +// torq_ref = torq.ref; // torque feedback torq.err = torq_ref - torq.sen; //[N] @@ -2839,7 +2853,7 @@ // } // } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator // if (SENSING_MODE == 0) { -// CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); +// CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); // } else if (SENSING_MODE == 1) { // CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); // }