Diff: CAN/function_CAN.cpp

Revision:

36:a46e63505ed8

Parent:

35:34ce7b0347b8

Child:

38:118df027d851

diff -r 34ce7b0347b8 -r a46e63505ed8 CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp	Wed Nov 20 12:55:18 2019 +0000
+++ b/CAN/function_CAN.cpp	Fri Nov 22 08:03:52 2019 +0000
@@ -659,6 +659,43 @@
             ROM_RESET_DATA();
             break;
         }
+        case CRX_SET_PID_GAIN_OPP: {
+            if (msg.data[1] == 0) {
+                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);
+            } else if (msg.data[1] == 2) {
+                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);
+            }
+
+            break;
+        }
     default:
         break;
     }