Shao Rui / CanMaster

1

Dependencies:   mbed-dev-f303

main.cpp@6:f51261d3fe8f, 2021-06-21 (annotated)

Committer:
shaorui
Date:
Mon Jun 21 01:54:49 2021 +0000
Revision:
6:f51261d3fe8f
Parent:
5:cc20a93f6ad5 
2021.6.21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1
benkatz 2:9b8cb56cfbfa 2 #define CAN_ID 0x0
benkatz 0:d6186b8990c5 3
benkatz 0:d6186b8990c5 4 #include "mbed.h"
benkatz 0:d6186b8990c5 5 #include "math_ops.h"
benkatz 0:d6186b8990c5 6
benkatz 0:d6186b8990c5 7
benkatz 0:d6186b8990c5 8 Serial pc(PA_2, PA_3);
benkatz 1:d24fd64d1fcb 9 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
benkatz 0:d6186b8990c5 10 CANMessage rxMsg;
benkatz 2:9b8cb56cfbfa 11 CANMessage txMsg;
benkatz 0:d6186b8990c5 12 int ledState;
benkatz 0:d6186b8990c5 13 Timer timer;
benkatz 0:d6186b8990c5 14 Ticker sendCAN;
benkatz 0:d6186b8990c5 15 int counter = 0;
benkatz 0:d6186b8990c5 16 volatile bool msgAvailable = false;
benkatz 0:d6186b8990c5 17 Ticker loop;
benkatz 0:d6186b8990c5 18
benkatz 0:d6186b8990c5 19 float theta1, theta2, dtheta1, dtheta2;
benkatz 0:d6186b8990c5 20
benkatz 2:9b8cb56cfbfa 21 float command_f[5];
benkatz 2:9b8cb56cfbfa 22 float data_f[3];
Rushu 4:e9442181a7ad 23 DigitalOut led(PC_5);
benkatz 0:d6186b8990c5 24 /// Value Limits ///
Rushu 4:e9442181a7ad 25 /*
benkatz 3:107df25e1eef 26 #define P_MIN -95.5f
benkatz 3:107df25e1eef 27 #define P_MAX 95.5f
benkatz 1:d24fd64d1fcb 28 #define V_MIN -45.0f
benkatz 1:d24fd64d1fcb 29 #define V_MAX 45.0f
benkatz 0:d6186b8990c5 30 #define KP_MIN 0.0f
benkatz 0:d6186b8990c5 31 #define KP_MAX 500.0f
benkatz 0:d6186b8990c5 32 #define KD_MIN 0.0f
benkatz 0:d6186b8990c5 33 #define KD_MAX 5.0f
benkatz 2:9b8cb56cfbfa 34 #define I_MIN -18.0f
benkatz 2:9b8cb56cfbfa 35 #define I_MAX 18.0f
Rushu 4:e9442181a7ad 36 */
shaorui 5:cc20a93f6ad5 37 //#define P_MIN -12.5f
shaorui 5:cc20a93f6ad5 38 //#define P_MAX 12.5f
shaorui 5:cc20a93f6ad5 39 #define P_MIN 0.04f
shaorui 5:cc20a93f6ad5 40 #define P_MAX 6.24f
shaorui 6:f51261d3fe8f 41 //#define V_MIN -45.0f
shaorui 6:f51261d3fe8f 42 //#define V_MAX 45.0f
shaorui 6:f51261d3fe8f 43 #define V_MIN -15.0f
shaorui 6:f51261d3fe8f 44 #define V_MAX 15.0f
Rushu 4:e9442181a7ad 45 #define KP_MIN 0.0f
Rushu 4:e9442181a7ad 46 #define KP_MAX 500.0f
Rushu 4:e9442181a7ad 47 #define KD_MIN 0.0f
Rushu 4:e9442181a7ad 48 #define KD_MAX 5.0f
shaorui 6:f51261d3fe8f 49 //#define I_MIN -18.0f
shaorui 6:f51261d3fe8f 50 //#define I_MAX 18.0f
shaorui 6:f51261d3fe8f 51 #define I_MIN -50.0f
shaorui 6:f51261d3fe8f 52 #define I_MAX 50.0f
benkatz 0:d6186b8990c5 53 /// CAN Command Packet Structure ///
benkatz 0:d6186b8990c5 54 /// 16 bit position command, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 55 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 56 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 0:d6186b8990c5 57 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 0:d6186b8990c5 58 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 0:d6186b8990c5 59 /// CAN Packet is 8 8-bit words
benkatz 0:d6186b8990c5 60 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 61 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 62 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 63 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 64 /// 3: [velocity[3-0], kp[11-8]]
benkatz 0:d6186b8990c5 65 /// 4: [kp[7-0]]
benkatz 0:d6186b8990c5 66 /// 5: [kd[11-4]]
benkatz 0:d6186b8990c5 67 /// 6: [kd[3-0], torque[11-8]]
benkatz 0:d6186b8990c5 68 /// 7: [torque[7-0]]
benkatz 0:d6186b8990c5 69
benkatz 2:9b8cb56cfbfa 70 union f_b
benkatz 2:9b8cb56cfbfa 71 {
benkatz 2:9b8cb56cfbfa 72 float f;
benkatz 2:9b8cb56cfbfa 73 uint8_t bytes[4];
benkatz 2:9b8cb56cfbfa 74 };
benkatz 2:9b8cb56cfbfa 75
benkatz 2:9b8cb56cfbfa 76 f_b p_act;
benkatz 2:9b8cb56cfbfa 77 f_b v_act;
benkatz 2:9b8cb56cfbfa 78 f_b i_act;
benkatz 2:9b8cb56cfbfa 79 int can_i;
benkatz 2:9b8cb56cfbfa 80
benkatz 0:d6186b8990c5 81 void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){
benkatz 0:d6186b8990c5 82 /// limit data to be within bounds ///
benkatz 0:d6186b8990c5 83 p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
benkatz 0:d6186b8990c5 84 v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
benkatz 0:d6186b8990c5 85 kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
benkatz 0:d6186b8990c5 86 kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
benkatz 2:9b8cb56cfbfa 87 t_ff = fminf(fmaxf(I_MIN, t_ff), I_MAX);
benkatz 0:d6186b8990c5 88 /// convert floats to unsigned ints ///
benkatz 0:d6186b8990c5 89 int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 90 int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 91 int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
benkatz 0:d6186b8990c5 92 int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
benkatz 2:9b8cb56cfbfa 93 int t_int = float_to_uint(t_ff, I_MIN, I_MAX, 12);
benkatz 0:d6186b8990c5 94 /// pack ints into the can buffer ///
benkatz 0:d6186b8990c5 95 msg->data[0] = p_int>>8;
benkatz 0:d6186b8990c5 96 msg->data[1] = p_int&0xFF;
benkatz 0:d6186b8990c5 97 msg->data[2] = v_int>>4;
benkatz 0:d6186b8990c5 98 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
benkatz 0:d6186b8990c5 99 msg->data[4] = kp_int&0xFF;
benkatz 0:d6186b8990c5 100 msg->data[5] = kd_int>>4;
benkatz 0:d6186b8990c5 101 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
benkatz 0:d6186b8990c5 102 msg->data[7] = t_int&0xff;
benkatz 0:d6186b8990c5 103 }
benkatz 0:d6186b8990c5 104
benkatz 0:d6186b8990c5 105 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 106 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 107 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 108 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 109 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 110 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 111 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 112 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 113 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 114 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 115 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 116
benkatz 0:d6186b8990c5 117 void unpack_reply(CANMessage msg){
benkatz 0:d6186b8990c5 118 /// unpack ints from can buffer ///
benkatz 0:d6186b8990c5 119 int id = msg.data[0];
benkatz 0:d6186b8990c5 120 int p_int = (msg.data[1]<<8)|msg.data[2];
benkatz 0:d6186b8990c5 121 int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
benkatz 0:d6186b8990c5 122 int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
benkatz 0:d6186b8990c5 123 /// convert ints to floats ///
benkatz 0:d6186b8990c5 124 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 125 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 126 float i = uint_to_float(i_int, -I_MAX, I_MAX, 12);
benkatz 2:9b8cb56cfbfa 127 //printf("%d %f\n\r", id, p);
benkatz 0:d6186b8990c5 128
benkatz 2:9b8cb56cfbfa 129 can_i = id;
benkatz 2:9b8cb56cfbfa 130 p_act.f = p;
benkatz 2:9b8cb56cfbfa 131 v_act.f = v;
benkatz 2:9b8cb56cfbfa 132 i_act.f = i;
benkatz 2:9b8cb56cfbfa 133
benkatz 0:d6186b8990c5 134
benkatz 1:d24fd64d1fcb 135 //printf("%d %.3f %.3f %.3f\n\r", id, p, v, i);
benkatz 0:d6186b8990c5 136 }
benkatz 0:d6186b8990c5 137
benkatz 0:d6186b8990c5 138 void onMsgReceived() {
benkatz 0:d6186b8990c5 139 can.read(rxMsg); // read message into Rx message storage
benkatz 0:d6186b8990c5 140 unpack_reply(rxMsg);
benkatz 0:d6186b8990c5 141 }
benkatz 0:d6186b8990c5 142
benkatz 2:9b8cb56cfbfa 143 void enable_motor(int id)
benkatz 2:9b8cb56cfbfa 144 {
benkatz 2:9b8cb56cfbfa 145 txMsg.data[0] = 0xFF;
benkatz 2:9b8cb56cfbfa 146 txMsg.data[1] = 0xFF;
benkatz 2:9b8cb56cfbfa 147 txMsg.data[2] = 0xFF;
benkatz 2:9b8cb56cfbfa 148 txMsg.data[3] = 0xFF;
benkatz 2:9b8cb56cfbfa 149 txMsg.data[4] = 0xFF;
benkatz 2:9b8cb56cfbfa 150 txMsg.data[5] = 0xFF;
benkatz 2:9b8cb56cfbfa 151 txMsg.data[6] = 0xFF;
benkatz 2:9b8cb56cfbfa 152 txMsg.data[7] = 0xFC;
benkatz 2:9b8cb56cfbfa 153 can.write(txMsg);
benkatz 2:9b8cb56cfbfa 154 }
benkatz 2:9b8cb56cfbfa 155 void disable_motor(int id)
benkatz 2:9b8cb56cfbfa 156 {
benkatz 2:9b8cb56cfbfa 157 txMsg.data[0] = 0xFF;
benkatz 2:9b8cb56cfbfa 158 txMsg.data[1] = 0xFF;
benkatz 2:9b8cb56cfbfa 159 txMsg.data[2] = 0xFF;
benkatz 2:9b8cb56cfbfa 160 txMsg.data[3] = 0xFF;
benkatz 2:9b8cb56cfbfa 161 txMsg.data[4] = 0xFF;
benkatz 2:9b8cb56cfbfa 162 txMsg.data[5] = 0xFF;
benkatz 2:9b8cb56cfbfa 163 txMsg.data[6] = 0xFF;
benkatz 2:9b8cb56cfbfa 164 txMsg.data[7] = 0xFD;
benkatz 2:9b8cb56cfbfa 165 can.write(txMsg);
benkatz 2:9b8cb56cfbfa 166 }
benkatz 0:d6186b8990c5 167
benkatz 0:d6186b8990c5 168 void sendCMD(){
benkatz 2:9b8cb56cfbfa 169 pack_cmd(&txMsg, 0, 0, 0, 0, -0.1f);
benkatz 1:d24fd64d1fcb 170 //pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0);
benkatz 1:d24fd64d1fcb 171 //pack_cmd(&txMsg2, 0, 0, 0, 0, 1.0);
benkatz 1:d24fd64d1fcb 172 //pack_cmd(&txMsg1, 0, 0, 0, 0, 1.0);
benkatz 1:d24fd64d1fcb 173 //can.write(txMsg2);
benkatz 0:d6186b8990c5 174 wait(.0003); // Give motor 1 time to respond.
benkatz 2:9b8cb56cfbfa 175 can.write(txMsg);
benkatz 2:9b8cb56cfbfa 176 //txMsg1.id = txMsg1.id+1;
benkatz 0:d6186b8990c5 177 }
benkatz 0:d6186b8990c5 178
benkatz 2:9b8cb56cfbfa 179 void serial_isr(void)
benkatz 2:9b8cb56cfbfa 180 {
benkatz 2:9b8cb56cfbfa 181 int b1 = pc.getc();
benkatz 2:9b8cb56cfbfa 182 if(b1==0xFF){
benkatz 2:9b8cb56cfbfa 183 int n = 9;
benkatz 2:9b8cb56cfbfa 184 int bytes[n];
benkatz 2:9b8cb56cfbfa 185 bytes[0] = b1;
benkatz 2:9b8cb56cfbfa 186 for(int i = 1; i<n; i++)
benkatz 2:9b8cb56cfbfa 187 {
benkatz 2:9b8cb56cfbfa 188 bytes[i] = pc.getc();
benkatz 2:9b8cb56cfbfa 189 }
benkatz 2:9b8cb56cfbfa 190 if(((bytes[0]==0xFF) & (bytes[1]==0xFF) & (bytes[2]==0xFF) & (bytes[3]==0xFF) & (bytes[4]==0xFF) & (bytes[5]==0xFF) & (bytes[6]==0xFF) & (bytes[7]==0xFC)))
benkatz 2:9b8cb56cfbfa 191 {enable_motor(bytes[8]);}
benkatz 2:9b8cb56cfbfa 192 else if(((bytes[0]==0xFF) & (bytes[1]==0xFF) & (bytes[2]==0xFF) & (bytes[3]==0xFF) & (bytes[4]==0xFF) & (bytes[5]==0xFF) & (bytes[6]==0xFF) & (bytes[7]==0xFD)))
benkatz 2:9b8cb56cfbfa 193 {disable_motor(bytes[8]);}
benkatz 2:9b8cb56cfbfa 194 }
benkatz 2:9b8cb56cfbfa 195 else
benkatz 2:9b8cb56cfbfa 196 {
benkatz 2:9b8cb56cfbfa 197 int n = 21;
benkatz 2:9b8cb56cfbfa 198 int bytes[n];
benkatz 2:9b8cb56cfbfa 199 bytes[0] = b1;
benkatz 2:9b8cb56cfbfa 200
benkatz 2:9b8cb56cfbfa 201 for(int i = 1; i<n; i++)
benkatz 2:9b8cb56cfbfa 202 {
benkatz 2:9b8cb56cfbfa 203 bytes[i] = pc.getc();
benkatz 0:d6186b8990c5 204 }
benkatz 2:9b8cb56cfbfa 205
benkatz 2:9b8cb56cfbfa 206
benkatz 2:9b8cb56cfbfa 207 f_b p_des;
benkatz 2:9b8cb56cfbfa 208 f_b v_des;
benkatz 2:9b8cb56cfbfa 209 f_b kp;
benkatz 2:9b8cb56cfbfa 210 f_b kd;
benkatz 2:9b8cb56cfbfa 211 f_b i_ff;
benkatz 2:9b8cb56cfbfa 212
benkatz 2:9b8cb56cfbfa 213
benkatz 2:9b8cb56cfbfa 214 for(int i = 0; i<4; i++)
benkatz 2:9b8cb56cfbfa 215 {
benkatz 2:9b8cb56cfbfa 216 p_des.bytes[i] = bytes[1+i];
benkatz 2:9b8cb56cfbfa 217 v_des.bytes[i] = bytes[5+i];
benkatz 2:9b8cb56cfbfa 218 kp.bytes[i] = bytes[9+i];
benkatz 2:9b8cb56cfbfa 219 kd.bytes[i] = bytes[13+i];
benkatz 2:9b8cb56cfbfa 220 i_ff.bytes[i] = bytes[17+i];
benkatz 2:9b8cb56cfbfa 221 }
benkatz 2:9b8cb56cfbfa 222
benkatz 2:9b8cb56cfbfa 223 while(pc.readable()){char flush = pc.getc();}
benkatz 0:d6186b8990c5 224
benkatz 2:9b8cb56cfbfa 225 command_f[0] = p_des.f;
benkatz 2:9b8cb56cfbfa 226 command_f[1] = v_des.f;
benkatz 2:9b8cb56cfbfa 227 command_f[2] = kp.f;
benkatz 2:9b8cb56cfbfa 228 command_f[3] = kd.f;
benkatz 2:9b8cb56cfbfa 229 command_f[4] = i_ff.f;
benkatz 2:9b8cb56cfbfa 230
benkatz 2:9b8cb56cfbfa 231 txMsg.id = bytes[0];
benkatz 2:9b8cb56cfbfa 232 pack_cmd(&txMsg, p_des.f, v_des.f, kp.f, kd.f, i_ff.f);
benkatz 2:9b8cb56cfbfa 233 can.write(txMsg);
benkatz 2:9b8cb56cfbfa 234 wait_us(10);
benkatz 2:9b8cb56cfbfa 235 int tx_bytes[13];
benkatz 2:9b8cb56cfbfa 236 tx_bytes[0] = can_i;
benkatz 2:9b8cb56cfbfa 237 for(int i = 0; i<4; i++)
benkatz 2:9b8cb56cfbfa 238 {
benkatz 2:9b8cb56cfbfa 239 tx_bytes[1+i] = p_act.bytes[i];
benkatz 2:9b8cb56cfbfa 240 tx_bytes[5+i] = v_act.bytes[i];
benkatz 2:9b8cb56cfbfa 241 tx_bytes[9+i] = i_act.bytes[i];
benkatz 2:9b8cb56cfbfa 242 }
benkatz 2:9b8cb56cfbfa 243
benkatz 2:9b8cb56cfbfa 244 for(int i = 0; i<13; i++)
benkatz 2:9b8cb56cfbfa 245 {
benkatz 2:9b8cb56cfbfa 246 pc.putc(tx_bytes[i]);
benkatz 2:9b8cb56cfbfa 247 }
benkatz 2:9b8cb56cfbfa 248
benkatz 0:d6186b8990c5 249 }
benkatz 0:d6186b8990c5 250
benkatz 2:9b8cb56cfbfa 251
benkatz 2:9b8cb56cfbfa 252 }
benkatz 2:9b8cb56cfbfa 253
benkatz 0:d6186b8990c5 254 int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
benkatz 0:d6186b8990c5 255 int main() {
Rushu 4:e9442181a7ad 256 pc.baud(921600);
benkatz 0:d6186b8990c5 257 pc.attach(&serial_isr);
benkatz 0:d6186b8990c5 258 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 2:9b8cb56cfbfa 259 can.filter(CAN_ID , 0xFFF, CANStandard, 0);
Rushu 4:e9442181a7ad 260 //printf("%d\n\r", 1 << 18);
benkatz 0:d6186b8990c5 261 int count = 0;
benkatz 2:9b8cb56cfbfa 262 txMsg.len = 8; //transmit 8 bytes
benkatz 0:d6186b8990c5 263 rxMsg.len = 6; //receive 5 bytes
benkatz 2:9b8cb56cfbfa 264 //loop.attach(&sendCMD, .001);
benkatz 2:9b8cb56cfbfa 265 txMsg.id = 0x1; //1st motor ID
benkatz 2:9b8cb56cfbfa 266 pack_cmd(&txMsg, 0, 0, 0, 0, 0); //Start out by sending all 0's
benkatz 0:d6186b8990c5 267 timer.start();
benkatz 0:d6186b8990c5 268
benkatz 0:d6186b8990c5 269 while(1) {
Rushu 4:e9442181a7ad 270 //printf("%d\n\r", txMsg.data[0]);
Rushu 4:e9442181a7ad 271 wait(0.5);
Rushu 4:e9442181a7ad 272 led = !led;
benkatz 0:d6186b8990c5 273 }
benkatz 0:d6186b8990c5 274
benkatz 0:d6186b8990c5 275 }
benkatz 0:d6186b8990c5 276
benkatz 0:d6186b8990c5 277
benkatz 0:d6186b8990c5 278
benkatz 0:d6186b8990c5 279
benkatz 0:d6186b8990c5 280