123
Diff: main.cpp
- Revision:
- 0:d6186b8990c5
- Child:
- 1:79e0d4791936
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri May 26 19:47:57 2017 +0000 @@ -0,0 +1,226 @@ + +#define CAN_ID 0x1 + +#include "mbed.h" +#include "math_ops.h" + + +Serial pc(PA_2, PA_3); +CAN can(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name +CANMessage rxMsg; +CANMessage txMsg1; +CANMessage txMsg2; +int ledState; +Timer timer; +Ticker sendCAN; +int counter = 0; +volatile bool msgAvailable = false; +Ticker loop; + + float theta1, theta2, dtheta1, dtheta2; + +/// Value Limits /// + #define P_MIN -12.5f + #define P_MAX 12.5f + #define V_MIN -30.0f + #define V_MAX 30.0f + #define KP_MIN 0.0f + #define KP_MAX 500.0f + #define KD_MIN 0.0f + #define KD_MAX 5.0f + #define T_MIN -18.0f + #define T_MAX 18.0f + #define I_MAX 40.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]] + +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(T_MIN, t_ff), T_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, T_MIN, T_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); + + if(id == 2){ + theta1 = p; + dtheta1 = v; + } + else if(id ==3){ + theta2 = p; + dtheta2 = v; + } + + } + + void onMsgReceived() { + can.read(rxMsg); // read message into Rx message storage + unpack_reply(rxMsg); +} + + +void sendCMD(){ + /// bilateral teleoperation demo /// + pack_cmd(&txMsg1, theta2, dtheta2, 10, .1, 0); + pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0); + can.write(txMsg2); + wait(.0003); // Give motor 1 time to respond. + can.write(txMsg1); + } + +void serial_isr(){ + /// hangle keyboard commands from the serial terminal /// + while(pc.readable()){ + char c = pc.getc(); + switch(c){ + case(27): + printf("\n\r exiting motor mode \n\r"); + txMsg1.data[0] = 0xFF; + txMsg1.data[1] = 0xFF; + txMsg1.data[2] = 0xFF; + txMsg1.data[3] = 0xFF; + txMsg1.data[4] = 0xFF; + txMsg1.data[5] = 0xFF; + txMsg1.data[6] = 0xFF; + txMsg1.data[7] = 0xFD; + + txMsg2.data[0] = 0xFF; + txMsg2.data[1] = 0xFF; + txMsg2.data[2] = 0xFF; + txMsg2.data[3] = 0xFF; + txMsg2.data[4] = 0xFF; + txMsg2.data[5] = 0xFF; + txMsg2.data[6] = 0xFF; + txMsg2.data[7] = 0xFD; + break; + case('m'): + printf("\n\r entering motor mode \n\r"); + txMsg1.data[0] = 0xFF; + txMsg1.data[1] = 0xFF; + txMsg1.data[2] = 0xFF; + txMsg1.data[3] = 0xFF; + txMsg1.data[4] = 0xFF; + txMsg1.data[5] = 0xFF; + txMsg1.data[6] = 0xFF; + txMsg1.data[7] = 0xFC; + + txMsg2.data[0] = 0xFF; + txMsg2.data[1] = 0xFF; + txMsg2.data[2] = 0xFF; + txMsg2.data[3] = 0xFF; + txMsg2.data[4] = 0xFF; + txMsg2.data[5] = 0xFF; + txMsg2.data[6] = 0xFF; + txMsg2.data[7] = 0xFC; + break; + case('z'): + printf("\n\r zeroing \n\r"); + txMsg1.data[0] = 0xFF; + txMsg1.data[1] = 0xFF; + txMsg1.data[2] = 0xFF; + txMsg1.data[3] = 0xFF; + txMsg1.data[4] = 0xFF; + txMsg1.data[5] = 0xFF; + txMsg1.data[6] = 0xFF; + txMsg1.data[7] = 0xFE; + + txMsg2.data[0] = 0xFF; + txMsg2.data[1] = 0xFF; + txMsg2.data[2] = 0xFF; + txMsg2.data[3] = 0xFF; + txMsg2.data[4] = 0xFF; + txMsg2.data[5] = 0xFF; + txMsg2.data[6] = 0xFF; + txMsg2.data[7] = 0xFE; + break; + } + } + can.write(txMsg1); + can.write(txMsg2); + + } + +int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8}; +int main() { + pc.baud(921600); + pc.attach(&serial_isr); + can.frequency(1000000); // set bit rate to 1Mbps + can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler + can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter + printf("Master\n\r"); + //printf("%d\n\r", RX_ID << 18); + int count = 0; + txMsg1.len = 8; //transmit 8 bytes + txMsg2.len = 8; //transmit 8 bytes + rxMsg.len = 6; //receive 5 bytes + loop.attach(&sendCMD, .001); + txMsg1.id = 0x2; //1st motor ID + txMsg2.id = 0x3; //2nd motor ID + pack_cmd(&txMsg1, 0, 0, 0, 0, 0); //Start out by sending all 0's + pack_cmd(&txMsg2, 0, 0, 0, 0, 0); + timer.start(); + + while(1) { + + } + + } + + + + +