Diff: CAN/function_CAN.cpp

Revision:

224:985dba42f261

Parent:

209:ebc69d6ee6f1

Child:

225:9c0becc196ba

--- a/CAN/function_CAN.cpp	Mon Dec 28 12:10:50 2020 +0000
+++ b/CAN/function_CAN.cpp	Mon Dec 28 14:27:11 2020 +0000
@@ -62,8 +62,6 @@
         }
         case CRX_SET_BNO: {
             BNO = (int16_t) msg.data[1];
-
-            //ROM_RESET_DATA();
             spi_eeprom_write(RID_BNO, (int16_t) BNO);
             CAN_ID_INIT(); // can id init
             break;
@@ -77,7 +75,6 @@
             SENSING_MODE = (uint8_t) msg.data[2];
             CURRENT_CONTROL_MODE = (uint8_t) msg.data[3];
             FLAG_VALVE_DEADZONE = (uint8_t) msg.data[4];
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
             spi_eeprom_write(RID_SENSING_MODE, (int16_t) SENSING_MODE);
             spi_eeprom_write(RID_CURRENT_CONTROL_MODE, (int16_t) CURRENT_CONTROL_MODE);
@@ -113,7 +110,6 @@
 
         case CRX_SET_CAN_FREQ: {
             CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
-            ROM_RESET_DATA();
             spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
             break;
         }
@@ -125,7 +121,6 @@
         }
 
         case CRX_SET_CONTROL_MODE: {
-            //CONTROL_MODE = (int16_t) (msg.data[1]);
             CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
             if (CONTROL_MODE == 22) {    //MODE_FIND_HOME
                 FLAG_FIND_HOME = true;
@@ -136,7 +131,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;
@@ -159,8 +153,6 @@
             else
                 DIR_JOINT_ENC = -1;
 
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
 
             break;
@@ -179,8 +171,6 @@
             else
                 DIR_VALVE = -1;
 
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
 
             break;
@@ -199,8 +189,6 @@
             else
                 DIR_VALVE_ENC = -1;
 
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
 
             break;
@@ -215,7 +203,6 @@
         case CRX_SET_VOLTAGE_SUPPLY: {
             SUPPLY_VOLTAGE = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
 
-//            ROM_RESET_DATA();
 
             spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.0f));
 
@@ -231,8 +218,6 @@
         case CRX_SET_VOLTAGE_VALVE: {
             VALVE_VOLTAGE_LIMIT = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
 
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f));
 
 
@@ -240,7 +225,6 @@
         }
 
         case CRX_SET_HOMEPOS: {
-            //CONTROL_MODE = 22;
             CONTROL_UTILITY_MODE = 22;
             break;
         }
@@ -257,7 +241,6 @@
                 I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
                 D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
 
-//                ROM_RESET_DATA();
                 spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
                 spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
                 spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
@@ -267,7 +250,6 @@
                 I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
                 D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
 
-//                ROM_RESET_DATA();
                 spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
                 spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
                 spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
@@ -277,7 +259,6 @@
                 I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
                 D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
 
-//                ROM_RESET_DATA();
                 spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
                 spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
                 spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
@@ -286,7 +267,6 @@
                 K_SPRING = (float) (((float) ((int16_t) (msg.data[2] | msg.data[3] << 8))) * 0.1f);
                 D_DAMPER = (float) (((float) ((int16_t) (msg.data[4] | msg.data[5] << 8))) * 0.01f);
 
-//                ROM_RESET_DATA();     //For Real-time changing
 //                spi_eeprom_write(RID_K_SPRING, (int16_t) K_SPRING);
 //                spi_eeprom_write(RID_D_DAMPER, (int16_t) D_DAMPER);
 
@@ -295,7 +275,6 @@
                 I_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[4] | msg.data[5] << 8);
                 D_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[6] | msg.data[7] << 8);
 
-//                ROM_RESET_DATA();
             }
 
             break;
@@ -312,7 +291,6 @@
             VALVE_DEADZONE_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
             VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
 
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
             spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
             spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
