N/A
main.cpp@0:a043c98470ae, 2021-03-07 (annotated)
- Committer:
- adimmit
- Date:
- Sun Mar 07 18:08:47 2021 +0000
- Revision:
- 0:a043c98470ae
- Child:
- 1:b0bdde8aa220
new repo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
adimmit | 0:a043c98470ae | 1 | |
adimmit | 0:a043c98470ae | 2 | #include "mbed.h" |
adimmit | 0:a043c98470ae | 3 | #include "leg_message.h" |
adimmit | 0:a043c98470ae | 4 | #include "math_ops.h" |
adimmit | 0:a043c98470ae | 5 | |
adimmit | 0:a043c98470ae | 6 | // Master CAN ID /// |
adimmit | 0:a043c98470ae | 7 | #define CAN_ID 0x0 |
adimmit | 0:a043c98470ae | 8 | |
adimmit | 0:a043c98470ae | 9 | /// Value Limits /// |
adimmit | 0:a043c98470ae | 10 | #define P_MIN -12.5f |
adimmit | 0:a043c98470ae | 11 | #define P_MAX 12.5f |
adimmit | 0:a043c98470ae | 12 | #define V_MIN -65.0f |
adimmit | 0:a043c98470ae | 13 | #define V_MAX 65.0f |
adimmit | 0:a043c98470ae | 14 | #define KP_MIN 0.0f |
adimmit | 0:a043c98470ae | 15 | #define KP_MAX 500.0f |
adimmit | 0:a043c98470ae | 16 | #define KD_MIN 0.0f |
adimmit | 0:a043c98470ae | 17 | #define KD_MAX 5.0f |
adimmit | 0:a043c98470ae | 18 | #define T_MIN -18.0f |
adimmit | 0:a043c98470ae | 19 | #define T_MAX 18.0f |
adimmit | 0:a043c98470ae | 20 | |
adimmit | 0:a043c98470ae | 21 | /// Joint Soft Stops /// |
adimmit | 0:a043c98470ae | 22 | #define A_LIM_P 1.5f |
adimmit | 0:a043c98470ae | 23 | #define A_LIM_N -1.5f |
adimmit | 0:a043c98470ae | 24 | #define H_LIM_P 5.0f |
adimmit | 0:a043c98470ae | 25 | #define H_LIM_N -5.0f |
adimmit | 0:a043c98470ae | 26 | #define K_LIM_P 0.2f |
adimmit | 0:a043c98470ae | 27 | #define K_LIM_N 7.7f |
adimmit | 0:a043c98470ae | 28 | #define KP_SOFTSTOP 100.0f |
adimmit | 0:a043c98470ae | 29 | #define KD_SOFTSTOP 0.4f; |
adimmit | 0:a043c98470ae | 30 | |
adimmit | 0:a043c98470ae | 31 | DigitalOut led(PC_5); |
adimmit | 0:a043c98470ae | 32 | |
adimmit | 0:a043c98470ae | 33 | Serial pc(PA_2, PA_3); |
adimmit | 0:a043c98470ae | 34 | CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN5 on board |
adimmit | 0:a043c98470ae | 35 | CAN can2(PA_11, PA_12, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN2 on board |
adimmit | 0:a043c98470ae | 36 | CAN can3(PA_8, PA_15, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN4 on board |
adimmit | 0:a043c98470ae | 37 | |
adimmit | 0:a043c98470ae | 38 | CANMessage rxMsg1, rxMsg2; |
adimmit | 0:a043c98470ae | 39 | CANMessage txMsg1, txMsg2; |
adimmit | 0:a043c98470ae | 40 | CANMessage q1_can, q2_can, q3_can; //TX Messages |
adimmit | 0:a043c98470ae | 41 | int ledState; |
adimmit | 0:a043c98470ae | 42 | Ticker sendCAN; |
adimmit | 0:a043c98470ae | 43 | int counter = 0; |
adimmit | 0:a043c98470ae | 44 | volatile bool msgAvailable = false; |
adimmit | 0:a043c98470ae | 45 | Ticker loop; |
adimmit | 0:a043c98470ae | 46 | |
adimmit | 0:a043c98470ae | 47 | leg_state l1_state, l2_state;; |
adimmit | 0:a043c98470ae | 48 | leg_control l1_control, l2_control; |
adimmit | 0:a043c98470ae | 49 | |
adimmit | 0:a043c98470ae | 50 | /// CAN Command Packet Structure /// |
adimmit | 0:a043c98470ae | 51 | /// 16 bit position command, between -4*pi and 4*pi |
adimmit | 0:a043c98470ae | 52 | /// 12 bit velocity command, between -30 and + 30 rad/s |
adimmit | 0:a043c98470ae | 53 | /// 12 bit kp, between 0 and 500 N-m/rad |
adimmit | 0:a043c98470ae | 54 | /// 12 bit kd, between 0 and 100 N-m*s/rad |
adimmit | 0:a043c98470ae | 55 | /// 12 bit feed forward torque, between -18 and 18 N-m |
adimmit | 0:a043c98470ae | 56 | /// CAN Packet is 8 8-bit words |
adimmit | 0:a043c98470ae | 57 | /// Formatted as follows. For each quantity, bit 0 is LSB |
adimmit | 0:a043c98470ae | 58 | /// 0: [position[15-8]] |
adimmit | 0:a043c98470ae | 59 | /// 1: [position[7-0]] |
adimmit | 0:a043c98470ae | 60 | /// 2: [velocity[11-4]] |
adimmit | 0:a043c98470ae | 61 | /// 3: [velocity[3-0], kp[11-8]] |
adimmit | 0:a043c98470ae | 62 | /// 4: [kp[7-0]] |
adimmit | 0:a043c98470ae | 63 | /// 5: [kd[11-4]] |
adimmit | 0:a043c98470ae | 64 | /// 6: [kd[3-0], torque[11-8]] |
adimmit | 0:a043c98470ae | 65 | /// 7: [torque[7-0]] |
adimmit | 0:a043c98470ae | 66 | |
adimmit | 0:a043c98470ae | 67 | void pack_cmd(CANMessage *msg, joint_control joint){ |
adimmit | 0:a043c98470ae | 68 | |
adimmit | 0:a043c98470ae | 69 | /// limit data to be within bounds /// |
adimmit | 0:a043c98470ae | 70 | float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX); |
adimmit | 0:a043c98470ae | 71 | float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX); |
adimmit | 0:a043c98470ae | 72 | float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX); |
adimmit | 0:a043c98470ae | 73 | float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX); |
adimmit | 0:a043c98470ae | 74 | float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX); |
adimmit | 0:a043c98470ae | 75 | /// convert floats to unsigned ints /// |
adimmit | 0:a043c98470ae | 76 | uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16); |
adimmit | 0:a043c98470ae | 77 | uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12); |
adimmit | 0:a043c98470ae | 78 | uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12); |
adimmit | 0:a043c98470ae | 79 | uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12); |
adimmit | 0:a043c98470ae | 80 | uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12); |
adimmit | 0:a043c98470ae | 81 | /// pack ints into the can buffer /// |
adimmit | 0:a043c98470ae | 82 | msg->data[0] = p_int>>8; |
adimmit | 0:a043c98470ae | 83 | msg->data[1] = p_int&0xFF; |
adimmit | 0:a043c98470ae | 84 | msg->data[2] = v_int>>4; |
adimmit | 0:a043c98470ae | 85 | msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8); |
adimmit | 0:a043c98470ae | 86 | msg->data[4] = kp_int&0xFF; |
adimmit | 0:a043c98470ae | 87 | msg->data[5] = kd_int>>4; |
adimmit | 0:a043c98470ae | 88 | msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8); |
adimmit | 0:a043c98470ae | 89 | msg->data[7] = t_int&0xff; |
adimmit | 0:a043c98470ae | 90 | } |
adimmit | 0:a043c98470ae | 91 | |
adimmit | 0:a043c98470ae | 92 | /// CAN Reply Packet Structure /// |
adimmit | 0:a043c98470ae | 93 | /// 16 bit position, between -4*pi and 4*pi |
adimmit | 0:a043c98470ae | 94 | /// 12 bit velocity, between -30 and + 30 rad/s |
adimmit | 0:a043c98470ae | 95 | /// 12 bit current, between -40 and 40; |
adimmit | 0:a043c98470ae | 96 | /// CAN Packet is 5 8-bit words |
adimmit | 0:a043c98470ae | 97 | /// Formatted as follows. For each quantity, bit 0 is LSB |
adimmit | 0:a043c98470ae | 98 | /// 0: [position[15-8]] |
adimmit | 0:a043c98470ae | 99 | /// 1: [position[7-0]] |
adimmit | 0:a043c98470ae | 100 | /// 2: [velocity[11-4]] |
adimmit | 0:a043c98470ae | 101 | /// 3: [velocity[3-0], current[11-8]] |
adimmit | 0:a043c98470ae | 102 | /// 4: [current[7-0]] |
adimmit | 0:a043c98470ae | 103 | |
adimmit | 0:a043c98470ae | 104 | void unpack_reply(CANMessage msg, leg_state *leg){ |
adimmit | 0:a043c98470ae | 105 | /// unpack ints from can buffer /// |
adimmit | 0:a043c98470ae | 106 | uint16_t id = msg.data[0]; |
adimmit | 0:a043c98470ae | 107 | uint16_t p_int = (msg.data[1]<<8)|msg.data[2]; |
adimmit | 0:a043c98470ae | 108 | uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4); |
adimmit | 0:a043c98470ae | 109 | uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5]; |
adimmit | 0:a043c98470ae | 110 | /// convert uints to floats /// |
adimmit | 0:a043c98470ae | 111 | float p = uint_to_float(p_int, P_MIN, P_MAX, 16); |
adimmit | 0:a043c98470ae | 112 | float v = uint_to_float(v_int, V_MIN, V_MAX, 12); |
adimmit | 0:a043c98470ae | 113 | float t = uint_to_float(i_int, -T_MAX, T_MAX, 12); |
adimmit | 0:a043c98470ae | 114 | |
adimmit | 0:a043c98470ae | 115 | if(id==1){ |
adimmit | 0:a043c98470ae | 116 | leg->a.p = p; |
adimmit | 0:a043c98470ae | 117 | leg->a.v = v; |
adimmit | 0:a043c98470ae | 118 | leg->a.t = t; |
adimmit | 0:a043c98470ae | 119 | } |
adimmit | 0:a043c98470ae | 120 | else if(id==2){ |
adimmit | 0:a043c98470ae | 121 | leg->h.p = p; |
adimmit | 0:a043c98470ae | 122 | leg->h.v = v; |
adimmit | 0:a043c98470ae | 123 | leg->h.t = t; |
adimmit | 0:a043c98470ae | 124 | } |
adimmit | 0:a043c98470ae | 125 | else if(id==3){ |
adimmit | 0:a043c98470ae | 126 | leg->k.p = p; |
adimmit | 0:a043c98470ae | 127 | leg->k.v = v; |
adimmit | 0:a043c98470ae | 128 | leg->k.t = t; |
adimmit | 0:a043c98470ae | 129 | } |
adimmit | 0:a043c98470ae | 130 | } |
adimmit | 0:a043c98470ae | 131 | |
adimmit | 0:a043c98470ae | 132 | void rxISR1() { |
adimmit | 0:a043c98470ae | 133 | can1.read(rxMsg1); // read message into Rx message storage |
adimmit | 0:a043c98470ae | 134 | unpack_reply(rxMsg1, &l1_state); |
adimmit | 0:a043c98470ae | 135 | } |
adimmit | 0:a043c98470ae | 136 | void rxISR2(){ |
adimmit | 0:a043c98470ae | 137 | can2.read(rxMsg2); |
adimmit | 0:a043c98470ae | 138 | unpack_reply(rxMsg2, &l2_state); |
adimmit | 0:a043c98470ae | 139 | } |
adimmit | 0:a043c98470ae | 140 | |
adimmit | 0:a043c98470ae | 141 | void WriteAll(){ |
adimmit | 0:a043c98470ae | 142 | //toggle = 1; |
adimmit | 0:a043c98470ae | 143 | int w1 = can1.write(q1_can); |
adimmit | 0:a043c98470ae | 144 | pc.printf("CAN1 Write: %d\n", w1); |
adimmit | 0:a043c98470ae | 145 | wait(.00002); |
adimmit | 0:a043c98470ae | 146 | int w2 = can2.write(q2_can); |
adimmit | 0:a043c98470ae | 147 | pc.printf("CAN2 Write: %d\n", w2); |
adimmit | 0:a043c98470ae | 148 | wait(.00002); |
adimmit | 0:a043c98470ae | 149 | int w3 = can3.write(q3_can); |
adimmit | 0:a043c98470ae | 150 | pc.printf("CAN3 Write: %d\n", w3); |
adimmit | 0:a043c98470ae | 151 | wait(.00002); |
adimmit | 0:a043c98470ae | 152 | //toggle = 0; |
adimmit | 0:a043c98470ae | 153 | } |
adimmit | 0:a043c98470ae | 154 | |
adimmit | 0:a043c98470ae | 155 | void Random(CANMessage *msg){ |
adimmit | 0:a043c98470ae | 156 | msg->data[0] = 0xF1; |
adimmit | 0:a043c98470ae | 157 | msg->data[1] = 0xF2; |
adimmit | 0:a043c98470ae | 158 | msg->data[2] = 0xF3; |
adimmit | 0:a043c98470ae | 159 | msg->data[3] = 0xF4; |
adimmit | 0:a043c98470ae | 160 | msg->data[4] = 0xF5; |
adimmit | 0:a043c98470ae | 161 | msg->data[5] = 0xF6; |
adimmit | 0:a043c98470ae | 162 | msg->data[6] = 0xF7; |
adimmit | 0:a043c98470ae | 163 | msg->data[7] = 0xF8; |
adimmit | 0:a043c98470ae | 164 | WriteAll(); |
adimmit | 0:a043c98470ae | 165 | } |
adimmit | 0:a043c98470ae | 166 | |
adimmit | 0:a043c98470ae | 167 | void Random2(CANMessage *msg){ |
adimmit | 0:a043c98470ae | 168 | msg->data[0] = 0xF8; |
adimmit | 0:a043c98470ae | 169 | msg->data[1] = 0xF7; |
adimmit | 0:a043c98470ae | 170 | msg->data[2] = 0xF6; |
adimmit | 0:a043c98470ae | 171 | msg->data[3] = 0xF5; |
adimmit | 0:a043c98470ae | 172 | msg->data[4] = 0xF4; |
adimmit | 0:a043c98470ae | 173 | msg->data[5] = 0xF3; |
adimmit | 0:a043c98470ae | 174 | msg->data[6] = 0xF2; |
adimmit | 0:a043c98470ae | 175 | msg->data[7] = 0xF1; |
adimmit | 0:a043c98470ae | 176 | WriteAll(); |
adimmit | 0:a043c98470ae | 177 | } |
adimmit | 0:a043c98470ae | 178 | |
adimmit | 0:a043c98470ae | 179 | void Random3(CANMessage *msg){ |
adimmit | 0:a043c98470ae | 180 | msg->data[0] = 0xF7; |
adimmit | 0:a043c98470ae | 181 | msg->data[1] = 0xF8; |
adimmit | 0:a043c98470ae | 182 | msg->data[2] = 0xF7; |
adimmit | 0:a043c98470ae | 183 | msg->data[3] = 0xF8; |
adimmit | 0:a043c98470ae | 184 | msg->data[4] = 0xF7; |
adimmit | 0:a043c98470ae | 185 | msg->data[5] = 0xF8; |
adimmit | 0:a043c98470ae | 186 | msg->data[6] = 0xF7; |
adimmit | 0:a043c98470ae | 187 | msg->data[7] = 0xF8; |
adimmit | 0:a043c98470ae | 188 | WriteAll(); |
adimmit | 0:a043c98470ae | 189 | } |
adimmit | 0:a043c98470ae | 190 | |
adimmit | 0:a043c98470ae | 191 | int main() { |
adimmit | 0:a043c98470ae | 192 | //setup the PC baud rate |
adimmit | 0:a043c98470ae | 193 | pc.baud(115200); |
adimmit | 0:a043c98470ae | 194 | |
adimmit | 0:a043c98470ae | 195 | //setup the CAN buffers |
adimmit | 0:a043c98470ae | 196 | can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter |
adimmit | 0:a043c98470ae | 197 | can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter |
adimmit | 0:a043c98470ae | 198 | can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter |
adimmit | 0:a043c98470ae | 199 | |
adimmit | 0:a043c98470ae | 200 | //set random messages |
adimmit | 0:a043c98470ae | 201 | Random(&q1_can); |
adimmit | 0:a043c98470ae | 202 | Random2(&q2_can); |
adimmit | 0:a043c98470ae | 203 | Random3(&q3_can); |
adimmit | 0:a043c98470ae | 204 | //write these |
adimmit | 0:a043c98470ae | 205 | while(1) { |
adimmit | 0:a043c98470ae | 206 | WriteAll(); |
adimmit | 0:a043c98470ae | 207 | wait(.25); |
adimmit | 0:a043c98470ae | 208 | //pc.printf("Published New...\n"); |
adimmit | 0:a043c98470ae | 209 | |
adimmit | 0:a043c98470ae | 210 | //IF WE WANT |
adimmit | 0:a043c98470ae | 211 | /* |
adimmit | 0:a043c98470ae | 212 | can2.read(rxMsg2); |
adimmit | 0:a043c98470ae | 213 | unpack_reply(rxMsg2, &l2_state); |
adimmit | 0:a043c98470ae | 214 | can1.read(rxMsg1); // read message into Rx message storage |
adimmit | 0:a043c98470ae | 215 | unpack_reply(rxMsg1, &l1_state); |
adimmit | 0:a043c98470ae | 216 | wait_us(10); |
adimmit | 0:a043c98470ae | 217 | */ |
adimmit | 0:a043c98470ae | 218 | } |
adimmit | 0:a043c98470ae | 219 | } |
adimmit | 0:a043c98470ae | 220 | |
adimmit | 0:a043c98470ae | 221 | |
adimmit | 0:a043c98470ae | 222 | |
adimmit | 0:a043c98470ae | 223 | |
adimmit | 0:a043c98470ae | 224 |