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