![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
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?
User | Revision | Line number | New 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 | } |