Ben Katz / Mbed 2 deprecated MotorModuleExample

MotorModule CAN example

Dependencies:   mbed

MotorModule/MotorModule.cpp@2:36a254d3dbf3, 2019-08-08 (annotated)

Committer:
benkatz
Date:
Thu Aug 08 17:03:30 2019 +0000
Revision:
2:36a254d3dbf3
Working example

Who changed what in which revision?

UserRevisionLine numberNew 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 }