Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ

eeprom_test

Dependencies:   mbed FastPWM

CAN/function_CAN.cpp@7:e9086c72bb22, 2019-08-20 (annotated)

Committer:
jobuuu
Date:
Tue Aug 20 12:27:19 2019 +0000
Revision:
7:e9086c72bb22
Parent:
4:58c8081de776
Child:
11:82d8768d7351
Updated : 20190820;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jobuuu 2:a1c0a37df760 1 #include "function_CAN.h"
jobuuu 2:a1c0a37df760 2
jobuuu 2:a1c0a37df760 3 // CAN ID Setting Variables
jobuuu 2:a1c0a37df760 4 int CID_RX_CMD = 100;
jobuuu 2:a1c0a37df760 5 int CID_RX_REF_POSITION = 200;
jobuuu 2:a1c0a37df760 6 int CID_RX_REF_TORQUE = 300;
jobuuu 2:a1c0a37df760 7 int CID_RX_REF_PRES_DIFF = 400;
jobuuu 7:e9086c72bb22 8 int CID_RX_REF_VOUT = 500;
jobuuu 2:a1c0a37df760 9 int CID_RX_REF_VALVE_POSITION = 600;
jobuuu 2:a1c0a37df760 10
jobuuu 2:a1c0a37df760 11 int CID_TX_INFO = 1100;
jobuuu 2:a1c0a37df760 12 int CID_TX_POSITION = 1200;
jobuuu 2:a1c0a37df760 13 int CID_TX_TORQUE = 1300;
jobuuu 2:a1c0a37df760 14 int CID_TX_PRES = 1400;
jobuuu 7:e9086c72bb22 15 int CID_TX_VOUT = 1500;
jobuuu 2:a1c0a37df760 16 int CID_TX_VALVE_POSITION = 1600;
jobuuu 2:a1c0a37df760 17
jobuuu 7:e9086c72bb22 18 // Board Information
jobuuu 7:e9086c72bb22 19 int BNO = 0;
jobuuu 7:e9086c72bb22 20 int CONTROL_MODE = 0;
jobuuu 7:e9086c72bb22 21 double P_GAIN_JOINT_POSITION = 0.0;
jobuuu 7:e9086c72bb22 22 double I_GAIN_JOINT_POSITION = 0.0;
jobuuu 7:e9086c72bb22 23 double D_GAIN_JOINT_POSITION = 0.0;
jobuuu 7:e9086c72bb22 24 double P_GAIN_JOINT_TORQUE = 0.0;
jobuuu 7:e9086c72bb22 25 double I_GAIN_JOINT_TORQUE = 0.0;
jobuuu 7:e9086c72bb22 26 double D_GAIN_JOINT_TORQUE = 0.0;
jobuuu 7:e9086c72bb22 27
jobuuu 2:a1c0a37df760 28
jobuuu 2:a1c0a37df760 29 void ReadCMD(char CMD)
jobuuu 2:a1c0a37df760 30 {
jobuuu 2:a1c0a37df760 31 switch(CMD){
jobuuu 2:a1c0a37df760 32 case CRX_ASK_INFO:
jobuuu 4:58c8081de776 33 // CAN_TX_INFO();
jobuuu 2:a1c0a37df760 34 break;
jobuuu 2:a1c0a37df760 35 case CRX_ASK_BNO:
jobuuu 4:58c8081de776 36 // CAN_TX_BNO();
jobuuu 2:a1c0a37df760 37 break;
jobuuu 2:a1c0a37df760 38 default:
jobuuu 2:a1c0a37df760 39 break;
jobuuu 2:a1c0a37df760 40 }
jobuuu 2:a1c0a37df760 41 }
jobuuu 2:a1c0a37df760 42
jobuuu 2:a1c0a37df760 43 void CAN_RX_HANDLER()
jobuuu 2:a1c0a37df760 44 {
jobuuu 2:a1c0a37df760 45 can.read(msg);
jobuuu 2:a1c0a37df760 46 unsigned int address = msg.id;
jobuuu 2:a1c0a37df760 47
jobuuu 7:e9086c72bb22 48 if(address==CID_RX_CMD){
jobuuu 2:a1c0a37df760 49 unsigned int CMD = msg.data[0];
jobuuu 2:a1c0a37df760 50 ReadCMD(CMD);
jobuuu 7:e9086c72bb22 51 } else if(address==CID_RX_REF_POSITION) {
jobuuu 7:e9086c72bb22 52 int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
jobuuu 7:e9086c72bb22 53 int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
jobuuu 7:e9086c72bb22 54 pos.ref = (double)temp_pos;
jobuuu 7:e9086c72bb22 55 vel.ref = (double)temp_vel;
jobuuu 7:e9086c72bb22 56 } else if(address==CID_RX_REF_TORQUE) {
jobuuu 7:e9086c72bb22 57 int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
jobuuu 7:e9086c72bb22 58 torq.ref = (double)temp_torq;
jobuuu 7:e9086c72bb22 59 } else if(address==CID_RX_REF_PRES_DIFF) {
jobuuu 7:e9086c72bb22 60 // int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8);
jobuuu 7:e9086c72bb22 61 // torq.ref = (double)temp_presdiff;
jobuuu 7:e9086c72bb22 62 } else if(address==CID_RX_REF_VOUT) {
jobuuu 7:e9086c72bb22 63 int16_t temp_PWM = (int16_t) (msg.data[0] | msg.data[1] << 8);
jobuuu 7:e9086c72bb22 64 Vout.ref = (double)temp_PWM;
jobuuu 7:e9086c72bb22 65 } else if(address==CID_RX_REF_VALVE_POSITION) {
jobuuu 7:e9086c72bb22 66 int16_t temp_cur = (int16_t) (msg.data[0] | msg.data[1] << 8);
jobuuu 7:e9086c72bb22 67 cur.ref = (double)temp_cur;
jobuuu 2:a1c0a37df760 68 }
jobuuu 7:e9086c72bb22 69
jobuuu 2:a1c0a37df760 70 }
jobuuu 2:a1c0a37df760 71
jobuuu 2:a1c0a37df760 72 /******************************************************************************
jobuuu 2:a1c0a37df760 73 Information Transmission Functions
jobuuu 2:a1c0a37df760 74 *******************************************************************************/
jobuuu 3:aa28672362bb 75 /*
jobuuu 2:a1c0a37df760 76 inline void CAN_TX_INFO(void) {
jobuuu 2:a1c0a37df760 77 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 78
jobuuu 2:a1c0a37df760 79 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 80 temp_msg.len = 7;
jobuuu 2:a1c0a37df760 81 temp_msg.data[0] = (unsigned char) CTX_SEND_INFO;
jobuuu 2:a1c0a37df760 82 temp_msg.data[1] = (unsigned char) BNO;
jobuuu 2:a1c0a37df760 83 temp_msg.data[2] = (unsigned char) CAN_FREQ;
jobuuu 2:a1c0a37df760 84 temp_msg.data[3] = (unsigned char) (CAN_FREQ >> 8);
jobuuu 2:a1c0a37df760 85 temp_msg.data[4] = (unsigned char) (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]);
jobuuu 2:a1c0a37df760 86 temp_msg.data[5] = (unsigned char) CONTROL_MODE;
jobuuu 2:a1c0a37df760 87 temp_msg.data[6] = (unsigned char) OPERATING_MODE;
jobuuu 2:a1c0a37df760 88
jobuuu 2:a1c0a37df760 89 can.write(temp_msg);
jobuuu 2:a1c0a37df760 90 }
jobuuu 2:a1c0a37df760 91
jobuuu 2:a1c0a37df760 92 inline void CAN_TX_BNO(void) {
jobuuu 2:a1c0a37df760 93 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 94
jobuuu 2:a1c0a37df760 95 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 96 temp_msg.len = 2;
jobuuu 2:a1c0a37df760 97 temp_msg.data[0] = (unsigned char) CTX_SEND_BNO;
jobuuu 2:a1c0a37df760 98 temp_msg.data[1] = (unsigned char) BNO;
jobuuu 2:a1c0a37df760 99
jobuuu 2:a1c0a37df760 100 can.write(temp_msg);
jobuuu 2:a1c0a37df760 101 }
jobuuu 2:a1c0a37df760 102
jobuuu 2:a1c0a37df760 103 inline void CAN_TX_OPERATING_MODE(void) {
jobuuu 2:a1c0a37df760 104 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 105
jobuuu 2:a1c0a37df760 106 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 107 temp_msg.len = 2;
jobuuu 2:a1c0a37df760 108 temp_msg.data[0] = (unsigned char) CTX_SEND_OPERATING_MODE;
jobuuu 2:a1c0a37df760 109 temp_msg.data[1] = (unsigned char) OPERATING_MODE;
jobuuu 2:a1c0a37df760 110
jobuuu 2:a1c0a37df760 111 can.write(temp_msg);
jobuuu 2:a1c0a37df760 112 }
jobuuu 2:a1c0a37df760 113
jobuuu 2:a1c0a37df760 114
jobuuu 2:a1c0a37df760 115 inline void CAN_TX_CAN_FREQ(void) {
jobuuu 2:a1c0a37df760 116 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 117
jobuuu 2:a1c0a37df760 118 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 119 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 120 temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ;
jobuuu 2:a1c0a37df760 121 temp_msg.data[1] = (uint8_t) CAN_FREQ;
jobuuu 2:a1c0a37df760 122 temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8);
jobuuu 2:a1c0a37df760 123
jobuuu 2:a1c0a37df760 124 can.write(temp_msg);
jobuuu 2:a1c0a37df760 125 }
jobuuu 7:e9086c72bb22 126 */
jobuuu 2:a1c0a37df760 127 inline void CAN_TX_CONTROL_MODE(void) {
jobuuu 2:a1c0a37df760 128 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 129
jobuuu 2:a1c0a37df760 130 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 131 temp_msg.len = 2;
jobuuu 2:a1c0a37df760 132 temp_msg.data[0] = (unsigned char) CTX_SEND_CONTROL_MODE;
jobuuu 2:a1c0a37df760 133 temp_msg.data[1] = (unsigned char) CONTROL_MODE;
jobuuu 2:a1c0a37df760 134
jobuuu 2:a1c0a37df760 135 can.write(temp_msg);
jobuuu 2:a1c0a37df760 136 }
jobuuu 7:e9086c72bb22 137 /*
jobuuu 2:a1c0a37df760 138 inline void CAN_TX_JOINT_ENC_DIR(void) {
jobuuu 2:a1c0a37df760 139 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 140
jobuuu 2:a1c0a37df760 141 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 142 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 143 temp_msg.data[0] = (unsigned char) CTX_SEND_JOINT_ENC_DIR;
jobuuu 2:a1c0a37df760 144 temp_msg.data[1] = (unsigned char) DIR_JOINT_ENC;
jobuuu 2:a1c0a37df760 145 temp_msg.data[2] = (unsigned char) (DIR_JOINT_ENC >> 8);
jobuuu 2:a1c0a37df760 146
jobuuu 2:a1c0a37df760 147 can.write(temp_msg);
jobuuu 2:a1c0a37df760 148 }
jobuuu 2:a1c0a37df760 149
jobuuu 2:a1c0a37df760 150 inline void CAN_TX_VALVE_DIR(void) {
jobuuu 2:a1c0a37df760 151 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 152
jobuuu 2:a1c0a37df760 153 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 154 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 155 temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DIR;
jobuuu 2:a1c0a37df760 156 temp_msg.data[1] = (unsigned char) DIR_VALVE;
jobuuu 2:a1c0a37df760 157 temp_msg.data[2] = (unsigned char) (DIR_VALVE >> 8);
jobuuu 2:a1c0a37df760 158
jobuuu 2:a1c0a37df760 159 can.write(temp_msg);
jobuuu 2:a1c0a37df760 160 }
jobuuu 2:a1c0a37df760 161
jobuuu 2:a1c0a37df760 162 inline void CAN_TX_VALVE_ENC_DIR(void) {
jobuuu 2:a1c0a37df760 163 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 164
jobuuu 2:a1c0a37df760 165 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 166 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 167 temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_ENC_DIR;
jobuuu 2:a1c0a37df760 168 temp_msg.data[1] = (unsigned char) DIR_VALVE_ENC;
jobuuu 2:a1c0a37df760 169 temp_msg.data[2] = (unsigned char) (DIR_VALVE_ENC >> 8);
jobuuu 2:a1c0a37df760 170
jobuuu 2:a1c0a37df760 171 can.write(temp_msg);
jobuuu 2:a1c0a37df760 172 }
jobuuu 2:a1c0a37df760 173
jobuuu 2:a1c0a37df760 174 inline void CAN_TX_VOLTAGE_SUPPLY(void) {
jobuuu 2:a1c0a37df760 175 long send_voltage_supply = (long) (SUPPLY_VOLTAGE * 10);
jobuuu 2:a1c0a37df760 176
jobuuu 2:a1c0a37df760 177 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 178
jobuuu 2:a1c0a37df760 179 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 180 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 181 temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_SUPPLY;
jobuuu 2:a1c0a37df760 182 temp_msg.data[1] = (unsigned char) (send_voltage_supply);
jobuuu 2:a1c0a37df760 183 temp_msg.data[2] = (unsigned char) (send_voltage_supply >> 8);
jobuuu 2:a1c0a37df760 184
jobuuu 2:a1c0a37df760 185 can.write(temp_msg);
jobuuu 2:a1c0a37df760 186 }
jobuuu 2:a1c0a37df760 187
jobuuu 2:a1c0a37df760 188 inline void CAN_TX_VOLTAGE_VALVE(void) {
jobuuu 2:a1c0a37df760 189 long send_voltage_valve = (long) (VALVE_VOLTAGE_LIMIT * 10);
jobuuu 2:a1c0a37df760 190
jobuuu 2:a1c0a37df760 191 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 192
jobuuu 2:a1c0a37df760 193 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 194 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 195 temp_msg.data[0] = (unsigned char) CTX_SEND_VOLTAGE_VALVE;
jobuuu 2:a1c0a37df760 196 temp_msg.data[1] = (unsigned char) send_voltage_valve;
jobuuu 2:a1c0a37df760 197 temp_msg.data[2] = (unsigned char) (send_voltage_valve >> 8);
jobuuu 2:a1c0a37df760 198
jobuuu 2:a1c0a37df760 199 can.write(temp_msg);
jobuuu 2:a1c0a37df760 200 }
jobuuu 7:e9086c72bb22 201 */
jobuuu 2:a1c0a37df760 202
jobuuu 2:a1c0a37df760 203 inline void CAN_TX_PID_GAIN(int t_type) {
jobuuu 2:a1c0a37df760 204 // t_type = 0 : valve position control gain
jobuuu 2:a1c0a37df760 205 // t_type = 1 : joint position control gain
jobuuu 2:a1c0a37df760 206 // t_type = 2 : joint torque control gain
jobuuu 2:a1c0a37df760 207
jobuuu 7:e9086c72bb22 208 long sendPgain=0, sendIgain=0, sendDgain=0;
jobuuu 2:a1c0a37df760 209 if (t_type == 0) {
jobuuu 7:e9086c72bb22 210 // sendPgain = (long) (P_GAIN_VALVE_POSITION);
jobuuu 7:e9086c72bb22 211 // sendIgain = (long) (I_GAIN_VALVE_POSITION);
jobuuu 7:e9086c72bb22 212 // sendDgain = (long) (D_GAIN_VALVE_POSITION);
jobuuu 2:a1c0a37df760 213 } else if (t_type == 1) {
jobuuu 2:a1c0a37df760 214 sendPgain = (long) (P_GAIN_JOINT_POSITION);
jobuuu 2:a1c0a37df760 215 sendIgain = (long) (I_GAIN_JOINT_POSITION);
jobuuu 2:a1c0a37df760 216 sendDgain = (long) (D_GAIN_JOINT_POSITION);
jobuuu 2:a1c0a37df760 217 } else if (t_type == 2) {
jobuuu 2:a1c0a37df760 218 sendPgain = (long) (P_GAIN_JOINT_TORQUE);
jobuuu 2:a1c0a37df760 219 sendIgain = (long) (I_GAIN_JOINT_TORQUE);
jobuuu 2:a1c0a37df760 220 sendDgain = (long) (D_GAIN_JOINT_TORQUE);
jobuuu 2:a1c0a37df760 221 }
jobuuu 2:a1c0a37df760 222
jobuuu 2:a1c0a37df760 223 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 224
jobuuu 2:a1c0a37df760 225 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 226 temp_msg.len = 8;
jobuuu 2:a1c0a37df760 227 temp_msg.data[0] = (unsigned char) CTX_SEND_PID_GAIN;
jobuuu 2:a1c0a37df760 228 temp_msg.data[1] = (unsigned char) t_type;
jobuuu 2:a1c0a37df760 229 temp_msg.data[2] = (unsigned char) sendPgain;
jobuuu 2:a1c0a37df760 230 temp_msg.data[3] = (unsigned char) (sendPgain >> 8);
jobuuu 2:a1c0a37df760 231 temp_msg.data[4] = (unsigned char) sendIgain;
jobuuu 2:a1c0a37df760 232 temp_msg.data[5] = (unsigned char) (sendIgain >> 8);
jobuuu 2:a1c0a37df760 233 temp_msg.data[6] = (unsigned char) sendDgain;
jobuuu 2:a1c0a37df760 234 temp_msg.data[7] = (unsigned char) (sendDgain >> 8);
jobuuu 2:a1c0a37df760 235
jobuuu 2:a1c0a37df760 236 can.write(temp_msg);
jobuuu 2:a1c0a37df760 237 }
jobuuu 2:a1c0a37df760 238
jobuuu 7:e9086c72bb22 239 /*
jobuuu 2:a1c0a37df760 240 inline void CAN_TX_VALVE_DEADZONE(void) {
jobuuu 2:a1c0a37df760 241 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 242
jobuuu 2:a1c0a37df760 243 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 244 temp_msg.len = 7;
jobuuu 2:a1c0a37df760 245 temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_DEADZONE;
jobuuu 2:a1c0a37df760 246 temp_msg.data[1] = (unsigned char) (int) (VALVE_CENTER);
jobuuu 2:a1c0a37df760 247 temp_msg.data[2] = (unsigned char) ((int) (VALVE_CENTER) >> 8);
jobuuu 2:a1c0a37df760 248 temp_msg.data[3] = (unsigned char) (int) (VALVE_DEADZONE_PLUS);
jobuuu 2:a1c0a37df760 249 temp_msg.data[4] = (unsigned char) ((int) (VALVE_DEADZONE_PLUS) >> 8);
jobuuu 2:a1c0a37df760 250 temp_msg.data[5] = (unsigned char) (int) (VALVE_DEADZONE_MINUS);
jobuuu 2:a1c0a37df760 251 temp_msg.data[6] = (unsigned char) ((int) (VALVE_DEADZONE_MINUS) >> 8);
jobuuu 2:a1c0a37df760 252
jobuuu 2:a1c0a37df760 253 can.write(temp_msg);
jobuuu 2:a1c0a37df760 254 }
jobuuu 2:a1c0a37df760 255
jobuuu 2:a1c0a37df760 256 inline void CAN_TX_ENC_PULSE_PER_POSITION(void) {
jobuuu 2:a1c0a37df760 257 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 258
jobuuu 2:a1c0a37df760 259 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 260 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 261 temp_msg.data[0] = (unsigned char) CTX_SEND_ENC_PULSE_PER_POSITION;
jobuuu 2:a1c0a37df760 262 temp_msg.data[1] = (unsigned char) ENC_PULSE_PER_POSITION;
jobuuu 2:a1c0a37df760 263 temp_msg.data[2] = (unsigned char) (ENC_PULSE_PER_POSITION >> 8);
jobuuu 2:a1c0a37df760 264
jobuuu 2:a1c0a37df760 265 can.write(temp_msg);
jobuuu 2:a1c0a37df760 266 }
jobuuu 2:a1c0a37df760 267
jobuuu 2:a1c0a37df760 268 inline void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void) {
jobuuu 2:a1c0a37df760 269 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 270
jobuuu 2:a1c0a37df760 271 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 272 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 273 temp_msg.data[0] = (unsigned char) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
jobuuu 2:a1c0a37df760 274 temp_msg.data[1] = (unsigned char) TORQUE_SENSOR_PULSE_PER_TORQUE;
jobuuu 2:a1c0a37df760 275 temp_msg.data[2] = (unsigned char) (TORQUE_SENSOR_PULSE_PER_TORQUE >> 8);
jobuuu 2:a1c0a37df760 276
jobuuu 2:a1c0a37df760 277 can.write(temp_msg);
jobuuu 2:a1c0a37df760 278 }
jobuuu 2:a1c0a37df760 279
jobuuu 2:a1c0a37df760 280 inline void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void) {
jobuuu 2:a1c0a37df760 281 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 282
jobuuu 2:a1c0a37df760 283 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 284 temp_msg.len = 5;
jobuuu 2:a1c0a37df760 285 temp_msg.data[0] = (unsigned char) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
jobuuu 2:a1c0a37df760 286 temp_msg.data[1] = (unsigned char) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.);
jobuuu 2:a1c0a37df760 287 temp_msg.data[2] = (unsigned char) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.) >> 8);
jobuuu 2:a1c0a37df760 288 temp_msg.data[3] = (unsigned char) (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.);
jobuuu 2:a1c0a37df760 289 temp_msg.data[4] = (unsigned char) ((int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.) >> 8);
jobuuu 2:a1c0a37df760 290
jobuuu 2:a1c0a37df760 291 can.write(temp_msg);
jobuuu 2:a1c0a37df760 292 }
jobuuu 2:a1c0a37df760 293
jobuuu 2:a1c0a37df760 294 inline void CAN_TX_HOMEPOS_OFFSET(void) {
jobuuu 2:a1c0a37df760 295 long send_homepos_offset;
jobuuu 2:a1c0a37df760 296 send_homepos_offset = (long) (HOMEPOS_OFFSET);
jobuuu 2:a1c0a37df760 297
jobuuu 2:a1c0a37df760 298 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 299
jobuuu 2:a1c0a37df760 300 temp_msg.id = CID_TX_INFO;
jobuuu 2:a1c0a37df760 301 temp_msg.len = 3;
jobuuu 2:a1c0a37df760 302 temp_msg.data[0] = (unsigned char) CTX_SEND_HOMEPOS_OFFSET;
jobuuu 2:a1c0a37df760 303 temp_msg.data[1] = (unsigned char) send_homepos_offset;
jobuuu 2:a1c0a37df760 304 temp_msg.data[2] = (unsigned char) (send_homepos_offset >> 8);
jobuuu 2:a1c0a37df760 305
jobuuu 2:a1c0a37df760 306 can.write(temp_msg);
jobuuu 2:a1c0a37df760 307 }
jobuuu 3:aa28672362bb 308 */
jobuuu 2:a1c0a37df760 309 /******************************************************************************
jobuuu 2:a1c0a37df760 310 Sensor & State Transmission Functions
jobuuu 2:a1c0a37df760 311 *******************************************************************************/
jobuuu 2:a1c0a37df760 312
jobuuu 2:a1c0a37df760 313 inline void CAN_TX_POSITION(long t_pos, long t_vel) {
jobuuu 2:a1c0a37df760 314 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 315
jobuuu 2:a1c0a37df760 316 temp_msg.id = CID_TX_POSITION;
jobuuu 2:a1c0a37df760 317 temp_msg.len = 8;
jobuuu 2:a1c0a37df760 318 temp_msg.data[0] = (unsigned char) t_pos;
jobuuu 2:a1c0a37df760 319 temp_msg.data[1] = (unsigned char) (t_pos >> 8);
jobuuu 2:a1c0a37df760 320 temp_msg.data[2] = (unsigned char) (t_pos >> 16);
jobuuu 2:a1c0a37df760 321 temp_msg.data[3] = (unsigned char) (t_pos >> 24);
jobuuu 2:a1c0a37df760 322 temp_msg.data[4] = (unsigned char) t_vel;
jobuuu 2:a1c0a37df760 323 temp_msg.data[5] = (unsigned char) (t_vel >> 8);
jobuuu 2:a1c0a37df760 324 temp_msg.data[6] = (unsigned char) (t_vel >> 16);
jobuuu 2:a1c0a37df760 325 temp_msg.data[7] = (unsigned char) (t_vel >> 24);
jobuuu 2:a1c0a37df760 326
jobuuu 2:a1c0a37df760 327 can.write(temp_msg);
jobuuu 2:a1c0a37df760 328 }
jobuuu 2:a1c0a37df760 329
jobuuu 2:a1c0a37df760 330 inline void CAN_TX_TORQUE(int16_t t_torque) {
jobuuu 2:a1c0a37df760 331 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 332
jobuuu 2:a1c0a37df760 333 temp_msg.id = CID_TX_TORQUE;
jobuuu 2:a1c0a37df760 334 temp_msg.len = 2;
jobuuu 2:a1c0a37df760 335 temp_msg.data[0] = (unsigned char) t_torque;
jobuuu 2:a1c0a37df760 336 temp_msg.data[1] = (unsigned char) (t_torque >> 8);
jobuuu 2:a1c0a37df760 337
jobuuu 2:a1c0a37df760 338 can.write(temp_msg);
jobuuu 2:a1c0a37df760 339 }
jobuuu 2:a1c0a37df760 340
jobuuu 2:a1c0a37df760 341 inline void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) {
jobuuu 2:a1c0a37df760 342 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 343
jobuuu 2:a1c0a37df760 344 temp_msg.id = CID_TX_PRES;
jobuuu 2:a1c0a37df760 345 temp_msg.len = 8;
jobuuu 2:a1c0a37df760 346 temp_msg.data[0] = (unsigned char) t_pres_a;
jobuuu 2:a1c0a37df760 347 temp_msg.data[1] = (unsigned char) (t_pres_a >> 8);
jobuuu 2:a1c0a37df760 348 temp_msg.data[2] = (unsigned char) t_pres_b;
jobuuu 2:a1c0a37df760 349 temp_msg.data[3] = (unsigned char) (t_pres_b >> 8);
jobuuu 2:a1c0a37df760 350
jobuuu 2:a1c0a37df760 351 can.write(temp_msg);
jobuuu 2:a1c0a37df760 352 }
jobuuu 2:a1c0a37df760 353
jobuuu 2:a1c0a37df760 354 inline void CAN_TX_PWM(int16_t t_pwm) {
jobuuu 2:a1c0a37df760 355 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 356
jobuuu 2:a1c0a37df760 357 temp_msg.id = CID_TX_POSITION;
jobuuu 2:a1c0a37df760 358 temp_msg.len = 8;
jobuuu 2:a1c0a37df760 359 temp_msg.data[0] = (unsigned char) t_pwm;
jobuuu 2:a1c0a37df760 360 temp_msg.data[1] = (unsigned char) (t_pwm >> 8);
jobuuu 2:a1c0a37df760 361
jobuuu 2:a1c0a37df760 362 can.write(temp_msg);
jobuuu 2:a1c0a37df760 363 }
jobuuu 2:a1c0a37df760 364
jobuuu 2:a1c0a37df760 365 inline void CAN_TX_VALVE_POSITION(int16_t t_valve_pos) {
jobuuu 2:a1c0a37df760 366 CANMessage temp_msg;
jobuuu 2:a1c0a37df760 367
jobuuu 2:a1c0a37df760 368 temp_msg.id = CID_TX_VALVE_POSITION;
jobuuu 2:a1c0a37df760 369 temp_msg.len = 8;
jobuuu 2:a1c0a37df760 370 temp_msg.data[0] = (unsigned char) t_valve_pos;
jobuuu 2:a1c0a37df760 371 temp_msg.data[1] = (unsigned char) (t_valve_pos >> 8);
jobuuu 2:a1c0a37df760 372
jobuuu 2:a1c0a37df760 373 can.write(temp_msg);
jobuuu 2:a1c0a37df760 374 }