Aditya Mehrotra / CAN_TEST

N/A

Dependencies:   mbed-dev

main.cpp@1:b0bdde8aa220, 2021-03-09 (annotated)

Committer:
adimmit
Date:
Tue Mar 09 20:37:49 2021 +0000
Revision:
1:b0bdde8aa220
Parent:
0:a043c98470ae 
added support for CAN3 fixed MBED library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adimmit 0:a043c98470ae 1
adimmit 0:a043c98470ae 2 #include "mbed.h"
adimmit 0:a043c98470ae 3 #include "leg_message.h"
adimmit 0:a043c98470ae 4 #include "math_ops.h"
adimmit 0:a043c98470ae 5
adimmit 0:a043c98470ae 6 // Master CAN ID ///
adimmit 0:a043c98470ae 7 #define CAN_ID 0x0
adimmit 0:a043c98470ae 8
adimmit 0:a043c98470ae 9 /// Value Limits ///
adimmit 0:a043c98470ae 10 #define P_MIN -12.5f
adimmit 0:a043c98470ae 11 #define P_MAX 12.5f
adimmit 0:a043c98470ae 12 #define V_MIN -65.0f
adimmit 0:a043c98470ae 13 #define V_MAX 65.0f
adimmit 0:a043c98470ae 14 #define KP_MIN 0.0f
adimmit 0:a043c98470ae 15 #define KP_MAX 500.0f
adimmit 0:a043c98470ae 16 #define KD_MIN 0.0f
adimmit 0:a043c98470ae 17 #define KD_MAX 5.0f
adimmit 0:a043c98470ae 18 #define T_MIN -18.0f
adimmit 0:a043c98470ae 19 #define T_MAX 18.0f
adimmit 0:a043c98470ae 20
adimmit 0:a043c98470ae 21 /// Joint Soft Stops ///
adimmit 0:a043c98470ae 22 #define A_LIM_P 1.5f
adimmit 0:a043c98470ae 23 #define A_LIM_N -1.5f
adimmit 0:a043c98470ae 24 #define H_LIM_P 5.0f
adimmit 0:a043c98470ae 25 #define H_LIM_N -5.0f
adimmit 0:a043c98470ae 26 #define K_LIM_P 0.2f
adimmit 0:a043c98470ae 27 #define K_LIM_N 7.7f
adimmit 0:a043c98470ae 28 #define KP_SOFTSTOP 100.0f
adimmit 0:a043c98470ae 29 #define KD_SOFTSTOP 0.4f;
adimmit 0:a043c98470ae 30
adimmit 0:a043c98470ae 31 DigitalOut led(PC_5);
adimmit 0:a043c98470ae 32
adimmit 0:a043c98470ae 33 Serial pc(PA_2, PA_3);
adimmit 0:a043c98470ae 34 CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN5 on board
adimmit 0:a043c98470ae 35 CAN can2(PA_11, PA_12, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN2 on board
adimmit 0:a043c98470ae 36 CAN can3(PA_8, PA_15, 1000000); // CAN Rx pin name, CAN Tx pin name //CAN4 on board
adimmit 0:a043c98470ae 37
adimmit 0:a043c98470ae 38 CANMessage rxMsg1, rxMsg2;
adimmit 0:a043c98470ae 39 CANMessage txMsg1, txMsg2;
adimmit 0:a043c98470ae 40 CANMessage q1_can, q2_can, q3_can; //TX Messages
adimmit 0:a043c98470ae 41 int ledState;
adimmit 0:a043c98470ae 42 Ticker sendCAN;
adimmit 0:a043c98470ae 43 int counter = 0;
adimmit 0:a043c98470ae 44 volatile bool msgAvailable = false;
adimmit 0:a043c98470ae 45 Ticker loop;
adimmit 0:a043c98470ae 46
adimmit 0:a043c98470ae 47 leg_state l1_state, l2_state;;
adimmit 0:a043c98470ae 48 leg_control l1_control, l2_control;
adimmit 0:a043c98470ae 49
adimmit 0:a043c98470ae 50 /// CAN Command Packet Structure ///
adimmit 0:a043c98470ae 51 /// 16 bit position command, between -4*pi and 4*pi
adimmit 0:a043c98470ae 52 /// 12 bit velocity command, between -30 and + 30 rad/s
adimmit 0:a043c98470ae 53 /// 12 bit kp, between 0 and 500 N-m/rad
adimmit 0:a043c98470ae 54 /// 12 bit kd, between 0 and 100 N-m*s/rad
adimmit 0:a043c98470ae 55 /// 12 bit feed forward torque, between -18 and 18 N-m
adimmit 0:a043c98470ae 56 /// CAN Packet is 8 8-bit words
adimmit 0:a043c98470ae 57 /// Formatted as follows. For each quantity, bit 0 is LSB
adimmit 0:a043c98470ae 58 /// 0: [position[15-8]]
adimmit 0:a043c98470ae 59 /// 1: [position[7-0]]
adimmit 0:a043c98470ae 60 /// 2: [velocity[11-4]]
adimmit 0:a043c98470ae 61 /// 3: [velocity[3-0], kp[11-8]]
adimmit 0:a043c98470ae 62 /// 4: [kp[7-0]]
adimmit 0:a043c98470ae 63 /// 5: [kd[11-4]]
adimmit 0:a043c98470ae 64 /// 6: [kd[3-0], torque[11-8]]
adimmit 0:a043c98470ae 65 /// 7: [torque[7-0]]
adimmit 0:a043c98470ae 66
adimmit 0:a043c98470ae 67 void pack_cmd(CANMessage *msg, joint_control joint){
adimmit 0:a043c98470ae 68
adimmit 0:a043c98470ae 69 /// limit data to be within bounds ///
adimmit 0:a043c98470ae 70 float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);
adimmit 0:a043c98470ae 71 float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
adimmit 0:a043c98470ae 72 float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
adimmit 0:a043c98470ae 73 float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
adimmit 0:a043c98470ae 74 float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
adimmit 0:a043c98470ae 75 /// convert floats to unsigned ints ///
adimmit 0:a043c98470ae 76 uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
adimmit 0:a043c98470ae 77 uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
adimmit 0:a043c98470ae 78 uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
adimmit 0:a043c98470ae 79 uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
adimmit 0:a043c98470ae 80 uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
adimmit 0:a043c98470ae 81 /// pack ints into the can buffer ///
adimmit 0:a043c98470ae 82 msg->data[0] = p_int>>8;
adimmit 0:a043c98470ae 83 msg->data[1] = p_int&0xFF;
adimmit 0:a043c98470ae 84 msg->data[2] = v_int>>4;
adimmit 0:a043c98470ae 85 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
adimmit 0:a043c98470ae 86 msg->data[4] = kp_int&0xFF;
adimmit 0:a043c98470ae 87 msg->data[5] = kd_int>>4;
adimmit 0:a043c98470ae 88 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
adimmit 0:a043c98470ae 89 msg->data[7] = t_int&0xff;
adimmit 0:a043c98470ae 90 }
adimmit 0:a043c98470ae 91
adimmit 0:a043c98470ae 92 /// CAN Reply Packet Structure ///
adimmit 0:a043c98470ae 93 /// 16 bit position, between -4*pi and 4*pi
adimmit 0:a043c98470ae 94 /// 12 bit velocity, between -30 and + 30 rad/s
adimmit 0:a043c98470ae 95 /// 12 bit current, between -40 and 40;
adimmit 0:a043c98470ae 96 /// CAN Packet is 5 8-bit words
adimmit 0:a043c98470ae 97 /// Formatted as follows. For each quantity, bit 0 is LSB
adimmit 0:a043c98470ae 98 /// 0: [position[15-8]]
adimmit 0:a043c98470ae 99 /// 1: [position[7-0]]
adimmit 0:a043c98470ae 100 /// 2: [velocity[11-4]]
adimmit 0:a043c98470ae 101 /// 3: [velocity[3-0], current[11-8]]
adimmit 0:a043c98470ae 102 /// 4: [current[7-0]]
adimmit 0:a043c98470ae 103
adimmit 0:a043c98470ae 104 void unpack_reply(CANMessage msg, leg_state *leg){
adimmit 0:a043c98470ae 105 /// unpack ints from can buffer ///
adimmit 0:a043c98470ae 106 uint16_t id = msg.data[0];
adimmit 0:a043c98470ae 107 uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
adimmit 0:a043c98470ae 108 uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
adimmit 0:a043c98470ae 109 uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
adimmit 0:a043c98470ae 110 /// convert uints to floats ///
adimmit 0:a043c98470ae 111 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
adimmit 0:a043c98470ae 112 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
adimmit 0:a043c98470ae 113 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
adimmit 0:a043c98470ae 114
adimmit 0:a043c98470ae 115 if(id==1){
adimmit 0:a043c98470ae 116 leg->a.p = p;
adimmit 0:a043c98470ae 117 leg->a.v = v;
adimmit 0:a043c98470ae 118 leg->a.t = t;
adimmit 0:a043c98470ae 119 }
adimmit 0:a043c98470ae 120 else if(id==2){
adimmit 0:a043c98470ae 121 leg->h.p = p;
adimmit 0:a043c98470ae 122 leg->h.v = v;
adimmit 0:a043c98470ae 123 leg->h.t = t;
adimmit 0:a043c98470ae 124 }
adimmit 0:a043c98470ae 125 else if(id==3){
adimmit 0:a043c98470ae 126 leg->k.p = p;
adimmit 0:a043c98470ae 127 leg->k.v = v;
adimmit 0:a043c98470ae 128 leg->k.t = t;
adimmit 0:a043c98470ae 129 }
adimmit 0:a043c98470ae 130 }
adimmit 0:a043c98470ae 131
adimmit 0:a043c98470ae 132 void rxISR1() {
adimmit 0:a043c98470ae 133 can1.read(rxMsg1); // read message into Rx message storage
adimmit 0:a043c98470ae 134 unpack_reply(rxMsg1, &l1_state);
adimmit 0:a043c98470ae 135 }
adimmit 0:a043c98470ae 136 void rxISR2(){
adimmit 0:a043c98470ae 137 can2.read(rxMsg2);
adimmit 0:a043c98470ae 138 unpack_reply(rxMsg2, &l2_state);
adimmit 0:a043c98470ae 139 }
adimmit 0:a043c98470ae 140
adimmit 0:a043c98470ae 141 void WriteAll(){
adimmit 0:a043c98470ae 142 //toggle = 1;
adimmit 0:a043c98470ae 143 int w1 = can1.write(q1_can);
adimmit 0:a043c98470ae 144 pc.printf("CAN1 Write: %d\n", w1);
adimmit 0:a043c98470ae 145 wait(.00002);
adimmit 0:a043c98470ae 146 int w2 = can2.write(q2_can);
adimmit 0:a043c98470ae 147 pc.printf("CAN2 Write: %d\n", w2);
adimmit 0:a043c98470ae 148 wait(.00002);
adimmit 0:a043c98470ae 149 int w3 = can3.write(q3_can);
adimmit 0:a043c98470ae 150 pc.printf("CAN3 Write: %d\n", w3);
adimmit 0:a043c98470ae 151 wait(.00002);
adimmit 0:a043c98470ae 152 //toggle = 0;
adimmit 0:a043c98470ae 153 }
adimmit 0:a043c98470ae 154
adimmit 0:a043c98470ae 155 void Random(CANMessage *msg){
adimmit 0:a043c98470ae 156 msg->data[0] = 0xF1;
adimmit 0:a043c98470ae 157 msg->data[1] = 0xF2;
adimmit 0:a043c98470ae 158 msg->data[2] = 0xF3;
adimmit 0:a043c98470ae 159 msg->data[3] = 0xF4;
adimmit 0:a043c98470ae 160 msg->data[4] = 0xF5;
adimmit 0:a043c98470ae 161 msg->data[5] = 0xF6;
adimmit 0:a043c98470ae 162 msg->data[6] = 0xF7;
adimmit 0:a043c98470ae 163 msg->data[7] = 0xF8;
adimmit 0:a043c98470ae 164 WriteAll();
adimmit 0:a043c98470ae 165 }
adimmit 0:a043c98470ae 166
adimmit 0:a043c98470ae 167 void Random2(CANMessage *msg){
adimmit 0:a043c98470ae 168 msg->data[0] = 0xF8;
adimmit 0:a043c98470ae 169 msg->data[1] = 0xF7;
adimmit 0:a043c98470ae 170 msg->data[2] = 0xF6;
adimmit 0:a043c98470ae 171 msg->data[3] = 0xF5;
adimmit 0:a043c98470ae 172 msg->data[4] = 0xF4;
adimmit 0:a043c98470ae 173 msg->data[5] = 0xF3;
adimmit 0:a043c98470ae 174 msg->data[6] = 0xF2;
adimmit 0:a043c98470ae 175 msg->data[7] = 0xF1;
adimmit 0:a043c98470ae 176 WriteAll();
adimmit 0:a043c98470ae 177 }
adimmit 0:a043c98470ae 178
adimmit 0:a043c98470ae 179 void Random3(CANMessage *msg){
adimmit 0:a043c98470ae 180 msg->data[0] = 0xF7;
adimmit 0:a043c98470ae 181 msg->data[1] = 0xF8;
adimmit 0:a043c98470ae 182 msg->data[2] = 0xF7;
adimmit 0:a043c98470ae 183 msg->data[3] = 0xF8;
adimmit 0:a043c98470ae 184 msg->data[4] = 0xF7;
adimmit 0:a043c98470ae 185 msg->data[5] = 0xF8;
adimmit 0:a043c98470ae 186 msg->data[6] = 0xF7;
adimmit 0:a043c98470ae 187 msg->data[7] = 0xF8;
adimmit 0:a043c98470ae 188 WriteAll();
adimmit 0:a043c98470ae 189 }
adimmit 0:a043c98470ae 190
adimmit 0:a043c98470ae 191 int main() {
adimmit 0:a043c98470ae 192 //setup the PC baud rate
adimmit 0:a043c98470ae 193 pc.baud(115200);
adimmit 0:a043c98470ae 194
adimmit 0:a043c98470ae 195 //setup the CAN buffers
adimmit 0:a043c98470ae 196 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
adimmit 0:a043c98470ae 197 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
adimmit 0:a043c98470ae 198 can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
adimmit 0:a043c98470ae 199
adimmit 1:b0bdde8aa220 200 //reset all the busses
adimmit 1:b0bdde8aa220 201 //can1.reset();
adimmit 1:b0bdde8aa220 202 //can2.reset();
adimmit 1:b0bdde8aa220 203 //can3.reset();
adimmit 1:b0bdde8aa220 204
adimmit 0:a043c98470ae 205 //set random messages
adimmit 0:a043c98470ae 206 Random(&q1_can);
adimmit 0:a043c98470ae 207 Random2(&q2_can);
adimmit 0:a043c98470ae 208 Random3(&q3_can);
adimmit 0:a043c98470ae 209 //write these
adimmit 0:a043c98470ae 210 while(1) {
adimmit 0:a043c98470ae 211 WriteAll();
adimmit 0:a043c98470ae 212 wait(.25);
adimmit 0:a043c98470ae 213 //pc.printf("Published New...\n");
adimmit 0:a043c98470ae 214
adimmit 0:a043c98470ae 215 //IF WE WANT
adimmit 0:a043c98470ae 216 /*
adimmit 0:a043c98470ae 217 can2.read(rxMsg2);
adimmit 0:a043c98470ae 218 unpack_reply(rxMsg2, &l2_state);
adimmit 0:a043c98470ae 219 can1.read(rxMsg1); // read message into Rx message storage
adimmit 0:a043c98470ae 220 unpack_reply(rxMsg1, &l1_state);
adimmit 0:a043c98470ae 221 wait_us(10);
adimmit 0:a043c98470ae 222 */
adimmit 0:a043c98470ae 223 }
adimmit 0:a043c98470ae 224 }
adimmit 0:a043c98470ae 225
adimmit 0:a043c98470ae 226
adimmit 0:a043c98470ae 227
adimmit 0:a043c98470ae 228
adimmit 0:a043c98470ae 229