Diff: CAN/function_CAN.cpp

Revision:

233:0a37cdd59651

Parent:

209:ebc69d6ee6f1

Child:

235:4be664734d36

--- a/CAN/function_CAN.cpp	Wed Dec 30 11:14:47 2020 +0000
+++ b/CAN/function_CAN.cpp	Thu Dec 31 15:08:02 2020 +0000
@@ -30,6 +30,7 @@
 extern float f_future[];
 extern float input_NN[];
 
+
 /*******************************************************************************
  * CAN functions
  ******************************************************************************/
@@ -142,6 +143,63 @@
             //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) {
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+                    if (SENSING_MODE == 0) {
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
+                    } else if (SENSING_MODE == 1) {
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+                    }
+                } 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)));
+                    } 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));
+                    }
+                }
+            }
+            if (flag_data_request[1] == HIGH) {
+                CAN_TX_TORQUE((int16_t) (return_G[0]*100.0f)); //1300
+            }
+
+
+            if (flag_data_request[2] == HIGH) {
+                double t_value = 0.0f;
+                if(value>=(float) VALVE_CENTER) {
+                    t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                } else {
+                    t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                }
+                double t_value_ref = 0.0f;
+                if(valve_pos.ref>=(float) VALVE_CENTER) {
+                    t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+                } else {
+                    t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+                }
+
+
+                CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
+            }
+
+            //If it doesn't rest, below can can not work.
+            for (int can_rest = 0; can_rest < 10000; can_rest++) {
+                ;
+            }
+
+            if (flag_data_request[3] == HIGH) {
+                //PWM
+                CAN_TX_PWM((int16_t) (torq.ref)); //1500
+//                CAN_TX_PWM((int16_t) (f_future[1])); //1500
+            }
+
+            if (flag_data_request[4] == HIGH) {
+                //valve position
+                //CAN_TX_VALVE_POSITION((int16_t) pos.sen/(float)(ENC_PULSE_PER_POSITION), (int16_t) virt_pos, (int16_t) (logging2*1000.0f), (int16_t) (logging4*1000.0f)); //1600
+                CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
+            }
+            
 
             break;
         }