![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
Diff: main.cpp
- Revision:
- 256:7c8cc8b56b88
- Parent:
- 255:aa424a9ca332
--- 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)); // }