@@ -329,7 +307,6 @@
         case CRX_SET_VELOCITY_COMP_GAIN: {
             VELOCITY_COMP_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
 
-//            ROM_RESET_DATA();
 
             spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
 
@@ -345,7 +322,6 @@
         case CRX_SET_COMPLIANCE_GAIN: {
             COMPLIANCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
 
-//            ROM_RESET_DATA();
 
             spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
 
@@ -361,8 +337,6 @@
         case CRX_SET_VALVE_FF: {
             VALVE_FF = (int16_t) (msg.data[1] | msg.data[2] << 8);
 
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
 
             break;
@@ -376,9 +350,6 @@
 
         case CRX_SET_BULK_MODULUS: {
             BULK_MODULUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
-
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
 
             break;
@@ -394,8 +365,6 @@
             CHAMBER_VOLUME_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
             CHAMBER_VOLUME_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
 
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
             spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
 
@@ -414,8 +383,6 @@
             PISTON_AREA_alpha = (double)PISTON_AREA_A/(double)PISTON_AREA_B;
             alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
 
-//            ROM_RESET_DATA();
-
             spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
             spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
             break;
@@ -433,7 +400,6 @@
         case CRX_SET_PRES: {
             PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8);
             PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8);
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
             spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
 
@@ -450,7 +416,6 @@
         case CRX_SET_ENC_LIMIT: {
             ENC_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
             ENC_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
             spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
 
@@ -464,7 +429,6 @@
 
         case CRX_SET_STROKE: {
             STROKE = (int16_t) (msg.data[1] | msg.data[2] << 8);
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
 
             break;
@@ -479,7 +443,6 @@
         case CRX_SET_VALVE_LIMIT: {
             VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8);
             VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8);
-//            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);
@@ -495,7 +458,6 @@
 
         case CRX_SET_ENC_PULSE_PER_POSITION: {
             ENC_PULSE_PER_POSITION = (int16_t) (msg.data[1] | msg.data[2] << 8);
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) (ENC_PULSE_PER_POSITION));
 
             break;
@@ -509,7 +471,6 @@
 
         case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
             TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.0001f);
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
 
             break;
@@ -524,7 +485,6 @@
         case CRX_SET_PRES_SENSOR_PULSE_PER_PRES: {
             PRES_SENSOR_A_PULSE_PER_BAR = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) * 0.01f;
             PRES_SENSOR_B_PULSE_PER_BAR = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) * 0.01f;
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
             spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
 
@@ -539,7 +499,6 @@
 
         case CRX_SET_FRICTION: {
             FRICTION = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.0f));
 
             break;
@@ -556,7 +515,6 @@
             VALVE_GAIN_LPM_PER_V[4] = (double) msg.data[3] / 50.0f;
             VALVE_GAIN_LPM_PER_V[6] = (double) msg.data[4] / 50.0f;
             VALVE_GAIN_LPM_PER_V[8] = (double) msg.data[5] / 50.0f;
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
             spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
             spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
@@ -577,7 +535,6 @@
             VALVE_GAIN_LPM_PER_V[5] = (double) msg.data[3] / 50.0f;
             VALVE_GAIN_LPM_PER_V[7] = (double) msg.data[4] / 50.0f;
             VALVE_GAIN_LPM_PER_V[9] = (double) msg.data[5] / 50.0f;
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
             spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
             spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
@@ -628,7 +585,6 @@
         }
         case CRX_SET_HOMEPOS_OFFSET: {
             HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8);
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
             break;
         }
@@ -639,7 +595,6 @@
         }
         case CRX_SET_HOMEPOS_VALVE_OPENING: {
             HOMEPOS_VALVE_OPENING = (int16_t) (msg.data[1] | msg.data[2] << 8);
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
             break;
         }
@@ -660,13 +615,11 @@
         }
 
         case CRX_SET_ROM: {
-            ROM_RESET_DATA();
             break;
         }
         case CRX_SET_VALVE_CENTER_OFFSET: {
             VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
             VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
 
             break;
@@ -674,7 +627,6 @@
         case CRX_SET_VALVE_DZ_MINUS_OFFSET: {
             VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
             VALVE_DEADZONE_MINUS = VALVE_DEADZONE_MINUS + VALVE_DZ_MINUS_OFFSET;
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f));
 
             break;
