N/A
Diff: main.cpp
- Revision:
- 0:a043c98470ae
- Child:
- 1:b0bdde8aa220
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Mar 07 18:08:47 2021 +0000 @@ -0,0 +1,224 @@ + +#include "mbed.h" +#include "leg_message.h" +#include "math_ops.h" + +// Master CAN ID /// +#define CAN_ID 0x0 + +/// Value Limits /// +#define P_MIN -12.5f +#define P_MAX 12.5f +#define V_MIN -65.0f +#define V_MAX 65.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 + +/// Joint Soft Stops /// +#define A_LIM_P 1.5f +#define A_LIM_N -1.5f +#define H_LIM_P 5.0f +#define H_LIM_N -5.0f +#define K_LIM_P 0.2f +#define K_LIM_N 7.7f +#define KP_SOFTSTOP 100.0f +#define KD_SOFTSTOP 0.4f; + +DigitalOut led(PC_5); + +Serial pc(PA_2, PA_3); +CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN5 on board +CAN can2(PA_11, PA_12, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN2 on board +CAN can3(PA_8, PA_15, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN4 on board + +CANMessage rxMsg1, rxMsg2; +CANMessage txMsg1, txMsg2; +CANMessage q1_can, q2_can, q3_can; //TX Messages +int ledState; +Ticker sendCAN; +int counter = 0; +volatile bool msgAvailable = false; +Ticker loop; + +leg_state l1_state, l2_state;; +leg_control l1_control, l2_control; + +/// 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, joint_control joint){ + + /// limit data to be within bounds /// + float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX); + float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX); + float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX); + float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX); + float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX); + /// convert floats to unsigned ints /// + uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16); + uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12); + uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12); + uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12); + uint16_t 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, leg_state *leg){ + /// unpack ints from can buffer /// + uint16_t id = msg.data[0]; + uint16_t p_int = (msg.data[1]<<8)|msg.data[2]; + uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4); + uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5]; + /// convert uints 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 t = uint_to_float(i_int, -T_MAX, T_MAX, 12); + + if(id==1){ + leg->a.p = p; + leg->a.v = v; + leg->a.t = t; + } + else if(id==2){ + leg->h.p = p; + leg->h.v = v; + leg->h.t = t; + } + else if(id==3){ + leg->k.p = p; + leg->k.v = v; + leg->k.t = t; + } + } + +void rxISR1() { + can1.read(rxMsg1); // read message into Rx message storage + unpack_reply(rxMsg1, &l1_state); +} +void rxISR2(){ + can2.read(rxMsg2); + unpack_reply(rxMsg2, &l2_state); +} + +void WriteAll(){ + //toggle = 1; + int w1 = can1.write(q1_can); + pc.printf("CAN1 Write: %d\n", w1); + wait(.00002); + int w2 = can2.write(q2_can); + pc.printf("CAN2 Write: %d\n", w2); + wait(.00002); + int w3 = can3.write(q3_can); + pc.printf("CAN3 Write: %d\n", w3); + wait(.00002); + //toggle = 0; + } + +void Random(CANMessage *msg){ + msg->data[0] = 0xF1; + msg->data[1] = 0xF2; + msg->data[2] = 0xF3; + msg->data[3] = 0xF4; + msg->data[4] = 0xF5; + msg->data[5] = 0xF6; + msg->data[6] = 0xF7; + msg->data[7] = 0xF8; + WriteAll(); + } + +void Random2(CANMessage *msg){ + msg->data[0] = 0xF8; + msg->data[1] = 0xF7; + msg->data[2] = 0xF6; + msg->data[3] = 0xF5; + msg->data[4] = 0xF4; + msg->data[5] = 0xF3; + msg->data[6] = 0xF2; + msg->data[7] = 0xF1; + WriteAll(); + } + +void Random3(CANMessage *msg){ + msg->data[0] = 0xF7; + msg->data[1] = 0xF8; + msg->data[2] = 0xF7; + msg->data[3] = 0xF8; + msg->data[4] = 0xF7; + msg->data[5] = 0xF8; + msg->data[6] = 0xF7; + msg->data[7] = 0xF8; + WriteAll(); + } + +int main() { + //setup the PC baud rate + pc.baud(115200); + + //setup the CAN buffers + can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter + can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter + can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter + + //set random messages + Random(&q1_can); + Random2(&q2_can); + Random3(&q3_can); + //write these + while(1) { + WriteAll(); + wait(.25); + //pc.printf("Published New...\n"); + + //IF WE WANT + /* + can2.read(rxMsg2); + unpack_reply(rxMsg2, &l2_state); + can1.read(rxMsg1); // read message into Rx message storage + unpack_reply(rxMsg1, &l1_state); + wait_us(10); + */ + } +} + + + + +