Diff: main.cpp

Revision:

256:7c8cc8b56b88

Parent:

255:aa424a9ca332

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));
 //                            }