Diff: main.cpp

Revision:

23:59218d4a256d

Parent:

21:e5f1a43ea6f9

Child:

24:ef6e1092e9e6

diff -r 966b35691d69 -r 59218d4a256d main.cpp
--- a/main.cpp	Mon Sep 09 10:41:16 2019 +0000
+++ b/main.cpp	Tue Sep 10 09:38:26 2019 +0000
@@ -181,16 +181,23 @@
     make_delay();
 
     // TMR5 init
-    Init_TMR5();
-    TIM5->CR1 ^= TIM_CR1_UDIS;
+    Init_TMR2();
+    TIM2->CR1 ^= TIM_CR1_UDIS;
     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, 4);
+    
+    //can.reset();
     can.filter(msg.id, 0xFFFFF000, CANStandard);
-
+    
     // spi _ enc
     spi_enc_set_init();
     make_delay();
@@ -270,6 +277,34 @@
     } else if(REF_VALVE_POS < VALVE_MIN_POS) {
         REF_VALVE_POS = VALVE_MIN_POS;
     }
+    
+    
+//    if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0])
+//    {
+//        if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[1]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[1] - VALVE_POS_VS_PWM[0]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[0]);
+//        }else if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[3]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[3] - VALVE_POS_VS_PWM[1]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[1]) + 3000.0* (double) ID_index_array[1];
+//        }else if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[5]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[5] - VALVE_POS_VS_PWM[3]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[3]) + 3000.0* (double) ID_index_array[3];
+//        }else if(REF_VALVE_POS <=  VALVE_POS_VS_PWM[7]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[7] - VALVE_POS_VS_PWM[5]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[5]) + 3000.0* (double) ID_index_array[5];
+//        }else
+//            VALVE_PWM_RAW_FF = 12000.0;
+//    }
+//    else
+//    {
+//        if(REF_VALVE_POS >= VALVE_POS_VS_PWM[2]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[0] - VALVE_POS_VS_PWM[2]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[2]) + 3000.0* (double) ID_index_array[2];
+//        }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[4]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[2] - VALVE_POS_VS_PWM[4]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[4]) + 3000.0* (double) ID_index_array[4];
+//        }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[6]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[4] - VALVE_POS_VS_PWM[6]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[6]) + 3000.0* (double) ID_index_array[6];
+//        }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[8]) {
+//            VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[6] - VALVE_POS_VS_PWM[8]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[8]) + 3000.0* (double) ID_index_array[8];
+//        }else
+//            VALVE_PWM_RAW_FF = -12000.0;
+//    }
 
     for(i=0; i<24; i++) {
         if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <=  max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) {
@@ -334,6 +369,7 @@
 double DT_TMR4 = (double)DT_10k;
 extern "C" void TIM4_IRQHandler(void)
 {
+
     if (TIM4->SR & TIM_SR_UIF ) {
 
         /*******************************************************
@@ -388,8 +424,10 @@
 
 int j =0;
 //unsigned long CNT_TMR3 = 0;
-double FREQ_TMR3 = (double)FREQ_5k;
-double DT_TMR3 = (double)DT_5k;
+//double FREQ_TMR3 = (double)FREQ_5k;
+double FREQ_TMR3 = (double)FREQ_1k;
+//double DT_TMR3 = (double)DT_5k;
+double DT_TMR3 = (double)DT_1k;
 extern "C" void TIM3_IRQHandler(void)
 {
     if (TIM3->SR & TIM_SR_UIF ) {
@@ -454,6 +492,8 @@
             }
 
             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_FORCE_FB = 0.0; // Valve Position by Force Feedback
@@ -488,9 +528,11 @@
                 //            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 = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+                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;
             }
 
@@ -608,7 +650,7 @@
 //              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;
             }
@@ -629,7 +671,7 @@
                 }
 
                 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
+                    //CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas
                 }
                 break;
             }
@@ -1166,10 +1208,6 @@
                     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];
                             VALVE_POS_AVG_OLD = VALVE_MAX_POS;
@@ -1178,12 +1216,7 @@
                             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));
                     ID_index = 0;
                     CONTROL_MODE = MODE_NO_ACT;
                 }
@@ -1216,7 +1249,7 @@
                         V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
                         DDV_POS_AVG = VALVE_POS_TMP / data_num;
                         START_POS = pos.sen;
-                        CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num));
+                        //CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num));
                         VALVE_POS_TMP = 0;
                         data_num = 0;
 
@@ -1374,7 +1407,7 @@
                                 DZ_index = 1;
                             }
 
-                            CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index));
+                            //CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index));
                         }
                     } else if(DZ_case == 0 && DZ_NUM ==1) {
                         if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
@@ -1467,7 +1500,7 @@
 
                                 //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);
+                                //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
                                 CONTROL_MODE = MODE_NO_ACT;
                                 DZ_index = 1;
                             }
@@ -1484,7 +1517,7 @@
                 if(first_check == 0) {
                     if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
                         V_out = VALVE_VOLTAGE_LIMIT;
-                        CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
+                        //CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
                     } else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
                         V_out = VALVE_VOLTAGE_LIMIT;
                         pos_plus_end = pos.sen;
@@ -1588,88 +1621,6 @@
             V_out = V_out;
         }
 
-        //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
-        //PWM_out = CUR_PWM;
-
-        /*
-
-                //CAN ----------------------------------------------------------------------
-                //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 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);
-                    }
-
-                    if (flag_data_request[1] == HIGH) {
-                        //torque
-                        //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);
-                    }
-
-                    if (flag_data_request[2] == HIGH) {
-                        //pressure A and B
-                        //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
-                        CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
-                        //                        CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
-                        //            CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
-                        //            CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
-
-                    }
-
-                    if (flag_data_request[3] == HIGH) {
-                        //PWM
-                        CAN_TX_PWM((int16_t) CUR_PWM);
-                        //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.));
-
-                    }
-
-                    if (flag_data_request[4] == HIGH) {
-                        //valve position
-                        CAN_TX_VALVE_POSITION((int16_t) (value));
-                        //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
-                        //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
-
-
-                        //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
-                        //                        CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));
-                        //            CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
-                        //            CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
-                        //            CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
-                    }
-
-                    TMR3_COUNT_CAN_TX = 0;
-
-                }
-                TMR3_COUNT_CAN_TX++;
-
-        */
-
-
-        /*******************************************************
-        ***     Valve Control
-        ********************************************************/
-        //ValveControl(CONTROL_MODE);
-
-//      double t = (double)CNT_TMR4*DT_TMR4;
-//      double T = 1.0;
-//      V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty)
-//      if(V_out > 0.0) V_out = 1000.0;
-//      else if(V_out < 0.0) V_out = -1000.0;
-
-        //V_out = 2000.0 * sin(2 * 3.14159 * (double) CNT_TMR3 / (double) 5000.0);
-
         /*******************************************************
         ***     PWM
         ********************************************************/
@@ -1695,37 +1646,6 @@
         TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
         TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
 
-        /*******************************************************
-        ***     Data Send (CAN) & Print out (UART)
-        ********************************************************/
-//        if((CNT_TMR3%1000)==0){
-//            msg.id = 50;
-//            msg.len = 4;
-//            int temp_CUR = (int)(cur.sen*1000.0);
-//            msg.data[0]=0x00FF&temp_CUR;
-//            msg.data[1]=0x00FF&(temp_CUR>>8);
-//            int temp_PWM = (int)(V_out);
-//            msg.data[2]=0x00FF&temp_PWM;
-//            msg.data[3]=0x00FF&(temp_PWM>>8);
-//            can.write(msg);
-//        }
-
-        //if((CNT_TMR3%5000)==0) {
-//            if(LED==1) {
-//                LED=0;
-//            } else
-//                LED = 1;
-//            LED != LED;
-//            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++;
-
     }
     TIM3->SR = 0x0;  // reset the status register
 
@@ -1736,9 +1656,9 @@
 //unsigned long CNT_TMR5 = 0;
 //double FREQ_TMR5 = (double)FREQ_500;
 //double DT_TMR5 = (double)DT_500;
-extern "C" void TIM5_IRQHandler(void)
+extern "C" void TIM2_IRQHandler(void)
 {
-    if (TIM5->SR & TIM_SR_UIF ) {
+    if (TIM2->SR & TIM_SR_UIF ) {
 
         //CAN ----------------------------------------------------------------------
         if (flag_data_request[0] == HIGH) {
@@ -1794,7 +1714,7 @@
         }
 
     }
-    TIM5->SR = 0x0;  // reset the status register
+    TIM2->SR = 0x0;  // reset the status register
 }
 
 
@@ -1831,3 +1751,4 @@
         cur.err_int = cur.err_int - V_rem*DT_5k;
     }
 }
+