Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Learning

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

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

CAN/function_CAN.cpp Show annotated file Show diff for this revision Revisions of this file
SPI_EEP_ENC/SPI_EEP_ENC.cpp Show annotated file Show diff for this revision Revisions of this file
function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- 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;
         }
 

--- 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

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

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