Buyoun Cho / Mbed 2 deprecated HydraulicControlBoard_PostLIGHT_210420

[Ver 1.0] The code was given by Seunghoon shin, used for hydraulic quadrupedal robot. Buyoun Cho will revise the code for Post-LIGHT (the robot name is not determined yet).

Dependencies:   mbed FastPWM

CAN/function_CAN.h@230:2c3e5ecbe7e1, 2021-04-21 (annotated)

Committer:
jobuuu
Date:
Wed Apr 21 04:20:39 2021 +0000
Revision:
230:2c3e5ecbe7e1
Parent:
227:699c3e572283 
Synchronize to HCB_210420 (MZ-board)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jobuuu 2:a1c0a37df760 1 #ifndef _FUNCTION_CAN_H_
jobuuu 2:a1c0a37df760 2 #define _FUNCTION_CAN_H_
jobuuu 2:a1c0a37df760 3
jobuuu 2:a1c0a37df760 4 #include "mbed.h"
jobuuu 2:a1c0a37df760 5
jobuuu 7:e9086c72bb22 6 extern CAN can;
jobuuu 7:e9086c72bb22 7 extern CANMessage msg;
jobuuu 7:e9086c72bb22 8
Lightvalve 11:82d8768d7351 9 // INIT_CID
Lightvalve 11:82d8768d7351 10 #define INIT_CID_RX_CMD 100
Lightvalve 11:82d8768d7351 11 #define INIT_CID_RX_REF_POSITION 200
hyhoon 227:699c3e572283 12 #define INIT_CID_RX_REF_OPENLOOP 300
Lightvalve 49:e7bcfc244d40 13 #define INIT_CID_RX_REF_PWM 400
Lightvalve 11:82d8768d7351 14
Lightvalve 11:82d8768d7351 15 #define INIT_CID_TX_INFO 1100
hyhoon 227:699c3e572283 16 #define INIT_CID_TX_POS_VEL_TORQ 1200
hyhoon 227:699c3e572283 17 #define INIT_CID_TX_PWM 1300
hyhoon 227:699c3e572283 18 #define INIT_CID_TX_CURRENT 1400
Lightvalve 11:82d8768d7351 19 #define INIT_CID_TX_VOUT 1500
Lightvalve 11:82d8768d7351 20 #define INIT_CID_TX_VALVE_POSITION 1600
jeongyeseong 225:9c0becc196ba 21 #define INIT_CID_TX_SOMETHING 1700
Lightvalve 11:82d8768d7351 22
Lightvalve 11:82d8768d7351 23
jobuuu 2:a1c0a37df760 24 // CID_RX_CMD - RX CMD type
jobuuu 2:a1c0a37df760 25 #define CRX_ASK_INFO 0
jobuuu 2:a1c0a37df760 26 #define CRX_ASK_BNO 1
jobuuu 2:a1c0a37df760 27 #define CRX_SET_BNO 101
jobuuu 2:a1c0a37df760 28 #define CRX_ASK_OPERATING_MODE 2
jobuuu 2:a1c0a37df760 29 #define CRX_SET_OPERATING_MODE 102
jobuuu 2:a1c0a37df760 30 #define CRX_SET_ENC_ZERO 103
jobuuu 2:a1c0a37df760 31 #define CRX_SET_FET_ON 104
Lightvalve 45:35fa6884d0c6 32 #define CRX_SET_POS_TORQ_TRANS 105
jobuuu 2:a1c0a37df760 33 #define CRX_ASK_CAN_FREQ 6
jobuuu 2:a1c0a37df760 34 #define CRX_SET_CAN_FREQ 106
jobuuu 2:a1c0a37df760 35 #define CRX_ASK_CONTROL_MODE 7
jobuuu 2:a1c0a37df760 36 #define CRX_SET_CONTROL_MODE 107
jobuuu 2:a1c0a37df760 37 #define CRX_SET_DATA_REQUEST 108
jobuuu 2:a1c0a37df760 38 #define CRX_ASK_JOINT_ENC_DIR 9
jobuuu 2:a1c0a37df760 39 #define CRX_SET_JOINT_ENC_DIR 109
jobuuu 2:a1c0a37df760 40 #define CRX_ASK_VALVE_DIR 10
jobuuu 2:a1c0a37df760 41 #define CRX_SET_VALVE_DIR 110
jobuuu 2:a1c0a37df760 42 #define CRX_ASK_VALVE_ENC_DIR 11
jobuuu 2:a1c0a37df760 43 #define CRX_SET_VALVE_ENC_DIR 111
jobuuu 2:a1c0a37df760 44 #define CRX_ASK_VOLTAGE_SUPPLY 12
jobuuu 2:a1c0a37df760 45 #define CRX_SET_VOLTAGE_SUPPLY 112
jobuuu 2:a1c0a37df760 46 #define CRX_ASK_VOLTAGE_VALVE 13
jobuuu 2:a1c0a37df760 47 #define CRX_SET_VOLTAGE_VALVE 113
jobuuu 230:2c3e5ecbe7e1 48 #define CRX_SET_HOMEPOS 114
jobuuu 230:2c3e5ecbe7e1 49 #define CRX_ASK_VARIABLE_SUPPLY 15
jobuuu 230:2c3e5ecbe7e1 50 #define CRX_SET_VARIABLE_SUPPLY 115
jobuuu 230:2c3e5ecbe7e1 51
jobuuu 2:a1c0a37df760 52 #define CRX_ASK_PID_GAIN 20
jobuuu 2:a1c0a37df760 53 #define CRX_SET_PID_GAIN 120
jobuuu 2:a1c0a37df760 54 #define CRX_ASK_VALVE_DEADZONE 21
jobuuu 2:a1c0a37df760 55 #define CRX_SET_VALVE_DEADZONE 121
jobuuu 2:a1c0a37df760 56 #define CRX_ASK_VELOCITY_COMP_GAIN 22
jobuuu 2:a1c0a37df760 57 #define CRX_SET_VELOCITY_COMP_GAIN 122
jobuuu 2:a1c0a37df760 58 #define CRX_ASK_COMPLIANCE_GAIN 23
jobuuu 2:a1c0a37df760 59 #define CRX_SET_COMPLIANCE_GAIN 123
jobuuu 2:a1c0a37df760 60 #define CRX_ASK_VALVE_FF 25
jobuuu 2:a1c0a37df760 61 #define CRX_SET_VALVE_FF 125
jobuuu 2:a1c0a37df760 62 #define CRX_ASK_BULK_MODULUS 26
jobuuu 2:a1c0a37df760 63 #define CRX_SET_BULK_MODULUS 126
jobuuu 2:a1c0a37df760 64 #define CRX_ASK_CHAMBER_VOLUME 27
jobuuu 2:a1c0a37df760 65 #define CRX_SET_CHAMBER_VOLUME 127
jobuuu 2:a1c0a37df760 66 #define CRX_ASK_PISTON_AREA 28
jobuuu 2:a1c0a37df760 67 #define CRX_SET_PISTON_AREA 128
jobuuu 230:2c3e5ecbe7e1 68 #define CRX_ASK_SUP_PRES 29
jobuuu 230:2c3e5ecbe7e1 69 #define CRX_SET_SUP_PRES 129
jobuuu 2:a1c0a37df760 70 #define CRX_ASK_ENC_LIMIT 30
jobuuu 2:a1c0a37df760 71 #define CRX_SET_ENC_LIMIT 130
jobuuu 2:a1c0a37df760 72 #define CRX_ASK_STROKE 31
jobuuu 2:a1c0a37df760 73 #define CRX_SET_STROKE 131
jobuuu 2:a1c0a37df760 74 #define CRX_ASK_VALVE_LIMIT 32
jobuuu 2:a1c0a37df760 75 #define CRX_SET_VALVE_LIMIT 132
jobuuu 2:a1c0a37df760 76 #define CRX_ASK_ENC_PULSE_PER_POSITION 33
jobuuu 2:a1c0a37df760 77 #define CRX_SET_ENC_PULSE_PER_POSITION 133
jobuuu 2:a1c0a37df760 78 #define CRX_ASK_TORQUE_SENSOR_PULSE_PER_TORQUE 34
jobuuu 2:a1c0a37df760 79 #define CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE 134
jobuuu 2:a1c0a37df760 80 #define CRX_ASK_PRES_SENSOR_PULSE_PER_PRES 35
jobuuu 2:a1c0a37df760 81 #define CRX_SET_PRES_SENSOR_PULSE_PER_PRES 135
jobuuu 2:a1c0a37df760 82 #define CRX_ASK_FRICTION 36
jobuuu 2:a1c0a37df760 83 #define CRX_SET_FRICTION 136
jobuuu 2:a1c0a37df760 84 #define CRX_ASK_VALVE_GAIN_PLUS 37
jobuuu 2:a1c0a37df760 85 #define CRX_SET_VALVE_GAIN_PLUS 137
jobuuu 2:a1c0a37df760 86 #define CRX_ASK_VALVE_GAIN_MINUS 38
jobuuu 2:a1c0a37df760 87 #define CRX_SET_VALVE_GAIN_MINUS 138
jobuuu 2:a1c0a37df760 88 #define CRX_LOW_REF 139
jobuuu 2:a1c0a37df760 89 #define CRX_ASK_HOMEPOS_OFFSET 40
jobuuu 2:a1c0a37df760 90 #define CRX_SET_HOMEPOS_OFFSET 140
jobuuu 2:a1c0a37df760 91 #define CRX_ASK_HOMEPOS_VALVE_OPENING 41
jobuuu 2:a1c0a37df760 92 #define CRX_SET_HOMEPOS_VALVE_OPENING 141
Lightvalve 11:82d8768d7351 93 #define CRX_ASK_DDV_VALVE_DEADZONE 42
Lightvalve 11:82d8768d7351 94 #define CRX_SET_DDV_VALVE_DEADZONE 142
Lightvalve 11:82d8768d7351 95 #define CRX_ASK_VALVE_PWM_VS_VALVE_POS 43
Lightvalve 11:82d8768d7351 96 #define CRX_ASK_VALVE_POS_VS_FLOWRATE 44
Lightvalve 11:82d8768d7351 97 #define CRX_ASK_VALVE_POS_NUM 45
Lightvalve 38:118df027d851 98 #define CRX_ASK_VALVE_MAX_MIN_POS 46
jobuuu 2:a1c0a37df760 99 #define CRX_SET_ERR_CLEAR 150
Lightvalve 27:a2254a485f23 100 #define CRX_SET_ROM 146
Lightvalve 32:4b8c0fedaf2c 101 #define CRX_SET_VALVE_CENTER_OFFSET 147
Lightvalve 33:91b17819ec30 102 #define CRX_SET_VALVE_DZ_MINUS_OFFSET 148
Lightvalve 33:91b17819ec30 103 #define CRX_SET_VALVE_DZ_PLUS_OFFSET 149
Lightvalve 36:a46e63505ed8 104 #define CRX_SET_PID_GAIN_OPP 152
Lightvalve 54:647072f5307a 105 #define CRX_DELAY_TEST 153
Lightvalve 68:328e1be06f5d 106 #define CRX_SET_NN_CONTROL_FLAG 154
Lightvalve 169:645207e160ca 107 #define CRX_SET_FREQ_TEST 155
Lightvalve 169:645207e160ca 108 #define CRX_ASK_BUFFER 156
Lightvalve 169:645207e160ca 109 #define CRX_SET_STEP_TEST 157
Lightvalve 192:637092202815 110 #define CRX_SET_CHANGE_EVERY_REFERNCE 158
jobuuu 2:a1c0a37df760 111 #define CRX_JUMP_STATUS 255
jobuuu 2:a1c0a37df760 112
jobuuu 7:e9086c72bb22 113 // CID_TX_INFO - TX INFO type
jobuuu 7:e9086c72bb22 114
jobuuu 7:e9086c72bb22 115 #define CTX_SEND_INFO 0
jobuuu 7:e9086c72bb22 116 #define CTX_SEND_BNO 1
jobuuu 7:e9086c72bb22 117 #define CTX_SEND_OPERATING_MODE 2
jobuuu 7:e9086c72bb22 118 #define CTX_SEND_CAN_FREQ 6
jobuuu 7:e9086c72bb22 119 #define CTX_SEND_CONTROL_MODE 7
jobuuu 7:e9086c72bb22 120 #define CTX_SEND_JOINT_ENC_DIR 9
jobuuu 7:e9086c72bb22 121 #define CTX_SEND_VALVE_DIR 10
jobuuu 7:e9086c72bb22 122 #define CTX_SEND_VALVE_ENC_DIR 11
jobuuu 7:e9086c72bb22 123 #define CTX_SEND_VOLTAGE_SUPPLY 12
jobuuu 7:e9086c72bb22 124 #define CTX_SEND_VOLTAGE_VALVE 13
jobuuu 230:2c3e5ecbe7e1 125 #define CTX_SEND_VARIABLE_SUPPLY 15
jobuuu 7:e9086c72bb22 126 #define CTX_SEND_PID_GAIN 20
jobuuu 7:e9086c72bb22 127 #define CTX_SEND_VALVE_DEADZONE 21
jobuuu 7:e9086c72bb22 128 #define CTX_SEND_VELOCITY_COMP_GAIN 22
jobuuu 7:e9086c72bb22 129 #define CTX_SEND_COMPLIANCE_GAIN 23
jobuuu 7:e9086c72bb22 130 #define CTX_SEND_VALVE_FF 25
jobuuu 7:e9086c72bb22 131 #define CTX_SEND_BULK_MODULUS 26
jobuuu 7:e9086c72bb22 132 #define CTX_SEND_CHAMBER_VOLUME 27
jobuuu 7:e9086c72bb22 133 #define CTX_SEND_PISTON_AREA 28
jobuuu 230:2c3e5ecbe7e1 134 #define CTX_SEND_SUP_PRES 29
jobuuu 7:e9086c72bb22 135 #define CTX_SEND_ENC_LIMIT 30
jobuuu 7:e9086c72bb22 136 #define CTX_SEND_STROKE 31
jobuuu 7:e9086c72bb22 137 #define CTX_SEND_VALVE_LIMIT 32
jobuuu 7:e9086c72bb22 138 #define CTX_SEND_ENC_PULSE_PER_POSITION 33
jobuuu 7:e9086c72bb22 139 #define CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE 34
jobuuu 7:e9086c72bb22 140 #define CTX_SEND_PRES_SENSOR_PULSE_PER_BAR 35
jobuuu 7:e9086c72bb22 141 #define CTX_SEND_FRICTION 36
jobuuu 7:e9086c72bb22 142 #define CTX_SEND_VALVE_GAIN_PLUS 37
jobuuu 7:e9086c72bb22 143 #define CTX_SEND_VALVE_GAIN_MINUS 38
jobuuu 7:e9086c72bb22 144 #define CTX_SEND_REFENCE_MODE 39
jobuuu 7:e9086c72bb22 145 #define CTX_SEND_HOMEPOS_OFFSET 40
jobuuu 7:e9086c72bb22 146 #define CTX_SEND_HOMEPOS_VALVE_OPENING 41
Lightvalve 11:82d8768d7351 147 #define CTX_SEND_DDV_VALVE_DEADZONE 42
Lightvalve 11:82d8768d7351 148 #define CTX_VALVE_PWM_VS_VALVE_POS 43
Lightvalve 11:82d8768d7351 149 #define CTX_VALVE_POS_VS_FLOWRATE 44
Lightvalve 11:82d8768d7351 150 #define CTX_VALVE_POS_NUM 45
Lightvalve 38:118df027d851 151 #define CTX_VALVE_MAX_MIN_POS 46
Lightvalve 169:645207e160ca 152 #define CTX_SEND_BUFFER 156
Lightvalve 49:e7bcfc244d40 153
Lightvalve 11:82d8768d7351 154 // Sensor & State Transmission
Lightvalve 52:8ea76864368a 155 void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq);
Lightvalve 52:8ea76864368a 156 void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb);
hyhoon 227:699c3e572283 157 //void CAN_TX_PWM(int16_t t_valve_pos, int16_t t_vout);
hyhoon 227:699c3e572283 158 void CAN_TX_PWM(int16_t t_valve_pos);
hyhoon 227:699c3e572283 159 void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b);
Lightvalve 11:82d8768d7351 160 void CAN_TX_PWM(int16_t t_pwm);
Lightvalve 73:f80dc3970c99 161 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);
jeongyeseong 225:9c0becc196ba 162 void CAN_TX_SOMETHING (int16_t t_a, int16_t t_b, int16_t t_c, int16_t t_d);
jobuuu 7:e9086c72bb22 163
jobuuu 2:a1c0a37df760 164 // Information Transmission
Lightvalve 11:82d8768d7351 165 void CAN_TX_INFO(void);
Lightvalve 11:82d8768d7351 166 void CAN_TX_BNO(void);
Lightvalve 11:82d8768d7351 167 void CAN_TX_OPERATING_MODE(void);
Lightvalve 11:82d8768d7351 168 void CAN_TX_CAN_FREQ(void);
Lightvalve 11:82d8768d7351 169 void CAN_TX_CONTROL_MODE(void);
Lightvalve 11:82d8768d7351 170 void CAN_TX_JOINT_ENC_DIR(void);
Lightvalve 11:82d8768d7351 171 void CAN_TX_VALVE_DIR(void);
Lightvalve 11:82d8768d7351 172 void CAN_TX_VALVE_ENC_DIR(void);
Lightvalve 11:82d8768d7351 173 void CAN_TX_VOLTAGE_SUPPLY(void);
Lightvalve 11:82d8768d7351 174 void CAN_TX_VOLTAGE_VALVE(void);
jobuuu 230:2c3e5ecbe7e1 175 void CAN_TX_VARIABLE_SUPPLY_ONOFF(void);
jobuuu 230:2c3e5ecbe7e1 176
Lightvalve 11:82d8768d7351 177 void CAN_TX_PID_GAIN(int t_type);
Lightvalve 11:82d8768d7351 178 void CAN_TX_VALVE_DEADZONE(void);
Lightvalve 11:82d8768d7351 179 void CAN_TX_VELOCITY_COMP_GAIN(void);
Lightvalve 11:82d8768d7351 180 void CAN_TX_COMPLIANCE_GAIN(void);
Lightvalve 11:82d8768d7351 181 void CAN_TX_VALVE_FF(void);
Lightvalve 11:82d8768d7351 182 void CAN_TX_BULK_MODULUS(void);
Lightvalve 11:82d8768d7351 183 void CAN_TX_CHAMBER_VOLUME(void);
Lightvalve 11:82d8768d7351 184 void CAN_TX_PISTON_AREA(void);
jobuuu 230:2c3e5ecbe7e1 185 void CAN_TX_SUP_PRES(void);
Lightvalve 11:82d8768d7351 186 void CAN_TX_ENC_LIMIT(void);
Lightvalve 11:82d8768d7351 187 void CAN_TX_STROKE(void);
Lightvalve 11:82d8768d7351 188 void CAN_TX_VALVE_LIMIT(void);
Lightvalve 11:82d8768d7351 189 void CAN_TX_ENC_PULSE_PER_POSITION(void);
Lightvalve 11:82d8768d7351 190 void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void);
Lightvalve 11:82d8768d7351 191 void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void);
Lightvalve 11:82d8768d7351 192 void CAN_TX_FRICTION(void);
Lightvalve 11:82d8768d7351 193 void CAN_TX_VALVE_GAIN_PLUS(void);
Lightvalve 11:82d8768d7351 194 void CAN_TX_VALVE_GAIN_MINUS(void);
Lightvalve 11:82d8768d7351 195 void CAN_TX_REFENCE_MODE(void);
Lightvalve 11:82d8768d7351 196 void CAN_TX_HOMEPOS_OFFSET(void);
Lightvalve 11:82d8768d7351 197 void CAN_TX_HOMPOS_VALVE_OPENING(void);
Lightvalve 12:6f2531038ea4 198 void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex);
Lightvalve 12:6f2531038ea4 199 void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex);
Lightvalve 11:82d8768d7351 200 void CAN_TX_VALVE_POS_NUM(void);
Lightvalve 38:118df027d851 201 void CAN_TX_DDV_VALVE_MAX_MIN_POS(void);
Lightvalve 169:645207e160ca 202 void CAN_TX_BUFFER(int16_t t_cnt_buffer);
jobuuu 7:e9086c72bb22 203
jobuuu 7:e9086c72bb22 204 class State
jobuuu 7:e9086c72bb22 205 {
jobuuu 7:e9086c72bb22 206 public:
Lightvalve 30:8d561f16383b 207 float sen;
jobuuu 230:2c3e5ecbe7e1 208 float sen_diff;
Lightvalve 30:8d561f16383b 209 float ref;
Lightvalve 30:8d561f16383b 210 float err;
Lightvalve 30:8d561f16383b 211 float err_int;
Lightvalve 30:8d561f16383b 212 float err_old;
Lightvalve 30:8d561f16383b 213 float err_diff;
Lightvalve 30:8d561f16383b 214 float err_sum;
jobuuu 7:e9086c72bb22 215 public:
jobuuu 7:e9086c72bb22 216 State(){
Lightvalve 30:8d561f16383b 217 sen = 0.0f;
jobuuu 230:2c3e5ecbe7e1 218 sen_diff = 0.0f;
Lightvalve 30:8d561f16383b 219 ref = 0.0f;
Lightvalve 30:8d561f16383b 220 err = 0.0f;
Lightvalve 30:8d561f16383b 221 err_int = 0.0f;
Lightvalve 30:8d561f16383b 222 err_old = 0.0f;
Lightvalve 30:8d561f16383b 223 err_diff = 0.0f;
jobuuu 7:e9086c72bb22 224 }
jobuuu 230:2c3e5ecbe7e1 225
jobuuu 230:2c3e5ecbe7e1 226 void UpdateSen(float sen_new, float Freq_update, float f_cut = 1000.0f);
jobuuu 230:2c3e5ecbe7e1 227 void UpdateRef(float ref_new);
jobuuu 230:2c3e5ecbe7e1 228 void Reset();
jobuuu 7:e9086c72bb22 229 };
jobuuu 7:e9086c72bb22 230
jobuuu 7:e9086c72bb22 231 extern State pos;
jobuuu 7:e9086c72bb22 232 extern State vel;
jobuuu 7:e9086c72bb22 233 extern State Vout;
jobuuu 230:2c3e5ecbe7e1 234 extern State force;
jobuuu 7:e9086c72bb22 235 extern State torq;
Lightvalve 207:c70c5a9f17dd 236 extern State torq_dot;
jobuuu 7:e9086c72bb22 237 extern State pres_A;
jobuuu 7:e9086c72bb22 238 extern State pres_B;
jobuuu 7:e9086c72bb22 239 extern State cur;
Lightvalve 14:8e7590227d22 240 extern State valve_pos;
Lightvalve 14:8e7590227d22 241
Lightvalve 14:8e7590227d22 242 extern State INIT_Vout;
Lightvalve 14:8e7590227d22 243 extern State INIT_Valve_Pos;
Lightvalve 14:8e7590227d22 244 extern State INIT_Pos;
Lightvalve 14:8e7590227d22 245 extern State INIT_torq;
jobuuu 7:e9086c72bb22 246
jobuuu 2:a1c0a37df760 247 // CAN Receive Functions
jobuuu 2:a1c0a37df760 248 void ReadCMD();
jobuuu 2:a1c0a37df760 249 void CAN_RX_HANDLER();
Lightvalve 11:82d8768d7351 250 void CAN_ID_INIT(void);
Lightvalve 11:82d8768d7351 251
Lightvalve 19:23b7c1ad8683 252 extern int ID_index_array[50];
jobuuu 2:a1c0a37df760 253
jobuuu 2:a1c0a37df760 254 #endif
Lightvalve 19:23b7c1ad8683 255
Lightvalve 30:8d561f16383b 256
Lightvalve 45:35fa6884d0c6 257
Lightvalve 50:3c630b5eba9f 258