@@ -682,7 +634,6 @@
         case CRX_SET_VALVE_DZ_PLUS_OFFSET: {
             VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f;
             VALVE_DEADZONE_PLUS = VALVE_DEADZONE_PLUS + VALVE_DZ_PLUS_OFFSET;
-//            ROM_RESET_DATA();
             spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f));
             break;
         }
@@ -691,21 +642,13 @@
                 P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
                 I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
                 D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
-
-//                ROM_RESET_DATA();
-
                 //spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
                 //spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
                 //spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
-
-
             } else if (msg.data[1] == 1) {
                 P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
                 I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
                 D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
-
-//                ROM_RESET_DATA();
-
                 //spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
                 //spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
                 //spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
@@ -713,9 +656,6 @@
                 P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
                 I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
                 D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
-
-//                ROM_RESET_DATA();
-
                 //spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
                 //spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
                 //spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
@@ -723,7 +663,6 @@
             break;
         }
         case CRX_ASK_VALVE_MAX_MIN_POS: {
-            //CAN_TX_DDV_VALVE_DEADZONE();
             CAN_TX_DDV_VALVE_MAX_MIN_POS();
             break;
         }
@@ -789,44 +728,9 @@
 
         torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;   //N
         torq.ref_diff = torq.ref - torq.ref_old;
-        torq_dot.sen = torq.sen-totq_sen_past;
+        torq_dot.sen = torq.sen-torq_sen_past;
         torq.ref_old = torq.ref;
-        totq_sen_past = torq.sen;
-
-        ///////////////Make Data///////////////////
-        for(int i=0; i<num_array_u_past-1; i++) {
-            u_past[i] = u_past[i+1];
-        }
-        u_past[num_array_u_past-1] = output_normalized;
-
-        for(int i=0; i<num_array_x_past-1; i++) {
-            x_past[i] = x_past[i+1];
-        }
-        x_past[num_array_x_past-1] = pos.sen / ENC_PULSE_PER_POSITION;   //mm
-
-//        for(int i=0;i<num_array_x_future-1;i++){
-//            x_future[i] = x_future[i+1];
-//        }
-//        x_future[num_array_x_future-1] = pos.sen / ENC_PULSE_PER_POSITION;  //mm
-
-        for(int i=0; i<num_array_f_past-1; i++) {
-            f_past[i] = f_past[i+1];
-        }
-        f_past[num_array_f_past-1] = torq.sen;  //N
-
-        f_future[0] = torq.sen;  //N
-
-        if(flag_every_reference == 1) {
-            for(int i=1; i<num_array_f_future-1; i++) {
-                f_future[i] = 500.0f;
-            }
-            f_future[num_array_f_future-1] = 500.0f;  //N
-        } else {
-            for(int i=1; i<num_array_f_future-1; i++) {
-                f_future[i] = f_future[i+1];
-            }
-            f_future[num_array_f_future-1] = torq.ref;  //N
-        }
+        torq_sen_past = torq.sen;
 
 
     } else if(address==CID_RX_REF_VALVE_POS) {
@@ -1271,8 +1175,6 @@
     float temp_valve_deadzone_plus = 0.0f;
     float temp_ddv_center = 0.0f;
 
-    //temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
-    //temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
     temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS;
     temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS;
     temp_ddv_center = (double)VALVE_CENTER;
@@ -1356,7 +1258,6 @@
 {
     CANMessage temp_msg;
     int16_t valve_pos_vs_pwm;
-//    valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
 
     if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) {
         valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
@@ -1504,18 +1405,6 @@
     can.write(temp_msg);
 }
 
-//void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout) {
-//    CANMessage temp_msg;
-//
-//    temp_msg.id = CID_TX_TORQUE;
-//    temp_msg.len = 4;
-//    temp_msg.data[0] = (uint8_t) t_valve_pos;
-//    temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
-//    temp_msg.data[2] = (uint8_t) t_vout;
-//    temp_msg.data[3] = (uint8_t) (t_vout >> 8);
-//
-//    can.write(temp_msg);
-//}
 
 void CAN_TX_TORQUE(int16_t t_valve_pos)
 {