1
main.cpp
- Committer:
- shaorui
- Date:
- 2020-09-08
- Revision:
- 5:cc20a93f6ad5
- Parent:
- 4:e9442181a7ad
- Child:
- 6:f51261d3fe8f
File content as of revision 5:cc20a93f6ad5:
#define CAN_ID 0x0 #include "mbed.h" #include "math_ops.h" Serial pc(PA_2, PA_3); CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name CANMessage rxMsg; CANMessage txMsg; int ledState; Timer timer; Ticker sendCAN; int counter = 0; volatile bool msgAvailable = false; Ticker loop; float theta1, theta2, dtheta1, dtheta2; float command_f[5]; float data_f[3]; DigitalOut led(PC_5); /// Value Limits /// /* #define P_MIN -95.5f #define P_MAX 95.5f #define V_MIN -45.0f #define V_MAX 45.0f #define KP_MIN 0.0f #define KP_MAX 500.0f #define KD_MIN 0.0f #define KD_MAX 5.0f #define I_MIN -18.0f #define I_MAX 18.0f */ //#define P_MIN -12.5f //#define P_MAX 12.5f #define P_MIN 0.04f #define P_MAX 6.24f #define V_MIN -5.0f #define V_MAX 5.0f #define KP_MIN 0.0f #define KP_MAX 500.0f #define KD_MIN 0.0f #define KD_MAX 5.0f #define I_MIN -18.0f #define I_MAX 18.0f /// CAN Command Packet Structure /// /// 16 bit position command, between -4*pi and 4*pi /// 12 bit velocity command, between -30 and + 30 rad/s /// 12 bit kp, between 0 and 500 N-m/rad /// 12 bit kd, between 0 and 100 N-m*s/rad /// 12 bit feed forward torque, between -18 and 18 N-m /// CAN Packet is 8 8-bit words /// Formatted as follows. For each quantity, bit 0 is LSB /// 0: [position[15-8]] /// 1: [position[7-0]] /// 2: [velocity[11-4]] /// 3: [velocity[3-0], kp[11-8]] /// 4: [kp[7-0]] /// 5: [kd[11-4]] /// 6: [kd[3-0], torque[11-8]] /// 7: [torque[7-0]] union f_b { float f; uint8_t bytes[4]; }; f_b p_act; f_b v_act; f_b i_act; int can_i; void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){ /// limit data to be within bounds /// p_des = fminf(fmaxf(P_MIN, p_des), P_MAX); v_des = fminf(fmaxf(V_MIN, v_des), V_MAX); kp = fminf(fmaxf(KP_MIN, kp), KP_MAX); kd = fminf(fmaxf(KD_MIN, kd), KD_MAX); t_ff = fminf(fmaxf(I_MIN, t_ff), I_MAX); /// convert floats to unsigned ints /// int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16); int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12); int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12); int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12); int t_int = float_to_uint(t_ff, I_MIN, I_MAX, 12); /// pack ints into the can buffer /// msg->data[0] = p_int>>8; msg->data[1] = p_int&0xFF; msg->data[2] = v_int>>4; msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8); msg->data[4] = kp_int&0xFF; msg->data[5] = kd_int>>4; msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8); msg->data[7] = t_int&0xff; } /// CAN Reply Packet Structure /// /// 16 bit position, between -4*pi and 4*pi /// 12 bit velocity, between -30 and + 30 rad/s /// 12 bit current, between -40 and 40; /// CAN Packet is 5 8-bit words /// Formatted as follows. For each quantity, bit 0 is LSB /// 0: [position[15-8]] /// 1: [position[7-0]] /// 2: [velocity[11-4]] /// 3: [velocity[3-0], current[11-8]] /// 4: [current[7-0]] void unpack_reply(CANMessage msg){ /// unpack ints from can buffer /// int id = msg.data[0]; int p_int = (msg.data[1]<<8)|msg.data[2]; int v_int = (msg.data[3]<<4)|(msg.data[4]>>4); int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5]; /// convert ints to floats /// float p = uint_to_float(p_int, P_MIN, P_MAX, 16); float v = uint_to_float(v_int, V_MIN, V_MAX, 12); float i = uint_to_float(i_int, -I_MAX, I_MAX, 12); //printf("%d %f\n\r", id, p); can_i = id; p_act.f = p; v_act.f = v; i_act.f = i; //printf("%d %.3f %.3f %.3f\n\r", id, p, v, i); } void onMsgReceived() { can.read(rxMsg); // read message into Rx message storage unpack_reply(rxMsg); } void enable_motor(int id) { txMsg.data[0] = 0xFF; txMsg.data[1] = 0xFF; txMsg.data[2] = 0xFF; txMsg.data[3] = 0xFF; txMsg.data[4] = 0xFF; txMsg.data[5] = 0xFF; txMsg.data[6] = 0xFF; txMsg.data[7] = 0xFC; can.write(txMsg); } void disable_motor(int id) { txMsg.data[0] = 0xFF; txMsg.data[1] = 0xFF; txMsg.data[2] = 0xFF; txMsg.data[3] = 0xFF; txMsg.data[4] = 0xFF; txMsg.data[5] = 0xFF; txMsg.data[6] = 0xFF; txMsg.data[7] = 0xFD; can.write(txMsg); } void sendCMD(){ pack_cmd(&txMsg, 0, 0, 0, 0, -0.1f); //pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0); //pack_cmd(&txMsg2, 0, 0, 0, 0, 1.0); //pack_cmd(&txMsg1, 0, 0, 0, 0, 1.0); //can.write(txMsg2); wait(.0003); // Give motor 1 time to respond. can.write(txMsg); //txMsg1.id = txMsg1.id+1; } void serial_isr(void) { int b1 = pc.getc(); if(b1==0xFF){ int n = 9; int bytes[n]; bytes[0] = b1; for(int i = 1; i<n; i++) { bytes[i] = pc.getc(); } 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))) {enable_motor(bytes[8]);} 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))) {disable_motor(bytes[8]);} } else { int n = 21; int bytes[n]; bytes[0] = b1; for(int i = 1; i<n; i++) { bytes[i] = pc.getc(); } f_b p_des; f_b v_des; f_b kp; f_b kd; f_b i_ff; for(int i = 0; i<4; i++) { p_des.bytes[i] = bytes[1+i]; v_des.bytes[i] = bytes[5+i]; kp.bytes[i] = bytes[9+i]; kd.bytes[i] = bytes[13+i]; i_ff.bytes[i] = bytes[17+i]; } while(pc.readable()){char flush = pc.getc();} command_f[0] = p_des.f; command_f[1] = v_des.f; command_f[2] = kp.f; command_f[3] = kd.f; command_f[4] = i_ff.f; txMsg.id = bytes[0]; pack_cmd(&txMsg, p_des.f, v_des.f, kp.f, kd.f, i_ff.f); can.write(txMsg); wait_us(10); int tx_bytes[13]; tx_bytes[0] = can_i; for(int i = 0; i<4; i++) { tx_bytes[1+i] = p_act.bytes[i]; tx_bytes[5+i] = v_act.bytes[i]; tx_bytes[9+i] = i_act.bytes[i]; } for(int i = 0; i<13; i++) { pc.putc(tx_bytes[i]); } } } int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8}; int main() { pc.baud(921600); pc.attach(&serial_isr); can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler can.filter(CAN_ID , 0xFFF, CANStandard, 0); //printf("%d\n\r", 1 << 18); int count = 0; txMsg.len = 8; //transmit 8 bytes rxMsg.len = 6; //receive 5 bytes //loop.attach(&sendCMD, .001); txMsg.id = 0x1; //1st motor ID pack_cmd(&txMsg, 0, 0, 0, 0, 0); //Start out by sending all 0's timer.start(); while(1) { //printf("%d\n\r", txMsg.data[0]); wait(0.5); led = !led; } }