Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Learning

for learning

Dependencies:   mbed FastPWM

CAN/function_CAN.cpp@256:7c8cc8b56b88, 2021-03-03 (annotated)

Committer:
Lightvalve
Date:
Wed Mar 03 12:23:13 2021 +0000
Revision:
256:7c8cc8b56b88
Parent:
254:725d7435dfa3 
210303

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jobuuu 2:a1c0a37df760 1 #include "function_CAN.h"
Lightvalve 11:82d8768d7351 2 #include "setting.h"
Lightvalve 11:82d8768d7351 3 #include "function_utilities.h"
Lightvalve 11:82d8768d7351 4 #include "SPI_EEP_ENC.h"
Lightvalve 16:903b5a4433b4 5 #include "stm32f4xx_flash.h"
Lightvalve 16:903b5a4433b4 6 #include "FlashWriter.h"
jobuuu 2:a1c0a37df760 7
jobuuu 2:a1c0a37df760 8 // CAN ID Setting Variables
jobuuu 2:a1c0a37df760 9 int CID_RX_CMD = 100;
jobuuu 2:a1c0a37df760 10 int CID_RX_REF_POSITION = 200;
Lightvalve 49:e7bcfc244d40 11 int CID_RX_REF_VALVE_POS = 300;
Lightvalve 49:e7bcfc244d40 12 int CID_RX_REF_PWM = 400;
jobuuu 2:a1c0a37df760 13
jobuuu 2:a1c0a37df760 14 int CID_TX_INFO = 1100;
jobuuu 2:a1c0a37df760 15 int CID_TX_POSITION = 1200;
jobuuu 2:a1c0a37df760 16 int CID_TX_TORQUE = 1300;
jobuuu 2:a1c0a37df760 17 int CID_TX_PRES = 1400;
jobuuu 7:e9086c72bb22 18 int CID_TX_VOUT = 1500;
jobuuu 2:a1c0a37df760 19 int CID_TX_VALVE_POSITION = 1600;
jobuuu 2:a1c0a37df760 20
Lightvalve 12:6f2531038ea4 21 // variables
Lightvalve 12:6f2531038ea4 22 uint8_t can_index = 0;
Lightvalve 67:c2812cf26c38 23
Lightvalve 66:a8e6799dbce3 24 extern DigitalOut LED;
Lightvalve 12:6f2531038ea4 25
Lightvalve 99:7bbcb3c0fb06 26 extern float u_past[];
Lightvalve 68:328e1be06f5d 27 extern float x_past[];
Lightvalve 85:a3b46118b5cd 28 extern float x_future[];
Lightvalve 68:328e1be06f5d 29 extern float f_past[];
Lightvalve 68:328e1be06f5d 30 extern float f_future[];
Lightvalve 73:f80dc3970c99 31 extern float input_NN[];
Lightvalve 68:328e1be06f5d 32
Lightvalve 233:0a37cdd59651 33
Lightvalve 11:82d8768d7351 34 /*******************************************************************************
Lightvalve 11:82d8768d7351 35 * CAN functions
Lightvalve 11:82d8768d7351 36 ******************************************************************************/
Lightvalve 170:42c938a40313 37 void CAN_ID_INIT(void)
Lightvalve 170:42c938a40313 38 {
jobuuu 7:e9086c72bb22 39
Lightvalve 11:82d8768d7351 40 CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
Lightvalve 11:82d8768d7351 41 CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
Lightvalve 49:e7bcfc244d40 42 CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS);
Lightvalve 45:35fa6884d0c6 43 CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM);
Lightvalve 11:82d8768d7351 44
Lightvalve 11:82d8768d7351 45 CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
Lightvalve 11:82d8768d7351 46 CID_TX_POSITION = (int) (BNO + INIT_CID_TX_POSITION);
Lightvalve 11:82d8768d7351 47 CID_TX_TORQUE = (int) (BNO + INIT_CID_TX_TORQUE);
Lightvalve 11:82d8768d7351 48 CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES);
Lightvalve 11:82d8768d7351 49 CID_TX_VOUT = (int) (BNO + INIT_CID_TX_VOUT);
Lightvalve 11:82d8768d7351 50 CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION);
Lightvalve 11:82d8768d7351 51 }
jobuuu 2:a1c0a37df760 52
Lightvalve 28:2a62d73e3dd0 53 void ReadCMD(int16_t CMD)
jobuuu 2:a1c0a37df760 54 {
Lightvalve 170:42c938a40313 55 switch(CMD) {
Lightvalve 11:82d8768d7351 56 case CRX_ASK_INFO: {
Lightvalve 11:82d8768d7351 57 CAN_TX_INFO();
Lightvalve 11:82d8768d7351 58 break;
Lightvalve 11:82d8768d7351 59 }
Lightvalve 11:82d8768d7351 60 case CRX_ASK_BNO: {
Lightvalve 11:82d8768d7351 61 CAN_TX_BNO();
Lightvalve 11:82d8768d7351 62 break;
Lightvalve 11:82d8768d7351 63 }
Lightvalve 11:82d8768d7351 64 case CRX_SET_BNO: {
Lightvalve 11:82d8768d7351 65 BNO = (int16_t) msg.data[1];
Lightvalve 170:42c938a40313 66
Lightvalve 170:42c938a40313 67 //ROM_RESET_DATA();
Lightvalve 170:42c938a40313 68 spi_eeprom_write(RID_BNO, (int16_t) BNO);
Lightvalve 11:82d8768d7351 69 CAN_ID_INIT(); // can id init
Lightvalve 11:82d8768d7351 70 break;
Lightvalve 11:82d8768d7351 71 }
Lightvalve 11:82d8768d7351 72 case CRX_ASK_OPERATING_MODE: {
Lightvalve 11:82d8768d7351 73 CAN_TX_OPERATING_MODE();
Lightvalve 11:82d8768d7351 74 break;
Lightvalve 11:82d8768d7351 75 }
Lightvalve 11:82d8768d7351 76 case CRX_SET_OPERATING_MODE: {
Lightvalve 52:8ea76864368a 77 OPERATING_MODE = (uint8_t) msg.data[1];
Lightvalve 52:8ea76864368a 78 SENSING_MODE = (uint8_t) msg.data[2];
Lightvalve 52:8ea76864368a 79 CURRENT_CONTROL_MODE = (uint8_t) msg.data[3];
Lightvalve 52:8ea76864368a 80 FLAG_VALVE_DEADZONE = (uint8_t) msg.data[4];
Lightvalve 170:42c938a40313 81 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 82 spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
Lightvalve 170:42c938a40313 83 spi_eeprom_write(RID_SENSING_MODE, (int16_t) SENSING_MODE);
Lightvalve 170:42c938a40313 84 spi_eeprom_write(RID_CURRENT_CONTROL_MODE, (int16_t) CURRENT_CONTROL_MODE);
Lightvalve 170:42c938a40313 85 spi_eeprom_write(RID_FLAG_VALVE_DEADZONE, (int16_t) FLAG_VALVE_DEADZONE);
Lightvalve 11:82d8768d7351 86 break;
Lightvalve 11:82d8768d7351 87 }
Lightvalve 11:82d8768d7351 88 case CRX_SET_ENC_ZERO: {
Lightvalve 11:82d8768d7351 89 ENC_SET_ZERO();
Lightvalve 11:82d8768d7351 90
Lightvalve 11:82d8768d7351 91 break;
Lightvalve 11:82d8768d7351 92 }
Lightvalve 11:82d8768d7351 93 case CRX_SET_FET_ON: {
Lightvalve 11:82d8768d7351 94
Lightvalve 11:82d8768d7351 95 break;
Lightvalve 11:82d8768d7351 96 }
Lightvalve 11:82d8768d7351 97
Lightvalve 45:35fa6884d0c6 98 case CRX_SET_POS_TORQ_TRANS: {
Lightvalve 45:35fa6884d0c6 99 MODE_POS_FT_TRANS = (int16_t) msg.data[1];
Lightvalve 45:35fa6884d0c6 100 /*
Lightvalve 45:35fa6884d0c6 101 MODE_POS_FT_TRANS == 0 : Position Control
Lightvalve 45:35fa6884d0c6 102 MODE_POS_FT_TRANS == 1 : Trasition(Position->Torque)
Lightvalve 45:35fa6884d0c6 103 MODE_POS_FT_TRANS == 2 : Torque Control (Convert to 2 automatically 3sec after transition)
Lightvalve 45:35fa6884d0c6 104 MODE_POS_FT_TRANS == 3 : Transition(Toque->Position)
Lightvalve 45:35fa6884d0c6 105 */
Lightvalve 11:82d8768d7351 106 break;
Lightvalve 11:82d8768d7351 107 }
Lightvalve 11:82d8768d7351 108
Lightvalve 11:82d8768d7351 109 case CRX_ASK_CAN_FREQ: {
Lightvalve 11:82d8768d7351 110 CAN_TX_CAN_FREQ();
Lightvalve 11:82d8768d7351 111
Lightvalve 11:82d8768d7351 112 break;
Lightvalve 11:82d8768d7351 113 }
Lightvalve 11:82d8768d7351 114
Lightvalve 11:82d8768d7351 115 case CRX_SET_CAN_FREQ: {
Lightvalve 11:82d8768d7351 116 CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 16:903b5a4433b4 117 ROM_RESET_DATA();
Lightvalve 170:42c938a40313 118 spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
Lightvalve 11:82d8768d7351 119 break;
Lightvalve 11:82d8768d7351 120 }
Lightvalve 11:82d8768d7351 121
Lightvalve 11:82d8768d7351 122 case CRX_ASK_CONTROL_MODE: {
Lightvalve 11:82d8768d7351 123 CAN_TX_CONTROL_MODE();
Lightvalve 11:82d8768d7351 124
Lightvalve 11:82d8768d7351 125 break;
Lightvalve 11:82d8768d7351 126 }
Lightvalve 11:82d8768d7351 127
Lightvalve 11:82d8768d7351 128 case CRX_SET_CONTROL_MODE: {
Lightvalve 57:f4819de54e7a 129 //CONTROL_MODE = (int16_t) (msg.data[1]);
Lightvalve 57:f4819de54e7a 130 CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
Lightvalve 12:6f2531038ea4 131 if (CONTROL_MODE == 22) { //MODE_FIND_HOME
Lightvalve 11:82d8768d7351 132 FLAG_FIND_HOME = true;
Lightvalve 11:82d8768d7351 133 }
Lightvalve 11:82d8768d7351 134 break;
Lightvalve 11:82d8768d7351 135 }
Lightvalve 11:82d8768d7351 136
Lightvalve 11:82d8768d7351 137 case CRX_SET_DATA_REQUEST: {
Lightvalve 11:82d8768d7351 138 int request_type = msg.data[2];
Lightvalve 11:82d8768d7351 139 flag_data_request[request_type] = msg.data[1];
Lightvalve 233:0a37cdd59651 140
Lightvalve 233:0a37cdd59651 141 // Position, Velocity, and Torque (ID:1200)
Lightvalve 233:0a37cdd59651 142 if (flag_data_request[0] == HIGH) {
Lightvalve 233:0a37cdd59651 143 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
Lightvalve 233:0a37cdd59651 144 if (SENSING_MODE == 0) {
Lightvalve 233:0a37cdd59651 145 CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
Lightvalve 233:0a37cdd59651 146 } else if (SENSING_MODE == 1) {
Lightvalve 233:0a37cdd59651 147 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));
Lightvalve 233:0a37cdd59651 148 }
Lightvalve 233:0a37cdd59651 149 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
Lightvalve 233:0a37cdd59651 150 if (SENSING_MODE == 0) {
Lightvalve 256:7c8cc8b56b88 151 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)));
Lightvalve 233:0a37cdd59651 152 } else if (SENSING_MODE == 1) {
Lightvalve 252:179a8c8791dc 153 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));
Lightvalve 233:0a37cdd59651 154 }
Lightvalve 233:0a37cdd59651 155 }
Lightvalve 233:0a37cdd59651 156 }
Lightvalve 233:0a37cdd59651 157 if (flag_data_request[1] == HIGH) {
Lightvalve 233:0a37cdd59651 158 CAN_TX_TORQUE((int16_t) (return_G[0]*100.0f)); //1300
Lightvalve 233:0a37cdd59651 159 }
Lightvalve 233:0a37cdd59651 160
Lightvalve 233:0a37cdd59651 161
Lightvalve 233:0a37cdd59651 162 if (flag_data_request[2] == HIGH) {
Lightvalve 252:179a8c8791dc 163
Lightvalve 252:179a8c8791dc 164
Lightvalve 233:0a37cdd59651 165 double t_value = 0.0f;
Lightvalve 252:179a8c8791dc 166 double t_value_ref = 0.0f;
Lightvalve 252:179a8c8791dc 167
Lightvalve 252:179a8c8791dc 168 if (CURRENT_CONTROL_MODE) {
Lightvalve 256:7c8cc8b56b88 169 t_value = cur.sen * 1000.0f; //Pulse
Lightvalve 256:7c8cc8b56b88 170 t_value_ref = I_REF_fil * 1000.0f; //Pulse
Lightvalve 233:0a37cdd59651 171 } else {
Lightvalve 252:179a8c8791dc 172 if(value>=(float) VALVE_CENTER) {
Lightvalve 252:179a8c8791dc 173 t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
Lightvalve 252:179a8c8791dc 174 } else {
Lightvalve 252:179a8c8791dc 175 t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
Lightvalve 252:179a8c8791dc 176 }
Lightvalve 252:179a8c8791dc 177
Lightvalve 252:179a8c8791dc 178 if(valve_pos.ref>=(float) VALVE_CENTER) {
Lightvalve 252:179a8c8791dc 179 t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
Lightvalve 252:179a8c8791dc 180 } else {
Lightvalve 252:179a8c8791dc 181 t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
Lightvalve 252:179a8c8791dc 182 }
Lightvalve 233:0a37cdd59651 183 }
Lightvalve 233:0a37cdd59651 184
Lightvalve 256:7c8cc8b56b88 185 CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
Lightvalve 233:0a37cdd59651 186 }
Lightvalve 233:0a37cdd59651 187
Lightvalve 235:4be664734d36 188 // //If it doesn't rest, below can can not work.
Lightvalve 235:4be664734d36 189 // for (int can_rest = 0; can_rest < 10000; can_rest++) {
Lightvalve 235:4be664734d36 190 // ;
Lightvalve 235:4be664734d36 191 // }
Lightvalve 235:4be664734d36 192 //
Lightvalve 233:0a37cdd59651 193 if (flag_data_request[3] == HIGH) {
Lightvalve 233:0a37cdd59651 194 //PWM
Lightvalve 233:0a37cdd59651 195 CAN_TX_PWM((int16_t) (torq.ref)); //1500
Lightvalve 233:0a37cdd59651 196 // CAN_TX_PWM((int16_t) (f_future[1])); //1500
Lightvalve 233:0a37cdd59651 197 }
Lightvalve 233:0a37cdd59651 198
Lightvalve 233:0a37cdd59651 199 if (flag_data_request[4] == HIGH) {
Lightvalve 233:0a37cdd59651 200 //valve position
Lightvalve 233:0a37cdd59651 201 //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
Lightvalve 233:0a37cdd59651 202 CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600
Lightvalve 233:0a37cdd59651 203 }
Lightvalve 233:0a37cdd59651 204
Lightvalve 11:82d8768d7351 205
Lightvalve 11:82d8768d7351 206 break;
Lightvalve 11:82d8768d7351 207 }
Lightvalve 11:82d8768d7351 208
Lightvalve 11:82d8768d7351 209 case CRX_ASK_JOINT_ENC_DIR: {
Lightvalve 11:82d8768d7351 210 CAN_TX_JOINT_ENC_DIR();
Lightvalve 11:82d8768d7351 211
Lightvalve 11:82d8768d7351 212 break;
Lightvalve 11:82d8768d7351 213 }
Lightvalve 11:82d8768d7351 214
Lightvalve 11:82d8768d7351 215 case CRX_SET_JOINT_ENC_DIR: {
Lightvalve 11:82d8768d7351 216 DIR_JOINT_ENC = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 217 if (DIR_JOINT_ENC >= 0)
Lightvalve 11:82d8768d7351 218 DIR_JOINT_ENC = 1;
Lightvalve 11:82d8768d7351 219 else
Lightvalve 11:82d8768d7351 220 DIR_JOINT_ENC = -1;
Lightvalve 170:42c938a40313 221
Lightvalve 170:42c938a40313 222 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 223
Lightvalve 170:42c938a40313 224 spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
Lightvalve 11:82d8768d7351 225
Lightvalve 11:82d8768d7351 226 break;
Lightvalve 11:82d8768d7351 227 }
Lightvalve 11:82d8768d7351 228
Lightvalve 11:82d8768d7351 229 case CRX_ASK_VALVE_DIR: {
Lightvalve 11:82d8768d7351 230 CAN_TX_VALVE_DIR();
Lightvalve 11:82d8768d7351 231
Lightvalve 11:82d8768d7351 232 break;
Lightvalve 11:82d8768d7351 233 }
Lightvalve 11:82d8768d7351 234
Lightvalve 11:82d8768d7351 235 case CRX_SET_VALVE_DIR: {
Lightvalve 11:82d8768d7351 236 DIR_VALVE = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 237 if (DIR_VALVE >= 0)
Lightvalve 11:82d8768d7351 238 DIR_VALVE = 1;
Lightvalve 11:82d8768d7351 239 else
Lightvalve 11:82d8768d7351 240 DIR_VALVE = -1;
Lightvalve 170:42c938a40313 241
Lightvalve 170:42c938a40313 242 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 243
Lightvalve 170:42c938a40313 244 spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
Lightvalve 11:82d8768d7351 245
Lightvalve 11:82d8768d7351 246 break;
Lightvalve 11:82d8768d7351 247 }
Lightvalve 11:82d8768d7351 248
Lightvalve 11:82d8768d7351 249 case CRX_ASK_VALVE_ENC_DIR: {
Lightvalve 11:82d8768d7351 250 CAN_TX_VALVE_ENC_DIR();
Lightvalve 11:82d8768d7351 251
Lightvalve 11:82d8768d7351 252 break;
Lightvalve 11:82d8768d7351 253 }
Lightvalve 11:82d8768d7351 254
Lightvalve 11:82d8768d7351 255 case CRX_SET_VALVE_ENC_DIR: {
Lightvalve 11:82d8768d7351 256 DIR_VALVE_ENC = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 257 if (DIR_VALVE_ENC >= 0)
Lightvalve 11:82d8768d7351 258 DIR_VALVE_ENC = 1;
Lightvalve 11:82d8768d7351 259 else
Lightvalve 11:82d8768d7351 260 DIR_VALVE_ENC = -1;
Lightvalve 11:82d8768d7351 261
Lightvalve 170:42c938a40313 262 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 263
Lightvalve 170:42c938a40313 264 spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
Lightvalve 11:82d8768d7351 265
Lightvalve 11:82d8768d7351 266 break;
Lightvalve 11:82d8768d7351 267 }
Lightvalve 11:82d8768d7351 268
Lightvalve 11:82d8768d7351 269 case CRX_ASK_VOLTAGE_SUPPLY: {
Lightvalve 11:82d8768d7351 270 CAN_TX_VOLTAGE_SUPPLY();
Lightvalve 11:82d8768d7351 271
Lightvalve 11:82d8768d7351 272 break;
Lightvalve 11:82d8768d7351 273 }
Lightvalve 11:82d8768d7351 274
Lightvalve 11:82d8768d7351 275 case CRX_SET_VOLTAGE_SUPPLY: {
Lightvalve 30:8d561f16383b 276 SUPPLY_VOLTAGE = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
Lightvalve 170:42c938a40313 277
Lightvalve 170:42c938a40313 278 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 279
Lightvalve 170:42c938a40313 280 spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.0f));
Lightvalve 11:82d8768d7351 281
Lightvalve 11:82d8768d7351 282 break;
Lightvalve 11:82d8768d7351 283 }
Lightvalve 11:82d8768d7351 284
Lightvalve 11:82d8768d7351 285 case CRX_ASK_VOLTAGE_VALVE: {
Lightvalve 11:82d8768d7351 286 CAN_TX_VOLTAGE_VALVE();
Lightvalve 11:82d8768d7351 287
Lightvalve 11:82d8768d7351 288 break;
Lightvalve 11:82d8768d7351 289 }
Lightvalve 11:82d8768d7351 290
Lightvalve 11:82d8768d7351 291 case CRX_SET_VOLTAGE_VALVE: {
Lightvalve 30:8d561f16383b 292 VALVE_VOLTAGE_LIMIT = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
Lightvalve 170:42c938a40313 293
Lightvalve 170:42c938a40313 294 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 295
Lightvalve 170:42c938a40313 296 spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f));
Lightvalve 11:82d8768d7351 297
Lightvalve 11:82d8768d7351 298
Lightvalve 11:82d8768d7351 299 break;
Lightvalve 11:82d8768d7351 300 }
Lightvalve 12:6f2531038ea4 301
Lightvalve 11:82d8768d7351 302 case CRX_SET_HOMEPOS: {
Lightvalve 57:f4819de54e7a 303 //CONTROL_MODE = 22;
Lightvalve 57:f4819de54e7a 304 CONTROL_UTILITY_MODE = 22;
Lightvalve 11:82d8768d7351 305 break;
Lightvalve 11:82d8768d7351 306 }
Lightvalve 11:82d8768d7351 307
Lightvalve 11:82d8768d7351 308 case CRX_ASK_PID_GAIN: {
Lightvalve 11:82d8768d7351 309 CAN_TX_PID_GAIN(msg.data[1]);
Lightvalve 11:82d8768d7351 310
Lightvalve 11:82d8768d7351 311 break;
Lightvalve 11:82d8768d7351 312 }
Lightvalve 11:82d8768d7351 313
Lightvalve 11:82d8768d7351 314 case CRX_SET_PID_GAIN: {
Lightvalve 11:82d8768d7351 315 if (msg.data[1] == 0) {
Lightvalve 11:82d8768d7351 316 P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 11:82d8768d7351 317 I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 11:82d8768d7351 318 D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
Lightvalve 170:42c938a40313 319
Lightvalve 170:42c938a40313 320 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 321 spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 322 spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
Lightvalve 170:42c938a40313 323 spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
Lightvalve 11:82d8768d7351 324
Lightvalve 11:82d8768d7351 325 } else if (msg.data[1] == 1) {
Lightvalve 11:82d8768d7351 326 P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 11:82d8768d7351 327 I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 11:82d8768d7351 328 D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
Lightvalve 170:42c938a40313 329
Lightvalve 170:42c938a40313 330 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 331 spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 332 spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 333 spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
Lightvalve 170:42c938a40313 334
Lightvalve 11:82d8768d7351 335 } else if (msg.data[1] == 2) {
Lightvalve 11:82d8768d7351 336 P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 11:82d8768d7351 337 I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 11:82d8768d7351 338 D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
Lightvalve 170:42c938a40313 339
Lightvalve 170:42c938a40313 340 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 341 spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 342 spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 343 spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
Lightvalve 170:42c938a40313 344
Lightvalve 46:2694daea349b 345 } else if (msg.data[1] == 3) {
Lightvalve 67:c2812cf26c38 346 K_SPRING = (float) (((float) ((int16_t) (msg.data[2] | msg.data[3] << 8))) * 0.1f);
Lightvalve 67:c2812cf26c38 347 D_DAMPER = (float) (((float) ((int16_t) (msg.data[4] | msg.data[5] << 8))) * 0.01f);
Lightvalve 170:42c938a40313 348
Lightvalve 67:c2812cf26c38 349 // ROM_RESET_DATA(); //For Real-time changing
Lightvalve 170:42c938a40313 350 // spi_eeprom_write(RID_K_SPRING, (int16_t) K_SPRING);
Lightvalve 170:42c938a40313 351 // spi_eeprom_write(RID_D_DAMPER, (int16_t) D_DAMPER);
Lightvalve 170:42c938a40313 352
Lightvalve 72:3436ce769b1e 353 } else if (msg.data[1] == 4) {
Lightvalve 72:3436ce769b1e 354 P_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 72:3436ce769b1e 355 I_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 72:3436ce769b1e 356 D_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[6] | msg.data[7] << 8);
Lightvalve 170:42c938a40313 357
Lightvalve 170:42c938a40313 358 // ROM_RESET_DATA();
Lightvalve 11:82d8768d7351 359 }
Lightvalve 11:82d8768d7351 360
Lightvalve 11:82d8768d7351 361 break;
Lightvalve 11:82d8768d7351 362 }
Lightvalve 11:82d8768d7351 363
Lightvalve 11:82d8768d7351 364 case CRX_ASK_VALVE_DEADZONE: {
Lightvalve 11:82d8768d7351 365 CAN_TX_VALVE_DEADZONE();
Lightvalve 11:82d8768d7351 366
Lightvalve 11:82d8768d7351 367 break;
Lightvalve 11:82d8768d7351 368 }
Lightvalve 11:82d8768d7351 369
Lightvalve 11:82d8768d7351 370 case CRX_SET_VALVE_DEADZONE: {
Lightvalve 11:82d8768d7351 371 VALVE_CENTER = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 372 VALVE_DEADZONE_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
Lightvalve 11:82d8768d7351 373 VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
Lightvalve 170:42c938a40313 374
Lightvalve 170:42c938a40313 375 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 376 spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
Lightvalve 252:179a8c8791dc 377 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
Lightvalve 252:179a8c8791dc 378 spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
Lightvalve 11:82d8768d7351 379
Lightvalve 11:82d8768d7351 380 break;
Lightvalve 11:82d8768d7351 381 }
Lightvalve 11:82d8768d7351 382
Lightvalve 11:82d8768d7351 383 case CRX_ASK_VELOCITY_COMP_GAIN: {
Lightvalve 11:82d8768d7351 384 CAN_TX_VELOCITY_COMP_GAIN();
Lightvalve 11:82d8768d7351 385
Lightvalve 11:82d8768d7351 386 break;
Lightvalve 11:82d8768d7351 387 }
Lightvalve 11:82d8768d7351 388
Lightvalve 11:82d8768d7351 389 case CRX_SET_VELOCITY_COMP_GAIN: {
Lightvalve 11:82d8768d7351 390 VELOCITY_COMP_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 170:42c938a40313 391
Lightvalve 170:42c938a40313 392 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 393
Lightvalve 170:42c938a40313 394 spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
Lightvalve 11:82d8768d7351 395
Lightvalve 11:82d8768d7351 396 break;
Lightvalve 11:82d8768d7351 397 }
Lightvalve 11:82d8768d7351 398
Lightvalve 11:82d8768d7351 399 case CRX_ASK_COMPLIANCE_GAIN: {
Lightvalve 11:82d8768d7351 400 CAN_TX_COMPLIANCE_GAIN();
Lightvalve 11:82d8768d7351 401
Lightvalve 11:82d8768d7351 402 break;
Lightvalve 11:82d8768d7351 403 }
Lightvalve 11:82d8768d7351 404
Lightvalve 11:82d8768d7351 405 case CRX_SET_COMPLIANCE_GAIN: {
Lightvalve 11:82d8768d7351 406 COMPLIANCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 407
Lightvalve 170:42c938a40313 408 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 409
Lightvalve 170:42c938a40313 410 spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
Lightvalve 11:82d8768d7351 411
Lightvalve 11:82d8768d7351 412 break;
Lightvalve 11:82d8768d7351 413 }
Lightvalve 11:82d8768d7351 414
Lightvalve 11:82d8768d7351 415 case CRX_ASK_VALVE_FF: {
Lightvalve 11:82d8768d7351 416 CAN_TX_VALVE_FF();
Lightvalve 11:82d8768d7351 417
Lightvalve 11:82d8768d7351 418 break;
Lightvalve 11:82d8768d7351 419 }
Lightvalve 11:82d8768d7351 420
Lightvalve 11:82d8768d7351 421 case CRX_SET_VALVE_FF: {
Lightvalve 11:82d8768d7351 422 VALVE_FF = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 170:42c938a40313 423
Lightvalve 170:42c938a40313 424 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 425
Lightvalve 170:42c938a40313 426 spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
Lightvalve 11:82d8768d7351 427
Lightvalve 11:82d8768d7351 428 break;
Lightvalve 11:82d8768d7351 429 }
Lightvalve 11:82d8768d7351 430
Lightvalve 11:82d8768d7351 431 case CRX_ASK_BULK_MODULUS: {
Lightvalve 11:82d8768d7351 432 CAN_TX_BULK_MODULUS();
Lightvalve 11:82d8768d7351 433
Lightvalve 11:82d8768d7351 434 break;
Lightvalve 11:82d8768d7351 435 }
Lightvalve 11:82d8768d7351 436
Lightvalve 11:82d8768d7351 437 case CRX_SET_BULK_MODULUS: {
Lightvalve 11:82d8768d7351 438 BULK_MODULUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 170:42c938a40313 439
Lightvalve 170:42c938a40313 440 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 441
Lightvalve 170:42c938a40313 442 spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
Lightvalve 11:82d8768d7351 443
Lightvalve 11:82d8768d7351 444 break;
Lightvalve 11:82d8768d7351 445 }
Lightvalve 11:82d8768d7351 446
Lightvalve 11:82d8768d7351 447 case CRX_ASK_CHAMBER_VOLUME: {
Lightvalve 11:82d8768d7351 448 CAN_TX_CHAMBER_VOLUME();
Lightvalve 11:82d8768d7351 449
Lightvalve 11:82d8768d7351 450 break;
Lightvalve 11:82d8768d7351 451 }
Lightvalve 11:82d8768d7351 452
Lightvalve 11:82d8768d7351 453 case CRX_SET_CHAMBER_VOLUME: {
Lightvalve 11:82d8768d7351 454 CHAMBER_VOLUME_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 455 CHAMBER_VOLUME_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
Lightvalve 11:82d8768d7351 456
Lightvalve 170:42c938a40313 457 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 458
Lightvalve 170:42c938a40313 459 spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
Lightvalve 170:42c938a40313 460 spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
Lightvalve 11:82d8768d7351 461
Lightvalve 11:82d8768d7351 462 break;
Lightvalve 11:82d8768d7351 463 }
Lightvalve 11:82d8768d7351 464
Lightvalve 11:82d8768d7351 465 case CRX_ASK_PISTON_AREA: {
Lightvalve 11:82d8768d7351 466 CAN_TX_PISTON_AREA();
Lightvalve 11:82d8768d7351 467
Lightvalve 11:82d8768d7351 468 break;
Lightvalve 11:82d8768d7351 469 }
Lightvalve 11:82d8768d7351 470
Lightvalve 11:82d8768d7351 471 case CRX_SET_PISTON_AREA: {
Lightvalve 11:82d8768d7351 472 PISTON_AREA_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 473 PISTON_AREA_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
Lightvalve 69:3995ffeaa786 474 PISTON_AREA_alpha = (double)PISTON_AREA_A/(double)PISTON_AREA_B;
Lightvalve 57:f4819de54e7a 475 alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
Lightvalve 170:42c938a40313 476
Lightvalve 170:42c938a40313 477 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 478
Lightvalve 170:42c938a40313 479 spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
Lightvalve 170:42c938a40313 480 spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
Lightvalve 11:82d8768d7351 481 break;
Lightvalve 11:82d8768d7351 482 }
Lightvalve 11:82d8768d7351 483
Lightvalve 11:82d8768d7351 484 case CRX_ASK_PRES: {
Lightvalve 11:82d8768d7351 485 CAN_TX_PRES_A_AND_B();
Lightvalve 12:6f2531038ea4 486 // SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
Lightvalve 16:903b5a4433b4 487 //dac_1 = PRES_A_VREF;
Lightvalve 16:903b5a4433b4 488 //dac_2 = PRES_B_VREF;
Lightvalve 170:42c938a40313 489
Lightvalve 11:82d8768d7351 490 break;
Lightvalve 11:82d8768d7351 491 }
Lightvalve 11:82d8768d7351 492
Lightvalve 11:82d8768d7351 493 case CRX_SET_PRES: {
Lightvalve 11:82d8768d7351 494 PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 495 PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8);
Lightvalve 170:42c938a40313 496 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 497 spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
Lightvalve 170:42c938a40313 498 spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
Lightvalve 11:82d8768d7351 499
Lightvalve 11:82d8768d7351 500
Lightvalve 11:82d8768d7351 501 break;
Lightvalve 11:82d8768d7351 502 }
Lightvalve 11:82d8768d7351 503
Lightvalve 11:82d8768d7351 504 case CRX_ASK_ENC_LIMIT: {
Lightvalve 11:82d8768d7351 505 CAN_TX_ENC_LIMIT();
Lightvalve 11:82d8768d7351 506
Lightvalve 11:82d8768d7351 507 break;
Lightvalve 11:82d8768d7351 508 }
Lightvalve 11:82d8768d7351 509
Lightvalve 11:82d8768d7351 510 case CRX_SET_ENC_LIMIT: {
Lightvalve 11:82d8768d7351 511 ENC_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 11:82d8768d7351 512 ENC_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
Lightvalve 170:42c938a40313 513 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 514 spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
Lightvalve 170:42c938a40313 515 spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
Lightvalve 11:82d8768d7351 516
Lightvalve 11:82d8768d7351 517 break;
Lightvalve 11:82d8768d7351 518 }
Lightvalve 11:82d8768d7351 519
Lightvalve 11:82d8768d7351 520 case CRX_ASK_STROKE: {
Lightvalve 11:82d8768d7351 521 CAN_TX_STROKE();
Lightvalve 11:82d8768d7351 522 break;
Lightvalve 11:82d8768d7351 523 }
Lightvalve 11:82d8768d7351 524
Lightvalve 11:82d8768d7351 525 case CRX_SET_STROKE: {
Lightvalve 11:82d8768d7351 526 STROKE = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 170:42c938a40313 527 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 528 spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
Lightvalve 11:82d8768d7351 529
Lightvalve 11:82d8768d7351 530 break;
Lightvalve 11:82d8768d7351 531 }
Lightvalve 11:82d8768d7351 532
Lightvalve 11:82d8768d7351 533 case CRX_ASK_VALVE_LIMIT: {
Lightvalve 11:82d8768d7351 534 CAN_TX_VALVE_LIMIT();
Lightvalve 11:82d8768d7351 535
Lightvalve 11:82d8768d7351 536 break;
Lightvalve 11:82d8768d7351 537 }
Lightvalve 11:82d8768d7351 538
Lightvalve 11:82d8768d7351 539 case CRX_SET_VALVE_LIMIT: {
Lightvalve 57:f4819de54e7a 540 VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 57:f4819de54e7a 541 VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8);
Lightvalve 170:42c938a40313 542 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 543
Lightvalve 170:42c938a40313 544 spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
Lightvalve 170:42c938a40313 545 spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
Lightvalve 11:82d8768d7351 546
Lightvalve 11:82d8768d7351 547 break;
Lightvalve 11:82d8768d7351 548 }
Lightvalve 11:82d8768d7351 549
Lightvalve 11:82d8768d7351 550 case CRX_ASK_ENC_PULSE_PER_POSITION: {
Lightvalve 11:82d8768d7351 551 CAN_TX_ENC_PULSE_PER_POSITION();
Lightvalve 11:82d8768d7351 552
Lightvalve 11:82d8768d7351 553 break;
Lightvalve 11:82d8768d7351 554 }
Lightvalve 11:82d8768d7351 555
Lightvalve 11:82d8768d7351 556 case CRX_SET_ENC_PULSE_PER_POSITION: {
Lightvalve 11:82d8768d7351 557 ENC_PULSE_PER_POSITION = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 170:42c938a40313 558 // ROM_RESET_DATA();
Lightvalve 180:02be1711ee0b 559 spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) (ENC_PULSE_PER_POSITION));
Lightvalve 11:82d8768d7351 560
Lightvalve 11:82d8768d7351 561 break;
Lightvalve 11:82d8768d7351 562 }
Lightvalve 11:82d8768d7351 563
Lightvalve 11:82d8768d7351 564 case CRX_ASK_TORQUE_SENSOR_PULSE_PER_TORQUE: {
Lightvalve 11:82d8768d7351 565 CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE();
Lightvalve 11:82d8768d7351 566
Lightvalve 11:82d8768d7351 567 break;
Lightvalve 11:82d8768d7351 568 }
Lightvalve 11:82d8768d7351 569
Lightvalve 11:82d8768d7351 570 case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
Lightvalve 139:15621998925b 571 TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.0001f);
Lightvalve 170:42c938a40313 572 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 573 spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
Lightvalve 11:82d8768d7351 574
Lightvalve 11:82d8768d7351 575 break;
Lightvalve 11:82d8768d7351 576 }
Lightvalve 11:82d8768d7351 577
Lightvalve 11:82d8768d7351 578 case CRX_ASK_PRES_SENSOR_PULSE_PER_PRES: {
Lightvalve 11:82d8768d7351 579 CAN_TX_PRES_SENSOR_PULSE_PER_PRES();
Lightvalve 11:82d8768d7351 580
Lightvalve 11:82d8768d7351 581 break;
Lightvalve 11:82d8768d7351 582 }
Lightvalve 11:82d8768d7351 583
Lightvalve 11:82d8768d7351 584 case CRX_SET_PRES_SENSOR_PULSE_PER_PRES: {
Lightvalve 253:0246366e0dcb 585 PRES_SENSOR_A_PULSE_PER_BAR = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8)) * 0.01f;
Lightvalve 253:0246366e0dcb 586 PRES_SENSOR_B_PULSE_PER_BAR = (float) ((int16_t) (msg.data[3] | msg.data[4] << 8)) * 0.01f;
Lightvalve 170:42c938a40313 587 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 588 spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
Lightvalve 170:42c938a40313 589 spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
Lightvalve 11:82d8768d7351 590
Lightvalve 11:82d8768d7351 591 break;
Lightvalve 11:82d8768d7351 592 }
Lightvalve 11:82d8768d7351 593
Lightvalve 11:82d8768d7351 594 case CRX_ASK_FRICTION: {
Lightvalve 11:82d8768d7351 595 CAN_TX_FRICTION();
Lightvalve 11:82d8768d7351 596
Lightvalve 11:82d8768d7351 597 break;
Lightvalve 11:82d8768d7351 598 }
Lightvalve 11:82d8768d7351 599
Lightvalve 11:82d8768d7351 600 case CRX_SET_FRICTION: {
Lightvalve 30:8d561f16383b 601 FRICTION = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
Lightvalve 170:42c938a40313 602 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 603 spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.0f));
Lightvalve 11:82d8768d7351 604
Lightvalve 11:82d8768d7351 605 break;
Lightvalve 11:82d8768d7351 606 }
Lightvalve 11:82d8768d7351 607
Lightvalve 11:82d8768d7351 608 case CRX_ASK_VALVE_GAIN_PLUS: {
Lightvalve 11:82d8768d7351 609 CAN_TX_VALVE_GAIN_PLUS();
Lightvalve 11:82d8768d7351 610
Lightvalve 11:82d8768d7351 611 break;
Lightvalve 11:82d8768d7351 612 }
Lightvalve 11:82d8768d7351 613 case CRX_SET_VALVE_GAIN_PLUS: {
Lightvalve 30:8d561f16383b 614 VALVE_GAIN_LPM_PER_V[0] = (double) msg.data[1] / 50.0f;
Lightvalve 30:8d561f16383b 615 VALVE_GAIN_LPM_PER_V[2] = (double) msg.data[2] / 50.0f;
Lightvalve 30:8d561f16383b 616 VALVE_GAIN_LPM_PER_V[4] = (double) msg.data[3] / 50.0f;
Lightvalve 30:8d561f16383b 617 VALVE_GAIN_LPM_PER_V[6] = (double) msg.data[4] / 50.0f;
Lightvalve 30:8d561f16383b 618 VALVE_GAIN_LPM_PER_V[8] = (double) msg.data[5] / 50.0f;
Lightvalve 170:42c938a40313 619 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 620 spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
Lightvalve 170:42c938a40313 621 spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
Lightvalve 170:42c938a40313 622 spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
Lightvalve 170:42c938a40313 623 spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
Lightvalve 170:42c938a40313 624 spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
Lightvalve 11:82d8768d7351 625
Lightvalve 11:82d8768d7351 626 break;
Lightvalve 11:82d8768d7351 627 }
Lightvalve 11:82d8768d7351 628
Lightvalve 11:82d8768d7351 629 case CRX_ASK_VALVE_GAIN_MINUS: {
Lightvalve 11:82d8768d7351 630 CAN_TX_VALVE_GAIN_MINUS();
Lightvalve 11:82d8768d7351 631
Lightvalve 11:82d8768d7351 632 break;
Lightvalve 11:82d8768d7351 633 }
Lightvalve 11:82d8768d7351 634 case CRX_SET_VALVE_GAIN_MINUS: {
Lightvalve 30:8d561f16383b 635 VALVE_GAIN_LPM_PER_V[1] = (double) msg.data[1] / 50.0f;
Lightvalve 30:8d561f16383b 636 VALVE_GAIN_LPM_PER_V[3] = (double) msg.data[2] / 50.0f;
Lightvalve 30:8d561f16383b 637 VALVE_GAIN_LPM_PER_V[5] = (double) msg.data[3] / 50.0f;
Lightvalve 30:8d561f16383b 638 VALVE_GAIN_LPM_PER_V[7] = (double) msg.data[4] / 50.0f;
Lightvalve 30:8d561f16383b 639 VALVE_GAIN_LPM_PER_V[9] = (double) msg.data[5] / 50.0f;
Lightvalve 170:42c938a40313 640 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 641 spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
Lightvalve 170:42c938a40313 642 spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
Lightvalve 170:42c938a40313 643 spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
Lightvalve 170:42c938a40313 644 spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
Lightvalve 170:42c938a40313 645 spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
Lightvalve 11:82d8768d7351 646
Lightvalve 11:82d8768d7351 647 break;
Lightvalve 11:82d8768d7351 648 }
Lightvalve 11:82d8768d7351 649 case CRX_ASK_DDV_VALVE_DEADZONE: {
Lightvalve 11:82d8768d7351 650 CAN_TX_DDV_VALVE_DEADZONE();
Lightvalve 11:82d8768d7351 651 break;
Lightvalve 11:82d8768d7351 652 }
Lightvalve 11:82d8768d7351 653
Lightvalve 11:82d8768d7351 654 case CRX_LOW_REF: {
Lightvalve 11:82d8768d7351 655 REFERENCE_MODE = msg.data[1];
Lightvalve 11:82d8768d7351 656
Lightvalve 11:82d8768d7351 657 REF_NUM = msg.data[2];
Lightvalve 30:8d561f16383b 658 REF_PERIOD = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) / 100.0f;
Lightvalve 30:8d561f16383b 659 if (REF_PERIOD <= 0.0f) REF_MOVE_TIME_5k = TMR_FREQ_5k / CAN_FREQ;
Lightvalve 11:82d8768d7351 660 else REF_MOVE_TIME_5k = (int) (REF_PERIOD * (double) TMR_FREQ_5k);
Lightvalve 30:8d561f16383b 661 REF_MAG = (double) ((int16_t) (msg.data[5] | msg.data[6] << 8)) / 100.0f;
Lightvalve 11:82d8768d7351 662
Lightvalve 11:82d8768d7351 663 break;
Lightvalve 11:82d8768d7351 664 }
Lightvalve 11:82d8768d7351 665
Lightvalve 11:82d8768d7351 666 case CRX_JUMP_STATUS: {
Lightvalve 11:82d8768d7351 667 MODE_JUMP_STATUS = msg.data[1];
Lightvalve 11:82d8768d7351 668
Lightvalve 11:82d8768d7351 669 break;
Lightvalve 11:82d8768d7351 670 }
Lightvalve 11:82d8768d7351 671
Lightvalve 11:82d8768d7351 672 case CRX_SET_ERR_CLEAR: {
Lightvalve 11:82d8768d7351 673
Lightvalve 12:6f2531038ea4 674 for (int i = 0; i < num_err; i++) {
Lightvalve 11:82d8768d7351 675 flag_err[i] = FALSE;
Lightvalve 11:82d8768d7351 676 flag_err_old[i] = FALSE;
Lightvalve 11:82d8768d7351 677 }
Lightvalve 11:82d8768d7351 678
Lightvalve 11:82d8768d7351 679 flag_err_rt = FALSE;
Lightvalve 11:82d8768d7351 680
Lightvalve 11:82d8768d7351 681
Lightvalve 11:82d8768d7351 682 break;
Lightvalve 11:82d8768d7351 683 }
Lightvalve 11:82d8768d7351 684
Lightvalve 11:82d8768d7351 685 case CRX_ASK_HOMEPOS_OFFSET: {
Lightvalve 11:82d8768d7351 686 CAN_TX_HOMEPOS_OFFSET();
Lightvalve 11:82d8768d7351 687 break;
Lightvalve 11:82d8768d7351 688 }
Lightvalve 11:82d8768d7351 689 case CRX_SET_HOMEPOS_OFFSET: {
Lightvalve 11:82d8768d7351 690 HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 170:42c938a40313 691 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 692 spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
Lightvalve 11:82d8768d7351 693 break;
Lightvalve 11:82d8768d7351 694 }
Lightvalve 11:82d8768d7351 695
Lightvalve 11:82d8768d7351 696 case CRX_ASK_HOMEPOS_VALVE_OPENING: {
Lightvalve 11:82d8768d7351 697 CAN_TX_HOMPOS_VALVE_OPENING();
Lightvalve 11:82d8768d7351 698 break;
Lightvalve 11:82d8768d7351 699 }
Lightvalve 11:82d8768d7351 700 case CRX_SET_HOMEPOS_VALVE_OPENING: {
Lightvalve 11:82d8768d7351 701 HOMEPOS_VALVE_OPENING = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 170:42c938a40313 702 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 703 spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
Lightvalve 11:82d8768d7351 704 break;
Lightvalve 11:82d8768d7351 705 }
Lightvalve 11:82d8768d7351 706
Lightvalve 11:82d8768d7351 707 case CRX_ASK_VALVE_PWM_VS_VALVE_POS: {
Lightvalve 11:82d8768d7351 708 can_index = (int16_t) msg.data[1];
Lightvalve 11:82d8768d7351 709 CAN_TX_VALVE_PWM_VS_VALVE_POS(can_index);
Lightvalve 11:82d8768d7351 710 break;
Lightvalve 11:82d8768d7351 711 }
Lightvalve 11:82d8768d7351 712 case CRX_ASK_VALVE_POS_VS_FLOWRATE: {
Lightvalve 11:82d8768d7351 713 can_index = (int16_t) msg.data[1];
Lightvalve 11:82d8768d7351 714 CAN_TX_VALVE_POS_VS_FLOWRATE(can_index);
Lightvalve 11:82d8768d7351 715 break;
Lightvalve 11:82d8768d7351 716 }
Lightvalve 11:82d8768d7351 717 case CRX_ASK_VALVE_POS_NUM: {
Lightvalve 11:82d8768d7351 718 CAN_TX_VALVE_POS_NUM();
Lightvalve 11:82d8768d7351 719 break;
Lightvalve 11:82d8768d7351 720 }
Lightvalve 170:42c938a40313 721
Lightvalve 28:2a62d73e3dd0 722 case CRX_SET_ROM: {
Lightvalve 28:2a62d73e3dd0 723 ROM_RESET_DATA();
Lightvalve 28:2a62d73e3dd0 724 break;
Lightvalve 28:2a62d73e3dd0 725 }
Lightvalve 32:4b8c0fedaf2c 726 case CRX_SET_VALVE_CENTER_OFFSET: {
Lightvalve 252:179a8c8791dc 727 VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)));
Lightvalve 57:f4819de54e7a 728 VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET;
Lightvalve 170:42c938a40313 729 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 730 spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
Lightvalve 170:42c938a40313 731
Lightvalve 33:91b17819ec30 732 break;
Lightvalve 33:91b17819ec30 733 }
Lightvalve 33:91b17819ec30 734 case CRX_SET_VALVE_DZ_MINUS_OFFSET: {
Lightvalve 252:179a8c8791dc 735 VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)));
Lightvalve 33:91b17819ec30 736 VALVE_DEADZONE_MINUS = VALVE_DEADZONE_MINUS + VALVE_DZ_MINUS_OFFSET;
Lightvalve 170:42c938a40313 737 // ROM_RESET_DATA();
Lightvalve 252:179a8c8791dc 738 spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
Lightvalve 170:42c938a40313 739
Lightvalve 33:91b17819ec30 740 break;
Lightvalve 33:91b17819ec30 741 }
Lightvalve 33:91b17819ec30 742 case CRX_SET_VALVE_DZ_PLUS_OFFSET: {
Lightvalve 252:179a8c8791dc 743 VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)));
Lightvalve 33:91b17819ec30 744 VALVE_DEADZONE_PLUS = VALVE_DEADZONE_PLUS + VALVE_DZ_PLUS_OFFSET;
Lightvalve 170:42c938a40313 745 // ROM_RESET_DATA();
Lightvalve 252:179a8c8791dc 746 spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
Lightvalve 32:4b8c0fedaf2c 747 break;
Lightvalve 32:4b8c0fedaf2c 748 }
Lightvalve 36:a46e63505ed8 749 case CRX_SET_PID_GAIN_OPP: {
Lightvalve 36:a46e63505ed8 750 if (msg.data[1] == 0) {
Lightvalve 36:a46e63505ed8 751 P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 36:a46e63505ed8 752 I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 36:a46e63505ed8 753 D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
Lightvalve 170:42c938a40313 754
Lightvalve 170:42c938a40313 755 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 756
Lightvalve 36:a46e63505ed8 757 //spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
Lightvalve 36:a46e63505ed8 758 //spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
Lightvalve 36:a46e63505ed8 759 //spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
Lightvalve 36:a46e63505ed8 760
Lightvalve 36:a46e63505ed8 761
Lightvalve 36:a46e63505ed8 762 } else if (msg.data[1] == 1) {
Lightvalve 36:a46e63505ed8 763 P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 36:a46e63505ed8 764 I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 36:a46e63505ed8 765 D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
Lightvalve 170:42c938a40313 766
Lightvalve 170:42c938a40313 767 // ROM_RESET_DATA();
Lightvalve 36:a46e63505ed8 768
Lightvalve 36:a46e63505ed8 769 //spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
Lightvalve 36:a46e63505ed8 770 //spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
Lightvalve 36:a46e63505ed8 771 //spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
Lightvalve 36:a46e63505ed8 772 } else if (msg.data[1] == 2) {
Lightvalve 36:a46e63505ed8 773 P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 36:a46e63505ed8 774 I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 36:a46e63505ed8 775 D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
Lightvalve 170:42c938a40313 776
Lightvalve 170:42c938a40313 777 // ROM_RESET_DATA();
Lightvalve 170:42c938a40313 778
Lightvalve 36:a46e63505ed8 779 //spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
Lightvalve 36:a46e63505ed8 780 //spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
Lightvalve 36:a46e63505ed8 781 //spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
Lightvalve 36:a46e63505ed8 782 }
Lightvalve 36:a46e63505ed8 783 break;
Lightvalve 36:a46e63505ed8 784 }
Lightvalve 38:118df027d851 785 case CRX_ASK_VALVE_MAX_MIN_POS: {
Lightvalve 57:f4819de54e7a 786 //CAN_TX_DDV_VALVE_DEADZONE();
Lightvalve 38:118df027d851 787 CAN_TX_DDV_VALVE_MAX_MIN_POS();
Lightvalve 38:118df027d851 788 break;
Lightvalve 38:118df027d851 789 }
Lightvalve 170:42c938a40313 790 case CRX_DELAY_TEST: {
Lightvalve 54:647072f5307a 791 flag_delay_test = 1;
Lightvalve 54:647072f5307a 792 break;
Lightvalve 54:647072f5307a 793 }
Lightvalve 68:328e1be06f5d 794 case CRX_SET_NN_CONTROL_FLAG: {
Lightvalve 68:328e1be06f5d 795 NN_Control_Flag = (int16_t) msg.data[1];
Lightvalve 68:328e1be06f5d 796 CONTROL_UTILITY_MODE = 1;
Lightvalve 68:328e1be06f5d 797 break;
Lightvalve 68:328e1be06f5d 798 }
Lightvalve 169:645207e160ca 799 case CRX_SET_FREQ_TEST: {
Lightvalve 169:645207e160ca 800 cnt_freq_test = 0;
Lightvalve 169:645207e160ca 801 CONTROL_UTILITY_MODE = 34;
Lightvalve 169:645207e160ca 802 break;
Lightvalve 169:645207e160ca 803 }
Lightvalve 170:42c938a40313 804
Lightvalve 169:645207e160ca 805 case CRX_ASK_BUFFER: {
Lightvalve 169:645207e160ca 806 cnt_buffer = (int16_t) (msg.data[1] | msg.data[2] << 8);
Lightvalve 169:645207e160ca 807 CAN_TX_BUFFER(cnt_buffer);
Lightvalve 169:645207e160ca 808 break;
Lightvalve 169:645207e160ca 809 }
Lightvalve 170:42c938a40313 810
Lightvalve 169:645207e160ca 811 case CRX_SET_STEP_TEST: {
Lightvalve 169:645207e160ca 812 cnt_step_test = 0;
Lightvalve 169:645207e160ca 813 CONTROL_UTILITY_MODE = 37;
Lightvalve 169:645207e160ca 814 break;
Lightvalve 169:645207e160ca 815 }
Lightvalve 38:118df027d851 816
Lightvalve 192:637092202815 817 case CRX_SET_CHANGE_EVERY_REFERNCE: {
Lightvalve 192:637092202815 818 flag_every_reference = 1;
Lightvalve 192:637092202815 819 break;
Lightvalve 192:637092202815 820 }
Lightvalve 192:637092202815 821
Lightvalve 170:42c938a40313 822 default:
Lightvalve 170:42c938a40313 823 break;
jobuuu 2:a1c0a37df760 824 }
jobuuu 2:a1c0a37df760 825 }
jobuuu 2:a1c0a37df760 826
jobuuu 2:a1c0a37df760 827 void CAN_RX_HANDLER()
jobuuu 2:a1c0a37df760 828 {
Lightvalve 170:42c938a40313 829
jobuuu 2:a1c0a37df760 830 can.read(msg);
jobuuu 2:a1c0a37df760 831 unsigned int address = msg.id;
Lightvalve 170:42c938a40313 832 if(address==CID_RX_CMD) {
jobuuu 2:a1c0a37df760 833 unsigned int CMD = msg.data[0];
jobuuu 2:a1c0a37df760 834 ReadCMD(CMD);
Lightvalve 170:42c938a40313 835
jobuuu 7:e9086c72bb22 836 } else if(address==CID_RX_REF_POSITION) {
Lightvalve 170:42c938a40313 837
Lightvalve 43:b084e5f5d0d5 838 int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
Lightvalve 43:b084e5f5d0d5 839 int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 8);
Lightvalve 57:f4819de54e7a 840 int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
Lightvalve 170:42c938a40313 841
Lightvalve 57:f4819de54e7a 842 if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
Lightvalve 57:f4819de54e7a 843 pos.ref = (double)temp_pos * 1.0f;
Lightvalve 57:f4819de54e7a 844 vel.ref = (double)temp_vel * 10.0f;
Lightvalve 170:42c938a40313 845 } else { //Linear Actuator
Lightvalve 256:7c8cc8b56b88 846 pos.ref = (double)temp_pos * 16.0f;
Lightvalve 67:c2812cf26c38 847 vel.ref = (double)temp_vel * 256.0f;
Lightvalve 57:f4819de54e7a 848 }
Lightvalve 170:42c938a40313 849
Lightvalve 68:328e1be06f5d 850 torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; //N
Lightvalve 133:22ab22818e01 851 torq.ref_diff = torq.ref - torq.ref_old;
Lightvalve 209:ebc69d6ee6f1 852 torq_dot.sen = torq.sen-totq_sen_past;
Lightvalve 133:22ab22818e01 853 torq.ref_old = torq.ref;
Lightvalve 209:ebc69d6ee6f1 854 totq_sen_past = torq.sen;
Lightvalve 170:42c938a40313 855
Lightvalve 68:328e1be06f5d 856 ///////////////Make Data///////////////////
Lightvalve 170:42c938a40313 857 for(int i=0; i<num_array_u_past-1; i++) {
Lightvalve 99:7bbcb3c0fb06 858 u_past[i] = u_past[i+1];
Lightvalve 99:7bbcb3c0fb06 859 }
Lightvalve 99:7bbcb3c0fb06 860 u_past[num_array_u_past-1] = output_normalized;
Lightvalve 170:42c938a40313 861
Lightvalve 170:42c938a40313 862 for(int i=0; i<num_array_x_past-1; i++) {
Lightvalve 68:328e1be06f5d 863 x_past[i] = x_past[i+1];
Lightvalve 68:328e1be06f5d 864 }
Lightvalve 68:328e1be06f5d 865 x_past[num_array_x_past-1] = pos.sen / ENC_PULSE_PER_POSITION; //mm
Lightvalve 170:42c938a40313 866
Lightvalve 122:dcb3ce3056a0 867 // for(int i=0;i<num_array_x_future-1;i++){
Lightvalve 122:dcb3ce3056a0 868 // x_future[i] = x_future[i+1];
Lightvalve 122:dcb3ce3056a0 869 // }
Lightvalve 122:dcb3ce3056a0 870 // x_future[num_array_x_future-1] = pos.sen / ENC_PULSE_PER_POSITION; //mm
Lightvalve 170:42c938a40313 871
Lightvalve 170:42c938a40313 872 for(int i=0; i<num_array_f_past-1; i++) {
Lightvalve 68:328e1be06f5d 873 f_past[i] = f_past[i+1];
Lightvalve 68:328e1be06f5d 874 }
Lightvalve 101:50159049a518 875 f_past[num_array_f_past-1] = torq.sen; //N
Lightvalve 170:42c938a40313 876
Lightvalve 68:328e1be06f5d 877 f_future[0] = torq.sen; //N
Lightvalve 192:637092202815 878
Lightvalve 192:637092202815 879 if(flag_every_reference == 1) {
Lightvalve 192:637092202815 880 for(int i=1; i<num_array_f_future-1; i++) {
Lightvalve 192:637092202815 881 f_future[i] = 500.0f;
Lightvalve 192:637092202815 882 }
Lightvalve 192:637092202815 883 f_future[num_array_f_future-1] = 500.0f; //N
Lightvalve 192:637092202815 884 } else {
Lightvalve 192:637092202815 885 for(int i=1; i<num_array_f_future-1; i++) {
Lightvalve 192:637092202815 886 f_future[i] = f_future[i+1];
Lightvalve 192:637092202815 887 }
Lightvalve 192:637092202815 888 f_future[num_array_f_future-1] = torq.ref; //N
Lightvalve 68:328e1be06f5d 889 }
Lightvalve 68:328e1be06f5d 890
Lightvalve 101:50159049a518 891
Lightvalve 49:e7bcfc244d40 892 } else if(address==CID_RX_REF_VALVE_POS) {
Lightvalve 47:fdcb8bd86fd6 893 int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
Lightvalve 170:42c938a40313 894
Lightvalve 57:f4819de54e7a 895 if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve
Lightvalve 57:f4819de54e7a 896 valve_pos.ref = (double) temp_ref_valve_pos;
Lightvalve 57:f4819de54e7a 897 } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve
Lightvalve 57:f4819de54e7a 898 valve_pos.ref = (double) temp_ref_valve_pos;
Lightvalve 57:f4819de54e7a 899 } else { //SW Valve
Lightvalve 57:f4819de54e7a 900 if(temp_ref_valve_pos >= 0) {
Lightvalve 57:f4819de54e7a 901 valve_pos.ref = (double)VALVE_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f;
Lightvalve 57:f4819de54e7a 902 } else {
Lightvalve 57:f4819de54e7a 903 valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
Lightvalve 57:f4819de54e7a 904 }
Lightvalve 47:fdcb8bd86fd6 905 }
Lightvalve 170:42c938a40313 906 } else if(address==CID_RX_REF_PWM) {
Lightvalve 49:e7bcfc244d40 907 int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
Lightvalve 49:e7bcfc244d40 908 Vout.ref = (double) temp_ref_pwm;
Lightvalve 170:42c938a40313 909 }
jobuuu 2:a1c0a37df760 910 }
jobuuu 2:a1c0a37df760 911
jobuuu 2:a1c0a37df760 912 /******************************************************************************
jobuuu 2:a1c0a37df760 913 Information Transmission Functions
jobuuu 2:a1c0a37df760 914 *******************************************************************************/
Lightvalve 11:82d8768d7351 915
Lightvalve 170:42c938a40313 916 void CAN_TX_INFO(void)
Lightvalve 170:42c938a40313 917 {
Lightvalve 12:6f2531038ea4 918 CANMessage temp_msg;
Lightvalve 170:42c938a40313 919
jobuuu 2:a1c0a37df760 920 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 921 temp_msg.len = 7;
Lightvalve 12:6f2531038ea4 922 temp_msg.data[0] = (uint8_t) CTX_SEND_INFO;
Lightvalve 12:6f2531038ea4 923 temp_msg.data[1] = (uint8_t) BNO;
Lightvalve 12:6f2531038ea4 924 temp_msg.data[2] = (uint8_t) CAN_FREQ;
Lightvalve 12:6f2531038ea4 925 temp_msg.data[3] = (uint8_t) (CAN_FREQ >> 8);
Lightvalve 12:6f2531038ea4 926 temp_msg.data[4] = (uint8_t) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
Lightvalve 57:f4819de54e7a 927 temp_msg.data[5] = (uint8_t) CONTROL_UTILITY_MODE;
Lightvalve 12:6f2531038ea4 928 temp_msg.data[6] = (uint8_t) OPERATING_MODE;
jobuuu 2:a1c0a37df760 929
jobuuu 2:a1c0a37df760 930 can.write(temp_msg);
jobuuu 2:a1c0a37df760 931 }
jobuuu 2:a1c0a37df760 932
Lightvalve 170:42c938a40313 933 void CAN_TX_BNO(void)
Lightvalve 170:42c938a40313 934 {
Lightvalve 12:6f2531038ea4 935 CANMessage temp_msg;
Lightvalve 170:42c938a40313 936
jobuuu 2:a1c0a37df760 937 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 938 temp_msg.len = 2;
Lightvalve 12:6f2531038ea4 939 temp_msg.data[0] = (uint8_t) CTX_SEND_BNO;
Lightvalve 12:6f2531038ea4 940 temp_msg.data[1] = (uint8_t) BNO;
jobuuu 2:a1c0a37df760 941
jobuuu 2:a1c0a37df760 942 can.write(temp_msg);
jobuuu 2:a1c0a37df760 943 }
jobuuu 2:a1c0a37df760 944
Lightvalve 170:42c938a40313 945 void CAN_TX_OPERATING_MODE(void)
Lightvalve 170:42c938a40313 946 {
jobuuu 2:a1c0a37df760 947 CANMessage temp_msg;
Lightvalve 170:42c938a40313 948
jobuuu 2:a1c0a37df760 949 temp_msg.id = CID_TX_INFO;
Lightvalve 52:8ea76864368a 950 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 951 temp_msg.data[0] = (uint8_t) CTX_SEND_OPERATING_MODE;
Lightvalve 12:6f2531038ea4 952 temp_msg.data[1] = (uint8_t) OPERATING_MODE;
Lightvalve 52:8ea76864368a 953 temp_msg.data[2] = (uint8_t) SENSING_MODE;
Lightvalve 52:8ea76864368a 954 temp_msg.data[3] = (uint8_t) CURRENT_CONTROL_MODE;
Lightvalve 52:8ea76864368a 955 temp_msg.data[4] = (uint8_t) FLAG_VALVE_DEADZONE;
jobuuu 2:a1c0a37df760 956
jobuuu 2:a1c0a37df760 957 can.write(temp_msg);
jobuuu 2:a1c0a37df760 958 }
jobuuu 2:a1c0a37df760 959
Lightvalve 170:42c938a40313 960 void CAN_TX_CAN_FREQ(void)
Lightvalve 170:42c938a40313 961 {
jobuuu 2:a1c0a37df760 962 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 963
jobuuu 2:a1c0a37df760 964 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 965 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 966 temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ;
jobuuu 2:a1c0a37df760 967 temp_msg.data[1] = (uint8_t) CAN_FREQ;
jobuuu 2:a1c0a37df760 968 temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8);
jobuuu 2:a1c0a37df760 969
jobuuu 2:a1c0a37df760 970 can.write(temp_msg);
jobuuu 2:a1c0a37df760 971 }
Lightvalve 11:82d8768d7351 972
Lightvalve 170:42c938a40313 973 void CAN_TX_CONTROL_MODE(void)
Lightvalve 170:42c938a40313 974 {
jobuuu 2:a1c0a37df760 975 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 976
jobuuu 2:a1c0a37df760 977 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 978 temp_msg.len = 2;
Lightvalve 12:6f2531038ea4 979 temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
Lightvalve 57:f4819de54e7a 980 temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
jobuuu 2:a1c0a37df760 981
jobuuu 2:a1c0a37df760 982 can.write(temp_msg);
jobuuu 2:a1c0a37df760 983 }
Lightvalve 11:82d8768d7351 984
Lightvalve 170:42c938a40313 985 void CAN_TX_JOINT_ENC_DIR(void)
Lightvalve 170:42c938a40313 986 {
jobuuu 2:a1c0a37df760 987 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 988
jobuuu 2:a1c0a37df760 989 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 990 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 991 temp_msg.data[0] = (uint8_t) CTX_SEND_JOINT_ENC_DIR;
Lightvalve 12:6f2531038ea4 992 temp_msg.data[1] = (uint8_t) DIR_JOINT_ENC;
Lightvalve 12:6f2531038ea4 993 temp_msg.data[2] = (uint8_t) (DIR_JOINT_ENC >> 8);
jobuuu 2:a1c0a37df760 994
jobuuu 2:a1c0a37df760 995 can.write(temp_msg);
jobuuu 2:a1c0a37df760 996 }
jobuuu 2:a1c0a37df760 997
Lightvalve 170:42c938a40313 998 void CAN_TX_VALVE_DIR(void)
Lightvalve 170:42c938a40313 999 {
jobuuu 2:a1c0a37df760 1000 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1001
jobuuu 2:a1c0a37df760 1002 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1003 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1004 temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DIR;
Lightvalve 12:6f2531038ea4 1005 temp_msg.data[1] = (uint8_t) DIR_VALVE;
Lightvalve 12:6f2531038ea4 1006 temp_msg.data[2] = (uint8_t) (DIR_VALVE >> 8);
jobuuu 2:a1c0a37df760 1007
jobuuu 2:a1c0a37df760 1008 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1009 }
jobuuu 2:a1c0a37df760 1010
Lightvalve 170:42c938a40313 1011 void CAN_TX_VALVE_ENC_DIR(void)
Lightvalve 170:42c938a40313 1012 {
jobuuu 2:a1c0a37df760 1013 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1014
jobuuu 2:a1c0a37df760 1015 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1016 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1017 temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_ENC_DIR;
Lightvalve 12:6f2531038ea4 1018 temp_msg.data[1] = (uint8_t) DIR_VALVE_ENC;
Lightvalve 12:6f2531038ea4 1019 temp_msg.data[2] = (uint8_t) (DIR_VALVE_ENC >> 8);
jobuuu 2:a1c0a37df760 1020
jobuuu 2:a1c0a37df760 1021 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1022 }
jobuuu 2:a1c0a37df760 1023
Lightvalve 170:42c938a40313 1024 void CAN_TX_VOLTAGE_SUPPLY(void)
Lightvalve 170:42c938a40313 1025 {
Lightvalve 49:e7bcfc244d40 1026 int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10.0f);
jobuuu 2:a1c0a37df760 1027
jobuuu 2:a1c0a37df760 1028 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1029
jobuuu 2:a1c0a37df760 1030 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1031 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1032 temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_SUPPLY;
Lightvalve 12:6f2531038ea4 1033 temp_msg.data[1] = (uint8_t) (send_voltage_supply);
Lightvalve 12:6f2531038ea4 1034 temp_msg.data[2] = (uint8_t) (send_voltage_supply >> 8);
jobuuu 2:a1c0a37df760 1035
jobuuu 2:a1c0a37df760 1036 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1037 }
jobuuu 2:a1c0a37df760 1038
Lightvalve 170:42c938a40313 1039 void CAN_TX_VOLTAGE_VALVE(void)
Lightvalve 170:42c938a40313 1040 {
Lightvalve 49:e7bcfc244d40 1041 int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f);
jobuuu 2:a1c0a37df760 1042
jobuuu 2:a1c0a37df760 1043 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1044
jobuuu 2:a1c0a37df760 1045 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1046 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1047 temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_VALVE;
Lightvalve 12:6f2531038ea4 1048 temp_msg.data[1] = (uint8_t) send_voltage_valve;
Lightvalve 12:6f2531038ea4 1049 temp_msg.data[2] = (uint8_t) (send_voltage_valve >> 8);
jobuuu 2:a1c0a37df760 1050
jobuuu 2:a1c0a37df760 1051 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1052 }
Lightvalve 11:82d8768d7351 1053
jobuuu 2:a1c0a37df760 1054
Lightvalve 170:42c938a40313 1055 void CAN_TX_PID_GAIN(int t_type)
Lightvalve 170:42c938a40313 1056 {
jobuuu 2:a1c0a37df760 1057 // t_type = 0 : valve position control gain
jobuuu 2:a1c0a37df760 1058 // t_type = 1 : joint position control gain
jobuuu 2:a1c0a37df760 1059 // t_type = 2 : joint torque control gain
jobuuu 2:a1c0a37df760 1060
Lightvalve 12:6f2531038ea4 1061 int16_t sendPgain=0, sendIgain=0, sendDgain=0;
jobuuu 2:a1c0a37df760 1062 if (t_type == 0) {
Lightvalve 30:8d561f16383b 1063 sendPgain = (int16_t) (P_GAIN_VALVE_POSITION);
Lightvalve 30:8d561f16383b 1064 sendIgain = (int16_t) (I_GAIN_VALVE_POSITION);
Lightvalve 30:8d561f16383b 1065 sendDgain = (int16_t) (D_GAIN_VALVE_POSITION);
jobuuu 2:a1c0a37df760 1066 } else if (t_type == 1) {
Lightvalve 12:6f2531038ea4 1067 sendPgain = (int16_t) (P_GAIN_JOINT_POSITION);
Lightvalve 12:6f2531038ea4 1068 sendIgain = (int16_t) (I_GAIN_JOINT_POSITION);
Lightvalve 12:6f2531038ea4 1069 sendDgain = (int16_t) (D_GAIN_JOINT_POSITION);
jobuuu 2:a1c0a37df760 1070 } else if (t_type == 2) {
Lightvalve 12:6f2531038ea4 1071 sendPgain = (int16_t) (P_GAIN_JOINT_TORQUE);
Lightvalve 12:6f2531038ea4 1072 sendIgain = (int16_t) (I_GAIN_JOINT_TORQUE);
Lightvalve 12:6f2531038ea4 1073 sendDgain = (int16_t) (D_GAIN_JOINT_TORQUE);
Lightvalve 46:2694daea349b 1074 } else if (t_type == 3) {
Lightvalve 67:c2812cf26c38 1075 sendPgain = (int16_t) (K_SPRING * 10.0f);
Lightvalve 67:c2812cf26c38 1076 sendIgain = (int16_t) (D_DAMPER * 100.0f);
jobuuu 2:a1c0a37df760 1077 }
jobuuu 2:a1c0a37df760 1078
jobuuu 2:a1c0a37df760 1079 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1080
jobuuu 2:a1c0a37df760 1081 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1082 temp_msg.len = 8;
Lightvalve 12:6f2531038ea4 1083 temp_msg.data[0] = (uint8_t) CTX_SEND_PID_GAIN;
Lightvalve 12:6f2531038ea4 1084 temp_msg.data[1] = (uint8_t) t_type;
Lightvalve 12:6f2531038ea4 1085 temp_msg.data[2] = (uint8_t) sendPgain;
Lightvalve 12:6f2531038ea4 1086 temp_msg.data[3] = (uint8_t) (sendPgain >> 8);
Lightvalve 12:6f2531038ea4 1087 temp_msg.data[4] = (uint8_t) sendIgain;
Lightvalve 12:6f2531038ea4 1088 temp_msg.data[5] = (uint8_t) (sendIgain >> 8);
Lightvalve 12:6f2531038ea4 1089 temp_msg.data[6] = (uint8_t) sendDgain;
Lightvalve 12:6f2531038ea4 1090 temp_msg.data[7] = (uint8_t) (sendDgain >> 8);
jobuuu 2:a1c0a37df760 1091
jobuuu 2:a1c0a37df760 1092 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1093 }
jobuuu 2:a1c0a37df760 1094
Lightvalve 11:82d8768d7351 1095
Lightvalve 170:42c938a40313 1096 void CAN_TX_VALVE_DEADZONE(void)
Lightvalve 170:42c938a40313 1097 {
jobuuu 2:a1c0a37df760 1098 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1099
jobuuu 2:a1c0a37df760 1100 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1101 temp_msg.len = 7;
Lightvalve 12:6f2531038ea4 1102 temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
Lightvalve 57:f4819de54e7a 1103 temp_msg.data[1] = (uint8_t) VALVE_CENTER;
Lightvalve 57:f4819de54e7a 1104 temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8);
Lightvalve 12:6f2531038ea4 1105 temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS);
Lightvalve 12:6f2531038ea4 1106 temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8);
Lightvalve 12:6f2531038ea4 1107 temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS);
Lightvalve 12:6f2531038ea4 1108 temp_msg.data[6] = (uint8_t) ((int) (VALVE_DEADZONE_MINUS) >> 8);
jobuuu 2:a1c0a37df760 1109
jobuuu 2:a1c0a37df760 1110 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1111 }
jobuuu 2:a1c0a37df760 1112
Lightvalve 170:42c938a40313 1113 void CAN_TX_VELOCITY_COMP_GAIN(void)
Lightvalve 170:42c938a40313 1114 {
Lightvalve 11:82d8768d7351 1115 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1116
Lightvalve 11:82d8768d7351 1117 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1118 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1119 temp_msg.data[0] = (uint8_t) CTX_SEND_VELOCITY_COMP_GAIN;
Lightvalve 12:6f2531038ea4 1120 temp_msg.data[1] = (uint8_t) VELOCITY_COMP_GAIN;
Lightvalve 12:6f2531038ea4 1121 temp_msg.data[2] = (uint8_t) (VELOCITY_COMP_GAIN >> 8);
Lightvalve 11:82d8768d7351 1122
Lightvalve 11:82d8768d7351 1123 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1124 }
Lightvalve 11:82d8768d7351 1125
Lightvalve 170:42c938a40313 1126 void CAN_TX_COMPLIANCE_GAIN(void)
Lightvalve 170:42c938a40313 1127 {
Lightvalve 11:82d8768d7351 1128 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1129
Lightvalve 11:82d8768d7351 1130 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1131 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1132 temp_msg.data[0] = (uint8_t) CTX_SEND_COMPLIANCE_GAIN;
Lightvalve 12:6f2531038ea4 1133 temp_msg.data[1] = (uint8_t) COMPLIANCE_GAIN;
Lightvalve 12:6f2531038ea4 1134 temp_msg.data[2] = (uint8_t) (COMPLIANCE_GAIN >> 8);
Lightvalve 11:82d8768d7351 1135
Lightvalve 11:82d8768d7351 1136 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1137 }
Lightvalve 11:82d8768d7351 1138
Lightvalve 170:42c938a40313 1139 void CAN_TX_VALVE_FF(void)
Lightvalve 170:42c938a40313 1140 {
Lightvalve 11:82d8768d7351 1141 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1142
Lightvalve 11:82d8768d7351 1143 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1144 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1145 temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_FF;
Lightvalve 12:6f2531038ea4 1146 temp_msg.data[1] = (uint8_t) VALVE_FF;
Lightvalve 12:6f2531038ea4 1147 temp_msg.data[2] = (uint8_t) (VALVE_FF >> 8);
Lightvalve 11:82d8768d7351 1148
Lightvalve 11:82d8768d7351 1149 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1150 }
Lightvalve 11:82d8768d7351 1151
Lightvalve 170:42c938a40313 1152 void CAN_TX_BULK_MODULUS(void)
Lightvalve 170:42c938a40313 1153 {
Lightvalve 11:82d8768d7351 1154 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1155
Lightvalve 11:82d8768d7351 1156 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1157 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1158 temp_msg.data[0] = (uint8_t) CTX_SEND_BULK_MODULUS;
Lightvalve 12:6f2531038ea4 1159 temp_msg.data[1] = (uint8_t) BULK_MODULUS;
Lightvalve 12:6f2531038ea4 1160 temp_msg.data[2] = (uint8_t) (BULK_MODULUS >> 8);
Lightvalve 11:82d8768d7351 1161
Lightvalve 11:82d8768d7351 1162 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1163 }
Lightvalve 11:82d8768d7351 1164
Lightvalve 170:42c938a40313 1165 void CAN_TX_CHAMBER_VOLUME(void)
Lightvalve 170:42c938a40313 1166 {
Lightvalve 11:82d8768d7351 1167 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1168
Lightvalve 11:82d8768d7351 1169 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1170 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 1171 temp_msg.data[0] = (uint8_t) CTX_SEND_CHAMBER_VOLUME;
Lightvalve 12:6f2531038ea4 1172 temp_msg.data[1] = (uint8_t) CHAMBER_VOLUME_A;
Lightvalve 12:6f2531038ea4 1173 temp_msg.data[2] = (uint8_t) (CHAMBER_VOLUME_A >> 8);
Lightvalve 12:6f2531038ea4 1174 temp_msg.data[3] = (uint8_t) CHAMBER_VOLUME_B;
Lightvalve 12:6f2531038ea4 1175 temp_msg.data[4] = (uint8_t) (CHAMBER_VOLUME_B >> 8);
Lightvalve 11:82d8768d7351 1176
Lightvalve 11:82d8768d7351 1177 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1178 }
Lightvalve 11:82d8768d7351 1179
Lightvalve 170:42c938a40313 1180 void CAN_TX_PISTON_AREA(void)
Lightvalve 170:42c938a40313 1181 {
jobuuu 2:a1c0a37df760 1182 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1183
Lightvalve 11:82d8768d7351 1184 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1185 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 1186 temp_msg.data[0] = (uint8_t) CTX_SEND_PISTON_AREA;
Lightvalve 12:6f2531038ea4 1187 temp_msg.data[1] = (uint8_t) PISTON_AREA_A;
Lightvalve 12:6f2531038ea4 1188 temp_msg.data[2] = (uint8_t) (PISTON_AREA_A >> 8);
Lightvalve 12:6f2531038ea4 1189 temp_msg.data[3] = (uint8_t) PISTON_AREA_B;
Lightvalve 12:6f2531038ea4 1190 temp_msg.data[4] = (uint8_t) (PISTON_AREA_B >> 8);
jobuuu 2:a1c0a37df760 1191
Lightvalve 11:82d8768d7351 1192 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1193 }
Lightvalve 11:82d8768d7351 1194
Lightvalve 170:42c938a40313 1195 void CAN_TX_PRES_A_AND_B(void)
Lightvalve 170:42c938a40313 1196 {
Lightvalve 11:82d8768d7351 1197 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1198
Lightvalve 11:82d8768d7351 1199 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1200 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 1201 temp_msg.data[0] = (uint8_t) CTX_SEND_PRES;
Lightvalve 12:6f2531038ea4 1202 temp_msg.data[1] = (uint8_t) PRES_SUPPLY;
Lightvalve 12:6f2531038ea4 1203 temp_msg.data[2] = (uint8_t) (PRES_SUPPLY >> 8);
Lightvalve 12:6f2531038ea4 1204 temp_msg.data[3] = (uint8_t) PRES_RETURN;
Lightvalve 12:6f2531038ea4 1205 temp_msg.data[4] = (uint8_t) (PRES_RETURN >> 8);
Lightvalve 11:82d8768d7351 1206
Lightvalve 11:82d8768d7351 1207 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1208 }
Lightvalve 11:82d8768d7351 1209
Lightvalve 170:42c938a40313 1210 void CAN_TX_ENC_LIMIT(void)
Lightvalve 170:42c938a40313 1211 {
Lightvalve 11:82d8768d7351 1212 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1213
Lightvalve 11:82d8768d7351 1214 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1215 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 1216 temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_LIMIT;
Lightvalve 12:6f2531038ea4 1217 temp_msg.data[1] = (uint8_t) ENC_LIMIT_MINUS;
Lightvalve 12:6f2531038ea4 1218 temp_msg.data[2] = (uint8_t) (ENC_LIMIT_MINUS >> 8);
Lightvalve 12:6f2531038ea4 1219 temp_msg.data[3] = (uint8_t) ENC_LIMIT_PLUS;
Lightvalve 12:6f2531038ea4 1220 temp_msg.data[4] = (uint8_t) (ENC_LIMIT_PLUS >> 8);
Lightvalve 11:82d8768d7351 1221
Lightvalve 11:82d8768d7351 1222 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1223 }
Lightvalve 11:82d8768d7351 1224
Lightvalve 170:42c938a40313 1225 void CAN_TX_STROKE(void)
Lightvalve 170:42c938a40313 1226 {
Lightvalve 11:82d8768d7351 1227 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1228
Lightvalve 11:82d8768d7351 1229 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1230 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1231 temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE;
Lightvalve 12:6f2531038ea4 1232 temp_msg.data[1] = (uint8_t) STROKE;
Lightvalve 12:6f2531038ea4 1233 temp_msg.data[2] = (uint8_t) (STROKE >> 8);
Lightvalve 11:82d8768d7351 1234
Lightvalve 11:82d8768d7351 1235 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1236 }
Lightvalve 11:82d8768d7351 1237
Lightvalve 170:42c938a40313 1238 void CAN_TX_VALVE_LIMIT(void)
Lightvalve 170:42c938a40313 1239 {
Lightvalve 11:82d8768d7351 1240 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1241
Lightvalve 11:82d8768d7351 1242 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1243 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 1244 temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
Lightvalve 57:f4819de54e7a 1245 temp_msg.data[1] = (uint8_t) VALVE_MIN_POS;
Lightvalve 57:f4819de54e7a 1246 temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8);
Lightvalve 57:f4819de54e7a 1247 temp_msg.data[3] = (uint8_t) VALVE_MAX_POS;
Lightvalve 57:f4819de54e7a 1248 temp_msg.data[4] = (uint8_t) (VALVE_MAX_POS >> 8);
Lightvalve 11:82d8768d7351 1249
Lightvalve 11:82d8768d7351 1250 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1251 }
Lightvalve 11:82d8768d7351 1252
Lightvalve 170:42c938a40313 1253 void CAN_TX_ENC_PULSE_PER_POSITION(void)
Lightvalve 170:42c938a40313 1254 {
Lightvalve 11:82d8768d7351 1255 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1256
jobuuu 2:a1c0a37df760 1257 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1258 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1259 temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
Lightvalve 54:647072f5307a 1260 int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION);
Lightvalve 48:889798ff9329 1261 temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position;
Lightvalve 48:889798ff9329 1262 temp_msg.data[2] = (uint8_t) (temp_enc_pulse_per_position >> 8);
jobuuu 2:a1c0a37df760 1263
jobuuu 2:a1c0a37df760 1264 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1265 }
jobuuu 2:a1c0a37df760 1266
Lightvalve 170:42c938a40313 1267 void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void)
Lightvalve 170:42c938a40313 1268 {
jobuuu 2:a1c0a37df760 1269 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1270
jobuuu 2:a1c0a37df760 1271 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1272 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1273 temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve 139:15621998925b 1274 int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f);
Lightvalve 48:889798ff9329 1275 temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
Lightvalve 48:889798ff9329 1276 temp_msg.data[2] = (uint8_t) (temp_torque_sensor_pulse_per_torque >> 8);
jobuuu 2:a1c0a37df760 1277
jobuuu 2:a1c0a37df760 1278 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1279 }
jobuuu 2:a1c0a37df760 1280
Lightvalve 170:42c938a40313 1281 void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void)
Lightvalve 170:42c938a40313 1282 {
jobuuu 2:a1c0a37df760 1283 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1284
jobuuu 2:a1c0a37df760 1285 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1286 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 1287 temp_msg.data[0] = (uint8_t) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
Lightvalve 30:8d561f16383b 1288 temp_msg.data[1] = (uint8_t) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f);
Lightvalve 30:8d561f16383b 1289 temp_msg.data[2] = (uint8_t) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f) >> 8);
Lightvalve 30:8d561f16383b 1290 temp_msg.data[3] = (uint8_t) (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f);
Lightvalve 30:8d561f16383b 1291 temp_msg.data[4] = (uint8_t) ((int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f) >> 8);
jobuuu 2:a1c0a37df760 1292
jobuuu 2:a1c0a37df760 1293 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1294 }
jobuuu 2:a1c0a37df760 1295
Lightvalve 170:42c938a40313 1296 void CAN_TX_FRICTION(void)
Lightvalve 170:42c938a40313 1297 {
Lightvalve 11:82d8768d7351 1298 CANMessage temp_msg;
Lightvalve 12:6f2531038ea4 1299 int16_t send_friction;
Lightvalve 30:8d561f16383b 1300 send_friction = (int16_t) (FRICTION * 10.0f);
Lightvalve 170:42c938a40313 1301
Lightvalve 11:82d8768d7351 1302 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1303 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1304 temp_msg.data[0] = (uint8_t) CTX_SEND_FRICTION;
Lightvalve 12:6f2531038ea4 1305 temp_msg.data[1] = (uint8_t) send_friction;
Lightvalve 12:6f2531038ea4 1306 temp_msg.data[2] = (uint8_t) (send_friction >> 8);
Lightvalve 11:82d8768d7351 1307
Lightvalve 11:82d8768d7351 1308 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1309 }
Lightvalve 11:82d8768d7351 1310
Lightvalve 170:42c938a40313 1311 void CAN_TX_VALVE_GAIN_PLUS(void)
Lightvalve 170:42c938a40313 1312 {
Lightvalve 11:82d8768d7351 1313 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1314
Lightvalve 11:82d8768d7351 1315 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1316 temp_msg.len = 6;
Lightvalve 12:6f2531038ea4 1317 temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_PLUS;
Lightvalve 30:8d561f16383b 1318 temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[0] * 50.0f);
Lightvalve 30:8d561f16383b 1319 temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[2] * 50.0f);
Lightvalve 30:8d561f16383b 1320 temp_msg.data[3] = (uint8_t) (VALVE_GAIN_LPM_PER_V[4] * 50.0f);
Lightvalve 30:8d561f16383b 1321 temp_msg.data[4] = (uint8_t) (VALVE_GAIN_LPM_PER_V[6] * 50.0f);
Lightvalve 30:8d561f16383b 1322 temp_msg.data[5] = (uint8_t) (VALVE_GAIN_LPM_PER_V[8] * 50.0f);
Lightvalve 11:82d8768d7351 1323
Lightvalve 11:82d8768d7351 1324 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1325 }
Lightvalve 11:82d8768d7351 1326
Lightvalve 170:42c938a40313 1327 void CAN_TX_DDV_VALVE_DEADZONE(void)
Lightvalve 170:42c938a40313 1328 {
Lightvalve 11:82d8768d7351 1329 CANMessage temp_msg;
Lightvalve 48:889798ff9329 1330 float temp_valve_deadzone_minus = 0.0f;
Lightvalve 48:889798ff9329 1331 float temp_valve_deadzone_plus = 0.0f;
Lightvalve 48:889798ff9329 1332 float temp_ddv_center = 0.0f;
Lightvalve 170:42c938a40313 1333
Lightvalve 57:f4819de54e7a 1334 //temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
Lightvalve 57:f4819de54e7a 1335 //temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
Lightvalve 57:f4819de54e7a 1336 temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS;
Lightvalve 57:f4819de54e7a 1337 temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS;
Lightvalve 57:f4819de54e7a 1338 temp_ddv_center = (double)VALVE_CENTER;
Lightvalve 170:42c938a40313 1339
Lightvalve 11:82d8768d7351 1340 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1341 temp_msg.len = 7;
Lightvalve 12:6f2531038ea4 1342 temp_msg.data[0] = (uint8_t) CTX_SEND_DDV_VALVE_DEADZONE;
Lightvalve 48:889798ff9329 1343 temp_msg.data[1] = (uint8_t) temp_valve_deadzone_minus;
Lightvalve 48:889798ff9329 1344 temp_msg.data[2] = (uint8_t) ((int) (temp_valve_deadzone_minus) >> 8);
Lightvalve 48:889798ff9329 1345 temp_msg.data[3] = (uint8_t) (temp_valve_deadzone_plus);
Lightvalve 48:889798ff9329 1346 temp_msg.data[4] = (uint8_t) ((int) (temp_valve_deadzone_plus) >> 8);
Lightvalve 48:889798ff9329 1347 temp_msg.data[5] = (uint8_t) (temp_ddv_center);
Lightvalve 48:889798ff9329 1348 temp_msg.data[6] = (uint8_t) ((int) (temp_ddv_center) >> 8);
Lightvalve 11:82d8768d7351 1349
Lightvalve 11:82d8768d7351 1350 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1351 }
Lightvalve 11:82d8768d7351 1352
Lightvalve 170:42c938a40313 1353 void CAN_TX_VALVE_GAIN_MINUS(void)
Lightvalve 170:42c938a40313 1354 {
Lightvalve 11:82d8768d7351 1355 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1356
Lightvalve 11:82d8768d7351 1357 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1358 temp_msg.len = 6;
Lightvalve 12:6f2531038ea4 1359 temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_MINUS;
Lightvalve 30:8d561f16383b 1360 temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[1] * 50.0f);
Lightvalve 30:8d561f16383b 1361 temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[3] * 50.0f);
Lightvalve 30:8d561f16383b 1362 temp_msg.data[3] = (uint8_t) (VALVE_GAIN_LPM_PER_V[5] * 50.0f);
Lightvalve 30:8d561f16383b 1363 temp_msg.data[4] = (uint8_t) (VALVE_GAIN_LPM_PER_V[7] * 50.0f);
Lightvalve 30:8d561f16383b 1364 temp_msg.data[5] = (uint8_t) (VALVE_GAIN_LPM_PER_V[9] * 50.0f);
Lightvalve 11:82d8768d7351 1365
Lightvalve 11:82d8768d7351 1366 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1367 }
Lightvalve 11:82d8768d7351 1368
Lightvalve 170:42c938a40313 1369 void CAN_TX_REFENCE_MODE(void)
Lightvalve 170:42c938a40313 1370 {
Lightvalve 11:82d8768d7351 1371 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1372
Lightvalve 11:82d8768d7351 1373 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1374 temp_msg.len = 6;
Lightvalve 12:6f2531038ea4 1375 temp_msg.data[0] = (uint8_t) CTX_SEND_REFENCE_MODE;
Lightvalve 12:6f2531038ea4 1376 temp_msg.data[1] = (uint8_t) REFERENCE_MODE;
Lightvalve 30:8d561f16383b 1377 temp_msg.data[2] = (uint8_t) (int) (REFERENCE_FREQ * 100.0f);
Lightvalve 30:8d561f16383b 1378 temp_msg.data[3] = (uint8_t) ((int) (REFERENCE_FREQ * 100.0f) >> 8);
Lightvalve 30:8d561f16383b 1379 temp_msg.data[4] = (uint8_t) (int) (REFERENCE_MAG * 100.0f);
Lightvalve 30:8d561f16383b 1380 temp_msg.data[5] = (uint8_t) ((int) (REFERENCE_MAG * 100.0f) >> 8);
Lightvalve 11:82d8768d7351 1381
Lightvalve 11:82d8768d7351 1382 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1383 }
Lightvalve 11:82d8768d7351 1384
Lightvalve 170:42c938a40313 1385 void CAN_TX_HOMEPOS_OFFSET(void)
Lightvalve 170:42c938a40313 1386 {
Lightvalve 11:82d8768d7351 1387 CANMessage temp_msg;
Lightvalve 12:6f2531038ea4 1388 int16_t send_homepos_offset;
Lightvalve 12:6f2531038ea4 1389 send_homepos_offset = (int16_t) (HOMEPOS_OFFSET);
Lightvalve 170:42c938a40313 1390
jobuuu 2:a1c0a37df760 1391 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 1392 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1393 temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_OFFSET;
Lightvalve 12:6f2531038ea4 1394 temp_msg.data[1] = (uint8_t) send_homepos_offset;
Lightvalve 12:6f2531038ea4 1395 temp_msg.data[2] = (uint8_t) (send_homepos_offset >> 8);
jobuuu 2:a1c0a37df760 1396
jobuuu 2:a1c0a37df760 1397 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1398 }
Lightvalve 11:82d8768d7351 1399
Lightvalve 170:42c938a40313 1400 void CAN_TX_HOMPOS_VALVE_OPENING(void)
Lightvalve 170:42c938a40313 1401 {
Lightvalve 11:82d8768d7351 1402 CANMessage temp_msg;
Lightvalve 12:6f2531038ea4 1403 int16_t send_homepos_valve_opening;
Lightvalve 12:6f2531038ea4 1404 send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING);
Lightvalve 170:42c938a40313 1405
Lightvalve 11:82d8768d7351 1406 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1407 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1408 temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_VALVE_OPENING;
Lightvalve 12:6f2531038ea4 1409 temp_msg.data[1] = (uint8_t) send_homepos_valve_opening;
Lightvalve 12:6f2531038ea4 1410 temp_msg.data[2] = (uint8_t) (send_homepos_valve_opening >> 8);
Lightvalve 11:82d8768d7351 1411
Lightvalve 11:82d8768d7351 1412 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1413 }
Lightvalve 11:82d8768d7351 1414
Lightvalve 170:42c938a40313 1415 void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex)
Lightvalve 170:42c938a40313 1416 {
Lightvalve 11:82d8768d7351 1417 CANMessage temp_msg;
Lightvalve 11:82d8768d7351 1418 int16_t valve_pos_vs_pwm;
Lightvalve 49:e7bcfc244d40 1419 // valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
Lightvalve 170:42c938a40313 1420
Lightvalve 57:f4819de54e7a 1421 if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) {
Lightvalve 57:f4819de54e7a 1422 valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
Lightvalve 49:e7bcfc244d40 1423 } else {
Lightvalve 57:f4819de54e7a 1424 valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
Lightvalve 49:e7bcfc244d40 1425 }
Lightvalve 170:42c938a40313 1426
Lightvalve 12:6f2531038ea4 1427 int16_t PWM_VALVE_ID;
Lightvalve 12:6f2531038ea4 1428 PWM_VALVE_ID = ID_index_array[canindex] * 1000;
Lightvalve 170:42c938a40313 1429
Lightvalve 11:82d8768d7351 1430 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1431 temp_msg.len = 5;
Lightvalve 12:6f2531038ea4 1432 temp_msg.data[0] = (uint8_t) CTX_VALVE_PWM_VS_VALVE_POS;
Lightvalve 12:6f2531038ea4 1433 temp_msg.data[1] = (uint8_t) PWM_VALVE_ID;
Lightvalve 12:6f2531038ea4 1434 temp_msg.data[2] = (uint8_t) (PWM_VALVE_ID >> 8);
Lightvalve 12:6f2531038ea4 1435 temp_msg.data[3] = (uint8_t) valve_pos_vs_pwm;
Lightvalve 12:6f2531038ea4 1436 temp_msg.data[4] = (uint8_t) (valve_pos_vs_pwm >> 8);
Lightvalve 11:82d8768d7351 1437
Lightvalve 11:82d8768d7351 1438 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1439 }
Lightvalve 11:82d8768d7351 1440
Lightvalve 170:42c938a40313 1441 void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex)
Lightvalve 170:42c938a40313 1442 {
Lightvalve 11:82d8768d7351 1443 CANMessage temp_msg;
Lightvalve 11:82d8768d7351 1444 int32_t valve_pos_vs_flowrate;
Lightvalve 11:82d8768d7351 1445 valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
Lightvalve 11:82d8768d7351 1446
Lightvalve 57:f4819de54e7a 1447 int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + VALVE_CENTER;
Lightvalve 49:e7bcfc244d40 1448 int16_t temp_valve_pos = 0;
Lightvalve 57:f4819de54e7a 1449 if(VALVE_POS_VALVE_ID>=VALVE_CENTER) {
Lightvalve 57:f4819de54e7a 1450 temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER));
Lightvalve 49:e7bcfc244d40 1451 } else {
Lightvalve 57:f4819de54e7a 1452 temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER));
Lightvalve 49:e7bcfc244d40 1453 }
Lightvalve 170:42c938a40313 1454
Lightvalve 11:82d8768d7351 1455 temp_msg.id = CID_TX_INFO;
Lightvalve 49:e7bcfc244d40 1456 temp_msg.len = 8;
Lightvalve 12:6f2531038ea4 1457 temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
Lightvalve 49:e7bcfc244d40 1458 temp_msg.data[1] = (uint8_t) temp_valve_pos;
Lightvalve 49:e7bcfc244d40 1459 temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8);
Lightvalve 49:e7bcfc244d40 1460 temp_msg.data[5] = (uint8_t) valve_pos_vs_flowrate;
Lightvalve 49:e7bcfc244d40 1461 temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 8);
Lightvalve 49:e7bcfc244d40 1462 temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 16);
Lightvalve 49:e7bcfc244d40 1463 temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 24);
Lightvalve 11:82d8768d7351 1464
Lightvalve 11:82d8768d7351 1465 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1466 }
Lightvalve 11:82d8768d7351 1467
Lightvalve 170:42c938a40313 1468 void CAN_TX_VALVE_POS_NUM(void)
Lightvalve 170:42c938a40313 1469 {
Lightvalve 11:82d8768d7351 1470 CANMessage temp_msg;
Lightvalve 11:82d8768d7351 1471 int32_t valve_pos_num;
Lightvalve 11:82d8768d7351 1472 valve_pos_num = (int16_t) VALVE_POS_NUM;
Lightvalve 170:42c938a40313 1473
Lightvalve 11:82d8768d7351 1474 temp_msg.id = CID_TX_INFO;
Lightvalve 11:82d8768d7351 1475 temp_msg.len = 3;
Lightvalve 12:6f2531038ea4 1476 temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_NUM;
Lightvalve 12:6f2531038ea4 1477 temp_msg.data[1] = (uint8_t) valve_pos_num;
Lightvalve 12:6f2531038ea4 1478 temp_msg.data[2] = (uint8_t) (valve_pos_num >> 8);
Lightvalve 11:82d8768d7351 1479
Lightvalve 11:82d8768d7351 1480 can.write(temp_msg);
Lightvalve 11:82d8768d7351 1481 }
Lightvalve 11:82d8768d7351 1482
Lightvalve 170:42c938a40313 1483 void CAN_TX_DDV_VALVE_MAX_MIN_POS(void)
Lightvalve 170:42c938a40313 1484 {
Lightvalve 38:118df027d851 1485 CANMessage temp_msg;
Lightvalve 170:42c938a40313 1486
Lightvalve 48:889798ff9329 1487 float temp_valve_max_pos = 0.0f;
Lightvalve 48:889798ff9329 1488 float temp_valve_min_pos = 0.0f;
Lightvalve 48:889798ff9329 1489 float temp_ddv_center = 0.0f;
Lightvalve 170:42c938a40313 1490
Lightvalve 57:f4819de54e7a 1491 temp_valve_max_pos = VALVE_MAX_POS;
Lightvalve 57:f4819de54e7a 1492 temp_valve_min_pos = VALVE_MIN_POS;
Lightvalve 57:f4819de54e7a 1493 temp_ddv_center = VALVE_CENTER;
Lightvalve 170:42c938a40313 1494
Lightvalve 38:118df027d851 1495 temp_msg.id = CID_TX_INFO;
Lightvalve 38:118df027d851 1496 temp_msg.len = 7;
Lightvalve 38:118df027d851 1497 temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS;
Lightvalve 48:889798ff9329 1498 temp_msg.data[1] = (uint8_t) temp_valve_max_pos;
Lightvalve 48:889798ff9329 1499 temp_msg.data[2] = (uint8_t) ((int) (temp_valve_max_pos) >> 8);
Lightvalve 48:889798ff9329 1500 temp_msg.data[3] = (uint8_t) (temp_valve_min_pos);
Lightvalve 48:889798ff9329 1501 temp_msg.data[4] = (uint8_t) ((int) (temp_valve_min_pos) >> 8);
Lightvalve 48:889798ff9329 1502 temp_msg.data[5] = (uint8_t) (temp_ddv_center);
Lightvalve 48:889798ff9329 1503 temp_msg.data[6] = (uint8_t) ((int) (temp_ddv_center) >> 8);
Lightvalve 38:118df027d851 1504
Lightvalve 38:118df027d851 1505 can.write(temp_msg);
Lightvalve 38:118df027d851 1506 }
Lightvalve 38:118df027d851 1507
Lightvalve 169:645207e160ca 1508
Lightvalve 170:42c938a40313 1509 void CAN_TX_BUFFER(int16_t t_cnt_buffer)
Lightvalve 170:42c938a40313 1510 {
Lightvalve 169:645207e160ca 1511 CANMessage temp_msg;
Lightvalve 169:645207e160ca 1512 int16_t send_pos_array, send_ref_array;
Lightvalve 169:645207e160ca 1513 send_pos_array = (int16_t) (pos_array[t_cnt_buffer]);
Lightvalve 169:645207e160ca 1514 send_ref_array = (int16_t) (ref_array[t_cnt_buffer]);
Lightvalve 170:42c938a40313 1515
Lightvalve 169:645207e160ca 1516 temp_msg.id = CID_TX_INFO;
Lightvalve 169:645207e160ca 1517 temp_msg.len = 5;
Lightvalve 169:645207e160ca 1518 temp_msg.data[0] = (uint8_t) CTX_SEND_BUFFER;
Lightvalve 169:645207e160ca 1519 temp_msg.data[1] = (uint8_t) send_pos_array;
Lightvalve 169:645207e160ca 1520 temp_msg.data[2] = (uint8_t) (send_pos_array >> 8);
Lightvalve 169:645207e160ca 1521 temp_msg.data[3] = (uint8_t) (send_ref_array);
Lightvalve 169:645207e160ca 1522 temp_msg.data[4] = (uint8_t) ((send_ref_array) >> 8);
Lightvalve 169:645207e160ca 1523
Lightvalve 169:645207e160ca 1524 can.write(temp_msg);
Lightvalve 169:645207e160ca 1525 }
Lightvalve 169:645207e160ca 1526
Lightvalve 169:645207e160ca 1527
jobuuu 2:a1c0a37df760 1528 /******************************************************************************
jobuuu 2:a1c0a37df760 1529 Sensor & State Transmission Functions
jobuuu 2:a1c0a37df760 1530 *******************************************************************************/
jobuuu 2:a1c0a37df760 1531
Lightvalve 170:42c938a40313 1532 void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq)
Lightvalve 170:42c938a40313 1533 {
Lightvalve 52:8ea76864368a 1534 CANMessage temp_msg;
Lightvalve 52:8ea76864368a 1535
Lightvalve 52:8ea76864368a 1536 temp_msg.id = CID_TX_POSITION;
Lightvalve 52:8ea76864368a 1537 temp_msg.len = 6;
Lightvalve 52:8ea76864368a 1538 temp_msg.data[0] = (uint8_t) t_pos;
Lightvalve 52:8ea76864368a 1539 temp_msg.data[1] = (uint8_t) (t_pos >> 8);
Lightvalve 52:8ea76864368a 1540 temp_msg.data[2] = (uint8_t) t_vel;
Lightvalve 52:8ea76864368a 1541 temp_msg.data[3] = (uint8_t) (t_vel >> 8);
Lightvalve 52:8ea76864368a 1542 temp_msg.data[4] = (uint8_t) t_torq;
Lightvalve 52:8ea76864368a 1543 temp_msg.data[5] = (uint8_t) (t_torq >> 8);
Lightvalve 52:8ea76864368a 1544
Lightvalve 52:8ea76864368a 1545 can.write(temp_msg);
Lightvalve 52:8ea76864368a 1546 }
Lightvalve 52:8ea76864368a 1547
Lightvalve 170:42c938a40313 1548 void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb)
Lightvalve 170:42c938a40313 1549 {
Lightvalve 170:42c938a40313 1550
jobuuu 2:a1c0a37df760 1551 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1552
jobuuu 2:a1c0a37df760 1553 temp_msg.id = CID_TX_POSITION;
jobuuu 2:a1c0a37df760 1554 temp_msg.len = 8;
Lightvalve 12:6f2531038ea4 1555 temp_msg.data[0] = (uint8_t) t_pos;
Lightvalve 12:6f2531038ea4 1556 temp_msg.data[1] = (uint8_t) (t_pos >> 8);
Lightvalve 41:abbd4e2af68b 1557 temp_msg.data[2] = (uint8_t) t_vel;
Lightvalve 41:abbd4e2af68b 1558 temp_msg.data[3] = (uint8_t) (t_vel >> 8);
Lightvalve 52:8ea76864368a 1559 temp_msg.data[4] = (uint8_t) t_pa;
Lightvalve 52:8ea76864368a 1560 temp_msg.data[5] = (uint8_t) (t_pa >> 8);
Lightvalve 52:8ea76864368a 1561 temp_msg.data[6] = (uint8_t) t_pb;
Lightvalve 52:8ea76864368a 1562 temp_msg.data[7] = (uint8_t) (t_pb >> 8);
jobuuu 2:a1c0a37df760 1563
jobuuu 2:a1c0a37df760 1564 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1565 }
jobuuu 2:a1c0a37df760 1566
Lightvalve 67:c2812cf26c38 1567 //void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout) {
Lightvalve 67:c2812cf26c38 1568 // CANMessage temp_msg;
Lightvalve 67:c2812cf26c38 1569 //
Lightvalve 67:c2812cf26c38 1570 // temp_msg.id = CID_TX_TORQUE;
Lightvalve 67:c2812cf26c38 1571 // temp_msg.len = 4;
Lightvalve 67:c2812cf26c38 1572 // temp_msg.data[0] = (uint8_t) t_valve_pos;
Lightvalve 67:c2812cf26c38 1573 // temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
Lightvalve 67:c2812cf26c38 1574 // temp_msg.data[2] = (uint8_t) t_vout;
Lightvalve 67:c2812cf26c38 1575 // temp_msg.data[3] = (uint8_t) (t_vout >> 8);
Lightvalve 67:c2812cf26c38 1576 //
Lightvalve 67:c2812cf26c38 1577 // can.write(temp_msg);
Lightvalve 67:c2812cf26c38 1578 //}
Lightvalve 67:c2812cf26c38 1579
Lightvalve 170:42c938a40313 1580 void CAN_TX_TORQUE(int16_t t_valve_pos)
Lightvalve 170:42c938a40313 1581 {
jobuuu 2:a1c0a37df760 1582 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1583
jobuuu 2:a1c0a37df760 1584 temp_msg.id = CID_TX_TORQUE;
Lightvalve 67:c2812cf26c38 1585 temp_msg.len = 2;
Lightvalve 48:889798ff9329 1586 temp_msg.data[0] = (uint8_t) t_valve_pos;
Lightvalve 48:889798ff9329 1587 temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
jobuuu 2:a1c0a37df760 1588
jobuuu 2:a1c0a37df760 1589 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1590 }
jobuuu 2:a1c0a37df760 1591
Lightvalve 170:42c938a40313 1592 void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b)
Lightvalve 170:42c938a40313 1593 {
jobuuu 2:a1c0a37df760 1594 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1595
jobuuu 2:a1c0a37df760 1596 temp_msg.id = CID_TX_PRES;
Lightvalve 57:f4819de54e7a 1597 temp_msg.len = 4;
Lightvalve 12:6f2531038ea4 1598 temp_msg.data[0] = (uint8_t) t_pres_a;
Lightvalve 12:6f2531038ea4 1599 temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
Lightvalve 12:6f2531038ea4 1600 temp_msg.data[2] = (uint8_t) t_pres_b;
Lightvalve 12:6f2531038ea4 1601 temp_msg.data[3] = (uint8_t) (t_pres_b >> 8);
jobuuu 2:a1c0a37df760 1602
jobuuu 2:a1c0a37df760 1603 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1604 }
jobuuu 2:a1c0a37df760 1605
Lightvalve 170:42c938a40313 1606 void CAN_TX_PWM(int16_t t_pwm)
Lightvalve 170:42c938a40313 1607 {
jobuuu 2:a1c0a37df760 1608 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1609
Lightvalve 57:f4819de54e7a 1610 temp_msg.id = CID_TX_VOUT;
Lightvalve 57:f4819de54e7a 1611 temp_msg.len = 2;
Lightvalve 12:6f2531038ea4 1612 temp_msg.data[0] = (uint8_t) t_pwm;
Lightvalve 12:6f2531038ea4 1613 temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
jobuuu 2:a1c0a37df760 1614
jobuuu 2:a1c0a37df760 1615 can.write(temp_msg);
jobuuu 2:a1c0a37df760 1616 }
jobuuu 2:a1c0a37df760 1617
Lightvalve 73:f80dc3970c99 1618 void CAN_TX_VALVE_POSITION(int16_t t_valve_pos_1, int16_t t_valve_pos_2, int16_t t_ref_valve_pos, int16_t t_pwm)
Lightvalve 27:a2254a485f23 1619 {
jobuuu 2:a1c0a37df760 1620 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 1621
jobuuu 2:a1c0a37df760 1622 temp_msg.id = CID_TX_VALVE_POSITION;
Lightvalve 73:f80dc3970c99 1623 temp_msg.len = 8;
Lightvalve 73:f80dc3970c99 1624 temp_msg.data[0] = (uint8_t) t_valve_pos_1;
Lightvalve 73:f80dc3970c99 1625 temp_msg.data[1] = (uint8_t) (t_valve_pos_1 >> 8);
Lightvalve 73:f80dc3970c99 1626 temp_msg.data[2] = (uint8_t) t_valve_pos_2;
Lightvalve 73:f80dc3970c99 1627 temp_msg.data[3] = (uint8_t) (t_valve_pos_2 >> 8);
Lightvalve 73:f80dc3970c99 1628 temp_msg.data[4] = (uint8_t) t_ref_valve_pos;
Lightvalve 73:f80dc3970c99 1629 temp_msg.data[5] = (uint8_t) (t_ref_valve_pos >> 8);
Lightvalve 73:f80dc3970c99 1630 temp_msg.data[6] = (uint8_t) t_pwm;
Lightvalve 73:f80dc3970c99 1631 temp_msg.data[7] = (uint8_t) (t_pwm >> 8);
Lightvalve 170:42c938a40313 1632
jobuuu 2:a1c0a37df760 1633
jobuuu 2:a1c0a37df760 1634 can.write(temp_msg);
Lightvalve 67:c2812cf26c38 1635 }