Ben Katz
/
MotorModuleExample
MotorModule CAN example
MotorModule/MotorModule.cpp@4:0ce97b9fde37, 2019-08-08 (annotated)
- Committer:
- benkatz
- Date:
- Thu Aug 08 20:03:14 2019 +0000
- Revision:
- 4:0ce97b9fde37
- Parent:
- 2:36a254d3dbf3
test stand program;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 2:36a254d3dbf3 | 1 | |
benkatz | 2:36a254d3dbf3 | 2 | #include "MotorModule.h" |
benkatz | 2:36a254d3dbf3 | 3 | |
benkatz | 2:36a254d3dbf3 | 4 | |
benkatz | 2:36a254d3dbf3 | 5 | /// CAN Command Packet Structure /// |
benkatz | 2:36a254d3dbf3 | 6 | /// 16 bit position command, between -4*pi and 4*pi |
benkatz | 2:36a254d3dbf3 | 7 | /// 12 bit velocity command, between -30 and + 30 rad/s |
benkatz | 2:36a254d3dbf3 | 8 | /// 12 bit kp, between 0 and 500 N-m/rad |
benkatz | 2:36a254d3dbf3 | 9 | /// 12 bit kd, between 0 and 100 N-m*s/rad |
benkatz | 2:36a254d3dbf3 | 10 | /// 12 bit feed forward torque, between -18 and 18 N-m |
benkatz | 2:36a254d3dbf3 | 11 | /// CAN Packet is 8 8-bit words |
benkatz | 2:36a254d3dbf3 | 12 | /// Formatted as follows. For each quantity, bit 0 is LSB |
benkatz | 2:36a254d3dbf3 | 13 | /// 0: [position[15-8]] |
benkatz | 2:36a254d3dbf3 | 14 | /// 1: [position[7-0]] |
benkatz | 2:36a254d3dbf3 | 15 | /// 2: [velocity[11-4]] |
benkatz | 2:36a254d3dbf3 | 16 | /// 3: [velocity[3-0], kp[11-8]] |
benkatz | 2:36a254d3dbf3 | 17 | /// 4: [kp[7-0]] |
benkatz | 2:36a254d3dbf3 | 18 | /// 5: [kd[11-4]] |
benkatz | 2:36a254d3dbf3 | 19 | /// 6: [kd[3-0], torque[11-8]] |
benkatz | 2:36a254d3dbf3 | 20 | /// 7: [torque[7-0]] |
benkatz | 2:36a254d3dbf3 | 21 | |
benkatz | 2:36a254d3dbf3 | 22 | void pack_cmd(MotorStruct * motor){ |
benkatz | 2:36a254d3dbf3 | 23 | /// limit data to be within bounds /// |
benkatz | 2:36a254d3dbf3 | 24 | motor->control.p_des = fminf(fmaxf(P_MIN, motor->control.p_des), P_MAX); |
benkatz | 2:36a254d3dbf3 | 25 | motor->control.v_des = fminf(fmaxf(V_MIN, motor->control.v_des), V_MAX); |
benkatz | 2:36a254d3dbf3 | 26 | motor->control.kp = fminf(fmaxf(KP_MIN, motor->control.kp), KP_MAX); |
benkatz | 2:36a254d3dbf3 | 27 | motor->control.kd = fminf(fmaxf(KD_MIN, motor->control.kd), KD_MAX); |
benkatz | 2:36a254d3dbf3 | 28 | motor->control.i_ff = fminf(fmaxf(I_MIN, motor->control.i_ff), I_MAX); |
benkatz | 2:36a254d3dbf3 | 29 | /// convert floats to unsigned ints /// |
benkatz | 2:36a254d3dbf3 | 30 | int p_int = float_to_uint(motor->control.p_des, P_MIN, P_MAX, 16); |
benkatz | 2:36a254d3dbf3 | 31 | int v_int = float_to_uint(motor->control.v_des, V_MIN, V_MAX, 12); |
benkatz | 2:36a254d3dbf3 | 32 | int kp_int = float_to_uint(motor->control.kp, KP_MIN, KP_MAX, 12); |
benkatz | 2:36a254d3dbf3 | 33 | int kd_int = float_to_uint(motor->control.kd, KD_MIN, KD_MAX, 12); |
benkatz | 2:36a254d3dbf3 | 34 | int t_int = float_to_uint(motor->control.i_ff, I_MIN, I_MAX, 12); |
benkatz | 2:36a254d3dbf3 | 35 | /// pack ints into the can buffer /// |
benkatz | 2:36a254d3dbf3 | 36 | motor->txMsg.data[0] = p_int>>8; |
benkatz | 2:36a254d3dbf3 | 37 | motor->txMsg.data[1] = p_int&0xFF; |
benkatz | 2:36a254d3dbf3 | 38 | motor->txMsg.data[2] = v_int>>4; |
benkatz | 2:36a254d3dbf3 | 39 | motor->txMsg.data[3] = ((v_int&0xF)<<4)|(kp_int>>8); |
benkatz | 2:36a254d3dbf3 | 40 | motor->txMsg.data[4] = kp_int&0xFF; |
benkatz | 2:36a254d3dbf3 | 41 | motor->txMsg.data[5] = kd_int>>4; |
benkatz | 2:36a254d3dbf3 | 42 | motor->txMsg.data[6] = ((kd_int&0xF)<<4)|(t_int>>8); |
benkatz | 2:36a254d3dbf3 | 43 | motor->txMsg.data[7] = t_int&0xff; |
benkatz | 2:36a254d3dbf3 | 44 | } |
benkatz | 2:36a254d3dbf3 | 45 | |
benkatz | 2:36a254d3dbf3 | 46 | /// CAN Reply Packet Structure /// |
benkatz | 2:36a254d3dbf3 | 47 | /// 16 bit position, between -4*pi and 4*pi |
benkatz | 2:36a254d3dbf3 | 48 | /// 12 bit velocity, between -30 and + 30 rad/s |
benkatz | 2:36a254d3dbf3 | 49 | /// 12 bit current, between -40 and 40; |
benkatz | 2:36a254d3dbf3 | 50 | /// CAN Packet is 5 8-bit words |
benkatz | 2:36a254d3dbf3 | 51 | /// Formatted as follows. For each quantity, bit 0 is LSB |
benkatz | 2:36a254d3dbf3 | 52 | /// 0: [position[15-8]] |
benkatz | 2:36a254d3dbf3 | 53 | /// 1: [position[7-0]] |
benkatz | 2:36a254d3dbf3 | 54 | /// 2: [velocity[11-4]] |
benkatz | 2:36a254d3dbf3 | 55 | /// 3: [velocity[3-0], current[11-8]] |
benkatz | 2:36a254d3dbf3 | 56 | /// 4: [current[7-0]] |
benkatz | 2:36a254d3dbf3 | 57 | |
benkatz | 2:36a254d3dbf3 | 58 | void unpack_reply(MotorStruct * motor){ |
benkatz | 2:36a254d3dbf3 | 59 | /// unpack ints from can buffer /// |
benkatz | 2:36a254d3dbf3 | 60 | int id = motor->rxMsg.data[0]; |
benkatz | 2:36a254d3dbf3 | 61 | int p_int = (motor->rxMsg.data[1]<<8)|motor->rxMsg.data[2]; |
benkatz | 2:36a254d3dbf3 | 62 | int v_int = (motor->rxMsg.data[3]<<4)|(motor->rxMsg.data[4]>>4); |
benkatz | 2:36a254d3dbf3 | 63 | int i_int = ((motor->rxMsg.data[4]&0xF)<<8)|motor->rxMsg.data[5]; |
benkatz | 2:36a254d3dbf3 | 64 | /// convert unsigned ints to floats /// |
benkatz | 2:36a254d3dbf3 | 65 | float p = uint_to_float(p_int, P_MIN, P_MAX, 16); |
benkatz | 2:36a254d3dbf3 | 66 | float v = uint_to_float(v_int, V_MIN, V_MAX, 12); |
benkatz | 2:36a254d3dbf3 | 67 | float i = uint_to_float(i_int, -I_MAX, I_MAX, 12); |
benkatz | 2:36a254d3dbf3 | 68 | |
benkatz | 2:36a254d3dbf3 | 69 | motor->state.position = p; |
benkatz | 2:36a254d3dbf3 | 70 | motor->state.velocity = v; |
benkatz | 2:36a254d3dbf3 | 71 | motor->state.current = i; |
benkatz | 2:36a254d3dbf3 | 72 | } |
benkatz | 2:36a254d3dbf3 | 73 | |
benkatz | 2:36a254d3dbf3 | 74 | |
benkatz | 2:36a254d3dbf3 | 75 | void enable_motor(MotorStruct * motor, CAN * can) |
benkatz | 2:36a254d3dbf3 | 76 | { |
benkatz | 2:36a254d3dbf3 | 77 | motor->txMsg.data[0] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 78 | motor->txMsg.data[1] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 79 | motor->txMsg.data[2] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 80 | motor->txMsg.data[3] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 81 | motor->txMsg.data[4] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 82 | motor->txMsg.data[5] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 83 | motor->txMsg.data[6] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 84 | motor->txMsg.data[7] = 0xFC; |
benkatz | 2:36a254d3dbf3 | 85 | can->write(motor->txMsg); |
benkatz | 2:36a254d3dbf3 | 86 | } |
benkatz | 2:36a254d3dbf3 | 87 | void disable_motor(MotorStruct * motor, CAN * can) |
benkatz | 2:36a254d3dbf3 | 88 | { |
benkatz | 2:36a254d3dbf3 | 89 | motor->txMsg.data[0] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 90 | motor->txMsg.data[1] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 91 | motor->txMsg.data[2] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 92 | motor->txMsg.data[3] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 93 | motor->txMsg.data[4] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 94 | motor->txMsg.data[5] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 95 | motor->txMsg.data[6] = 0xFF; |
benkatz | 2:36a254d3dbf3 | 96 | motor->txMsg.data[7] = 0xFD; |
benkatz | 2:36a254d3dbf3 | 97 | can->write(motor->txMsg); |
benkatz | 2:36a254d3dbf3 | 98 | } |