Ikhee Jo / MCP_SPIne

MCP_SPIne

Dependencies:   mbed-dev_spine

main.cpp@10:54e95428798b, 2020-06-04 (annotated)

Committer:
Ikhee
Date:
Thu Jun 04 01:26:01 2020 +0000
Revision:
10:54e95428798b
Parent:
9:56747e76c9c1 
Test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1
benkatz 0:d6186b8990c5 2
benkatz 0:d6186b8990c5 3 #include "mbed.h"
benkatz 0:d6186b8990c5 4 #include "math_ops.h"
benkatz 3:9ef9b4c66648 5 #include <cstring>
benkatz 3:9ef9b4c66648 6 #include "leg_message.h"
benkatz 0:d6186b8990c5 7
benkatz 3:9ef9b4c66648 8 // length of receive/transmit buffers
benkatz 3:9ef9b4c66648 9 #define RX_LEN 66
benkatz 3:9ef9b4c66648 10 #define TX_LEN 66
benkatz 0:d6186b8990c5 11
benkatz 3:9ef9b4c66648 12 // length of outgoing/incoming messages
benkatz 3:9ef9b4c66648 13 #define DATA_LEN 30
benkatz 3:9ef9b4c66648 14 #define CMD_LEN 66
benkatz 2:25837cbaee98 15
benkatz 3:9ef9b4c66648 16 // Master CAN ID ///
benkatz 3:9ef9b4c66648 17 #define CAN_ID 0x0
benkatz 2:25837cbaee98 18
benkatz 0:d6186b8990c5 19
benkatz 0:d6186b8990c5 20 /// Value Limits ///
benkatz 0:d6186b8990c5 21 #define P_MIN -12.5f
benkatz 0:d6186b8990c5 22 #define P_MAX 12.5f
benkatz 9:56747e76c9c1 23 #define V_MIN -65.0f
benkatz 9:56747e76c9c1 24 #define V_MAX 65.0f
benkatz 0:d6186b8990c5 25 #define KP_MIN 0.0f
benkatz 0:d6186b8990c5 26 #define KP_MAX 500.0f
benkatz 0:d6186b8990c5 27 #define KD_MIN 0.0f
benkatz 0:d6186b8990c5 28 #define KD_MAX 5.0f
benkatz 0:d6186b8990c5 29 #define T_MIN -18.0f
benkatz 0:d6186b8990c5 30 #define T_MAX 18.0f
benkatz 0:d6186b8990c5 31
benkatz 3:9ef9b4c66648 32 /// Joint Soft Stops ///
benkatz 3:9ef9b4c66648 33 #define A_LIM_P 1.5f
benkatz 3:9ef9b4c66648 34 #define A_LIM_N -1.5f
benkatz 3:9ef9b4c66648 35 #define H_LIM_P 5.0f
benkatz 3:9ef9b4c66648 36 #define H_LIM_N -5.0f
benkatz 3:9ef9b4c66648 37 #define K_LIM_P 0.2f
benkatz 3:9ef9b4c66648 38 #define K_LIM_N 7.7f
benkatz 3:9ef9b4c66648 39 #define KP_SOFTSTOP 100.0f
benkatz 3:9ef9b4c66648 40 #define KD_SOFTSTOP 0.4f;
benkatz 3:9ef9b4c66648 41
benkatz 3:9ef9b4c66648 42 #define ENABLE_CMD 0xFFFF
benkatz 3:9ef9b4c66648 43 #define DISABLE_CMD 0x1F1F
benkatz 3:9ef9b4c66648 44
benkatz 3:9ef9b4c66648 45 spi_data_t spi_data; // data from spine to up
benkatz 3:9ef9b4c66648 46 spi_command_t spi_command; // data from up to spine
benkatz 3:9ef9b4c66648 47
benkatz 3:9ef9b4c66648 48 // spi buffers
benkatz 3:9ef9b4c66648 49 uint16_t rx_buff[RX_LEN];
benkatz 3:9ef9b4c66648 50 uint16_t tx_buff[TX_LEN];
benkatz 3:9ef9b4c66648 51
benkatz 3:9ef9b4c66648 52 DigitalOut led(PC_5);
benkatz 3:9ef9b4c66648 53
benkatz 3:9ef9b4c66648 54
Ikhee 10:54e95428798b 55 //Serial pc(PA_2, PA_3);
Ikhee 10:54e95428798b 56 Serial pc(PA_9, PA_10);
Ikhee 10:54e95428798b 57 CAN can1(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
Ikhee 10:54e95428798b 58 CAN can2(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name
Ikhee 10:54e95428798b 59 //CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name
Ikhee 10:54e95428798b 60 //CAN can2(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
benkatz 3:9ef9b4c66648 61
benkatz 3:9ef9b4c66648 62 CANMessage rxMsg1, rxMsg2;
benkatz 3:9ef9b4c66648 63 CANMessage txMsg1, txMsg2;
benkatz 3:9ef9b4c66648 64 CANMessage a1_can, a2_can, h1_can, h2_can, k1_can, k2_can; //TX Messages
benkatz 3:9ef9b4c66648 65 int ledState;
benkatz 3:9ef9b4c66648 66 Ticker sendCAN;
benkatz 3:9ef9b4c66648 67 int counter = 0;
benkatz 3:9ef9b4c66648 68 volatile bool msgAvailable = false;
benkatz 3:9ef9b4c66648 69 Ticker loop;
benkatz 3:9ef9b4c66648 70
benkatz 3:9ef9b4c66648 71 int spi_enabled = 0;
benkatz 3:9ef9b4c66648 72 InterruptIn cs(PA_4);
Ikhee 10:54e95428798b 73 //DigitalIn estop(PB_15);
benkatz 3:9ef9b4c66648 74 //SPISlave spi(PA_7, PA_6, PA_5, PA_4);
benkatz 3:9ef9b4c66648 75
benkatz 3:9ef9b4c66648 76
benkatz 3:9ef9b4c66648 77 leg_state l1_state, l2_state;;
benkatz 3:9ef9b4c66648 78 leg_control l1_control, l2_control;
benkatz 3:9ef9b4c66648 79
benkatz 3:9ef9b4c66648 80 uint16_t x = 0;
benkatz 3:9ef9b4c66648 81 uint16_t x2 = 0;
benkatz 3:9ef9b4c66648 82 uint16_t count = 0;
benkatz 3:9ef9b4c66648 83 uint16_t counter2 = 0;
benkatz 3:9ef9b4c66648 84
benkatz 3:9ef9b4c66648 85 int control_mode = 1;
benkatz 3:9ef9b4c66648 86 int is_standing = 0;
benkatz 3:9ef9b4c66648 87 int enabled = 0;
benkatz 3:9ef9b4c66648 88
benkatz 3:9ef9b4c66648 89 // generates fake spi data from spi command
benkatz 3:9ef9b4c66648 90 void test_control();
benkatz 3:9ef9b4c66648 91 void control();
benkatz 3:9ef9b4c66648 92
benkatz 3:9ef9b4c66648 93
benkatz 0:d6186b8990c5 94 /// CAN Command Packet Structure ///
benkatz 0:d6186b8990c5 95 /// 16 bit position command, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 96 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 97 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 0:d6186b8990c5 98 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 0:d6186b8990c5 99 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 0:d6186b8990c5 100 /// CAN Packet is 8 8-bit words
benkatz 0:d6186b8990c5 101 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 102 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 103 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 104 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 105 /// 3: [velocity[3-0], kp[11-8]]
benkatz 0:d6186b8990c5 106 /// 4: [kp[7-0]]
benkatz 0:d6186b8990c5 107 /// 5: [kd[11-4]]
benkatz 0:d6186b8990c5 108 /// 6: [kd[3-0], torque[11-8]]
benkatz 0:d6186b8990c5 109 /// 7: [torque[7-0]]
benkatz 0:d6186b8990c5 110
benkatz 3:9ef9b4c66648 111 void pack_cmd(CANMessage * msg, joint_control joint){
benkatz 2:25837cbaee98 112
benkatz 0:d6186b8990c5 113 /// limit data to be within bounds ///
benkatz 3:9ef9b4c66648 114 float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);
benkatz 3:9ef9b4c66648 115 float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
benkatz 3:9ef9b4c66648 116 float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
benkatz 3:9ef9b4c66648 117 float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
benkatz 3:9ef9b4c66648 118 float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
benkatz 0:d6186b8990c5 119 /// convert floats to unsigned ints ///
benkatz 3:9ef9b4c66648 120 uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
benkatz 3:9ef9b4c66648 121 uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
benkatz 3:9ef9b4c66648 122 uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
benkatz 3:9ef9b4c66648 123 uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
benkatz 3:9ef9b4c66648 124 uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
benkatz 0:d6186b8990c5 125 /// pack ints into the can buffer ///
benkatz 0:d6186b8990c5 126 msg->data[0] = p_int>>8;
benkatz 0:d6186b8990c5 127 msg->data[1] = p_int&0xFF;
benkatz 0:d6186b8990c5 128 msg->data[2] = v_int>>4;
benkatz 0:d6186b8990c5 129 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
benkatz 0:d6186b8990c5 130 msg->data[4] = kp_int&0xFF;
benkatz 0:d6186b8990c5 131 msg->data[5] = kd_int>>4;
benkatz 0:d6186b8990c5 132 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
benkatz 0:d6186b8990c5 133 msg->data[7] = t_int&0xff;
benkatz 0:d6186b8990c5 134 }
benkatz 0:d6186b8990c5 135
benkatz 0:d6186b8990c5 136 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 137 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 138 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 139 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 140 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 141 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 142 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 143 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 144 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 145 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 146 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 147
benkatz 3:9ef9b4c66648 148 void unpack_reply(CANMessage msg, leg_state * leg){
benkatz 0:d6186b8990c5 149 /// unpack ints from can buffer ///
benkatz 3:9ef9b4c66648 150 uint16_t id = msg.data[0];
benkatz 3:9ef9b4c66648 151 uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
benkatz 3:9ef9b4c66648 152 uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
benkatz 3:9ef9b4c66648 153 uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
benkatz 3:9ef9b4c66648 154 /// convert uints to floats ///
benkatz 0:d6186b8990c5 155 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 156 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 1:79e0d4791936 157 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
benkatz 3:9ef9b4c66648 158
benkatz 3:9ef9b4c66648 159 if(id==1){
benkatz 3:9ef9b4c66648 160 leg->a.p = p;
benkatz 3:9ef9b4c66648 161 leg->a.v = v;
benkatz 3:9ef9b4c66648 162 leg->a.t = t;
benkatz 2:25837cbaee98 163 }
benkatz 3:9ef9b4c66648 164 else if(id==2){
benkatz 3:9ef9b4c66648 165 leg->h.p = p;
benkatz 3:9ef9b4c66648 166 leg->h.v = v;
benkatz 3:9ef9b4c66648 167 leg->h.t = t;
benkatz 2:25837cbaee98 168 }
benkatz 3:9ef9b4c66648 169 else if(id==3){
benkatz 3:9ef9b4c66648 170 leg->k.p = p;
benkatz 3:9ef9b4c66648 171 leg->k.v = v;
benkatz 3:9ef9b4c66648 172 leg->k.t = t;
benkatz 0:d6186b8990c5 173 }
benkatz 0:d6186b8990c5 174 }
benkatz 3:9ef9b4c66648 175
benkatz 1:79e0d4791936 176 void rxISR1() {
benkatz 1:79e0d4791936 177 can1.read(rxMsg1); // read message into Rx message storage
benkatz 3:9ef9b4c66648 178 unpack_reply(rxMsg1, &l1_state);
benkatz 0:d6186b8990c5 179 }
benkatz 1:79e0d4791936 180 void rxISR2(){
benkatz 1:79e0d4791936 181 can2.read(rxMsg2);
benkatz 3:9ef9b4c66648 182 unpack_reply(rxMsg2, &l2_state);
benkatz 1:79e0d4791936 183 }
benkatz 3:9ef9b4c66648 184 void PackAll(){
benkatz 3:9ef9b4c66648 185 pack_cmd(&a1_can, l1_control.a);
benkatz 3:9ef9b4c66648 186 pack_cmd(&a2_can, l2_control.a);
benkatz 3:9ef9b4c66648 187 pack_cmd(&h1_can, l1_control.h);
benkatz 3:9ef9b4c66648 188 pack_cmd(&h2_can, l2_control.h);
benkatz 3:9ef9b4c66648 189 pack_cmd(&k1_can, l1_control.k);
benkatz 3:9ef9b4c66648 190 pack_cmd(&k2_can, l2_control.k);
benkatz 3:9ef9b4c66648 191
benkatz 3:9ef9b4c66648 192 }
benkatz 1:79e0d4791936 193 void WriteAll(){
benkatz 2:25837cbaee98 194 //toggle = 1;
benkatz 3:9ef9b4c66648 195 can1.write(a1_can);
benkatz 3:9ef9b4c66648 196 wait(.00002);
benkatz 3:9ef9b4c66648 197 can2.write(a2_can);
benkatz 3:9ef9b4c66648 198 wait(.00002);
benkatz 3:9ef9b4c66648 199 can1.write(h1_can);
benkatz 3:9ef9b4c66648 200 wait(.00002);
benkatz 3:9ef9b4c66648 201 can2.write(h2_can);
benkatz 3:9ef9b4c66648 202 wait(.00002);
benkatz 3:9ef9b4c66648 203 can1.write(k1_can);
benkatz 3:9ef9b4c66648 204 wait(.00002);
benkatz 3:9ef9b4c66648 205 can2.write(k2_can);
benkatz 3:9ef9b4c66648 206 wait(.00002);
benkatz 2:25837cbaee98 207 //toggle = 0;
Ikhee 10:54e95428798b 208
Ikhee 10:54e95428798b 209
Ikhee 10:54e95428798b 210 // //toggle = 1;
Ikhee 10:54e95428798b 211 // can1.write(a1_can);
Ikhee 10:54e95428798b 212 // wait(.00002);
Ikhee 10:54e95428798b 213 // can1.write(h1_can);
Ikhee 10:54e95428798b 214 // wait(.00002);
Ikhee 10:54e95428798b 215 // can1.write(k1_can);
Ikhee 10:54e95428798b 216 // wait(.00002);
Ikhee 10:54e95428798b 217 //
Ikhee 10:54e95428798b 218 // can2.write(a2_can);
Ikhee 10:54e95428798b 219 // wait(.00002);
Ikhee 10:54e95428798b 220 // can2.write(h2_can);
Ikhee 10:54e95428798b 221 // wait(.00002);
Ikhee 10:54e95428798b 222 // can2.write(k2_can);
Ikhee 10:54e95428798b 223 // wait(.00002);
Ikhee 10:54e95428798b 224 // //toggle = 0;
Ikhee 10:54e95428798b 225
benkatz 1:79e0d4791936 226 }
benkatz 0:d6186b8990c5 227
benkatz 0:d6186b8990c5 228 void sendCMD(){
benkatz 1:79e0d4791936 229 counter ++;
benkatz 3:9ef9b4c66648 230
benkatz 3:9ef9b4c66648 231 PackAll();
benkatz 3:9ef9b4c66648 232
benkatz 3:9ef9b4c66648 233 if(counter>100){
benkatz 3:9ef9b4c66648 234 printf("%.3f %.3f %.3f %.3f %.3f %.3f\n\r", l1_state.a.p, l1_state.h.p, l1_state.k.p, l2_state.a.p, l2_state.h.p, l2_state.k.p);
benkatz 3:9ef9b4c66648 235 counter = 0 ;
benkatz 3:9ef9b4c66648 236 }
benkatz 1:79e0d4791936 237
benkatz 3:9ef9b4c66648 238 WriteAll();
benkatz 2:25837cbaee98 239
benkatz 3:9ef9b4c66648 240 }
benkatz 2:25837cbaee98 241
benkatz 3:9ef9b4c66648 242
benkatz 3:9ef9b4c66648 243
benkatz 0:d6186b8990c5 244
benkatz 1:79e0d4791936 245 void Zero(CANMessage * msg){
benkatz 1:79e0d4791936 246 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 247 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 248 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 249 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 250 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 251 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 252 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 253 msg->data[7] = 0xFE;
benkatz 1:79e0d4791936 254 WriteAll();
benkatz 1:79e0d4791936 255 }
benkatz 1:79e0d4791936 256
benkatz 1:79e0d4791936 257 void EnterMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 258 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 259 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 260 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 261 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 262 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 263 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 264 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 265 msg->data[7] = 0xFC;
benkatz 3:9ef9b4c66648 266 //WriteAll();
benkatz 1:79e0d4791936 267 }
benkatz 1:79e0d4791936 268
benkatz 1:79e0d4791936 269 void ExitMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 270 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 271 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 272 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 273 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 274 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 275 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 276 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 277 msg->data[7] = 0xFD;
benkatz 3:9ef9b4c66648 278 //WriteAll();
benkatz 1:79e0d4791936 279 }
benkatz 0:d6186b8990c5 280 void serial_isr(){
benkatz 1:79e0d4791936 281 /// handle keyboard commands from the serial terminal ///
benkatz 0:d6186b8990c5 282 while(pc.readable()){
benkatz 0:d6186b8990c5 283 char c = pc.getc();
benkatz 3:9ef9b4c66648 284 //led = !led;
benkatz 0:d6186b8990c5 285 switch(c){
benkatz 0:d6186b8990c5 286 case(27):
benkatz 3:9ef9b4c66648 287 //loop.detach();
benkatz 0:d6186b8990c5 288 printf("\n\r exiting motor mode \n\r");
benkatz 3:9ef9b4c66648 289 ExitMotorMode(&a1_can);
benkatz 3:9ef9b4c66648 290 ExitMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 291 ExitMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 292 ExitMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 293 ExitMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 294 ExitMotorMode(&k2_can);
benkatz 1:79e0d4791936 295 enabled = 0;
benkatz 0:d6186b8990c5 296 break;
benkatz 0:d6186b8990c5 297 case('m'):
benkatz 0:d6186b8990c5 298 printf("\n\r entering motor mode \n\r");
benkatz 3:9ef9b4c66648 299 EnterMotorMode(&a1_can);
benkatz 3:9ef9b4c66648 300 EnterMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 301 EnterMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 302 EnterMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 303 EnterMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 304 EnterMotorMode(&k2_can);
benkatz 1:79e0d4791936 305 wait(.5);
benkatz 1:79e0d4791936 306 enabled = 1;
benkatz 3:9ef9b4c66648 307 //loop.attach(&sendCMD, .001);
benkatz 3:9ef9b4c66648 308 break;
benkatz 3:9ef9b4c66648 309 case('s'):
benkatz 3:9ef9b4c66648 310 printf("\n\r standing \n\r");
benkatz 3:9ef9b4c66648 311 counter2 = 0;
benkatz 3:9ef9b4c66648 312 is_standing = 1;
benkatz 3:9ef9b4c66648 313 //stand();
benkatz 0:d6186b8990c5 314 break;
benkatz 0:d6186b8990c5 315 case('z'):
benkatz 0:d6186b8990c5 316 printf("\n\r zeroing \n\r");
benkatz 3:9ef9b4c66648 317 Zero(&a1_can);
benkatz 3:9ef9b4c66648 318 Zero(&a2_can);
benkatz 3:9ef9b4c66648 319 Zero(&h1_can);
benkatz 3:9ef9b4c66648 320 Zero(&h2_can);
benkatz 3:9ef9b4c66648 321 Zero(&k1_can);
benkatz 3:9ef9b4c66648 322 Zero(&k2_can);
benkatz 2:25837cbaee98 323 break;
benkatz 0:d6186b8990c5 324 }
benkatz 0:d6186b8990c5 325 }
benkatz 1:79e0d4791936 326 WriteAll();
benkatz 0:d6186b8990c5 327
benkatz 0:d6186b8990c5 328 }
benkatz 0:d6186b8990c5 329
benkatz 3:9ef9b4c66648 330 uint32_t xor_checksum(uint32_t* data, size_t len)
benkatz 3:9ef9b4c66648 331 {
benkatz 3:9ef9b4c66648 332 uint32_t t = 0;
benkatz 3:9ef9b4c66648 333 for(int i = 0; i < len; i++)
benkatz 3:9ef9b4c66648 334 t = t ^ data[i];
benkatz 3:9ef9b4c66648 335 return t;
benkatz 3:9ef9b4c66648 336 }
benkatz 3:9ef9b4c66648 337
benkatz 3:9ef9b4c66648 338 void spi_isr(void)
benkatz 3:9ef9b4c66648 339 {
benkatz 3:9ef9b4c66648 340 GPIOC->ODR |= (1 << 8);
benkatz 3:9ef9b4c66648 341 GPIOC->ODR &= ~(1 << 8);
benkatz 3:9ef9b4c66648 342 int bytecount = 0;
benkatz 3:9ef9b4c66648 343 SPI1->DR = tx_buff[0];
benkatz 3:9ef9b4c66648 344 while(cs == 0) {
benkatz 3:9ef9b4c66648 345 if(SPI1->SR&0x1) {
benkatz 3:9ef9b4c66648 346 rx_buff[bytecount] = SPI1->DR;
benkatz 3:9ef9b4c66648 347 bytecount++;
benkatz 3:9ef9b4c66648 348 if(bytecount<TX_LEN) {
benkatz 3:9ef9b4c66648 349 SPI1->DR = tx_buff[bytecount];
benkatz 3:9ef9b4c66648 350 }
benkatz 3:9ef9b4c66648 351 }
benkatz 3:9ef9b4c66648 352
benkatz 3:9ef9b4c66648 353 }
benkatz 2:25837cbaee98 354
benkatz 3:9ef9b4c66648 355 // after reading, save into spi_command
benkatz 3:9ef9b4c66648 356 // should probably check checksum first!
benkatz 3:9ef9b4c66648 357 uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
benkatz 3:9ef9b4c66648 358 for(int i = 0; i < CMD_LEN; i++)
benkatz 3:9ef9b4c66648 359 {
benkatz 3:9ef9b4c66648 360 ((uint16_t*)(&spi_command))[i] = rx_buff[i];
benkatz 3:9ef9b4c66648 361 }
benkatz 1:79e0d4791936 362
benkatz 3:9ef9b4c66648 363 // run control, which fills in tx_buff for the next iteration
benkatz 3:9ef9b4c66648 364 if(calc_checksum != spi_command.checksum){
benkatz 3:9ef9b4c66648 365 spi_data.flags[1] = 0xdead;}
benkatz 3:9ef9b4c66648 366
benkatz 3:9ef9b4c66648 367 //test_control();
benkatz 3:9ef9b4c66648 368 //spi_data.q_abad[0] = 12.0f;
benkatz 3:9ef9b4c66648 369 control();
benkatz 3:9ef9b4c66648 370 PackAll();
benkatz 3:9ef9b4c66648 371 WriteAll();
benkatz 1:79e0d4791936 372
benkatz 2:25837cbaee98 373
benkatz 3:9ef9b4c66648 374 //for (int i = 0; i<TX_LEN; i++) {
benkatz 3:9ef9b4c66648 375 // tx_buff[i] = 2*rx_buff[i];
benkatz 3:9ef9b4c66648 376 //}
benkatz 3:9ef9b4c66648 377 // for (int i=0; i<TX_LEN; i++) {
benkatz 3:9ef9b4c66648 378 // //printf("%d ", rx_buff[i]);
benkatz 3:9ef9b4c66648 379 // }
benkatz 3:9ef9b4c66648 380 //printf("\n\r");
benkatz 3:9ef9b4c66648 381 }
benkatz 3:9ef9b4c66648 382
benkatz 3:9ef9b4c66648 383 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
benkatz 3:9ef9b4c66648 384 if((state.p)>=limit_p){
benkatz 3:9ef9b4c66648 385 //control->p_des = limit_p;
benkatz 3:9ef9b4c66648 386 control->v_des = 0.0f;
benkatz 3:9ef9b4c66648 387 control->kp = 0;
benkatz 3:9ef9b4c66648 388 control->kd = KD_SOFTSTOP;
benkatz 3:9ef9b4c66648 389 control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
benkatz 3:9ef9b4c66648 390 return 1;
benkatz 3:9ef9b4c66648 391 }
benkatz 3:9ef9b4c66648 392 else if((state.p)<=limit_n){
benkatz 3:9ef9b4c66648 393 //control->p_des = limit_n;
benkatz 3:9ef9b4c66648 394 control->v_des = 0.0f;
benkatz 3:9ef9b4c66648 395 control->kp = 0;
benkatz 3:9ef9b4c66648 396 control->kd = KD_SOFTSTOP;
benkatz 3:9ef9b4c66648 397 control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
benkatz 3:9ef9b4c66648 398 return 1;
benkatz 3:9ef9b4c66648 399 }
benkatz 3:9ef9b4c66648 400 return 0;
benkatz 3:9ef9b4c66648 401
benkatz 3:9ef9b4c66648 402 }
benkatz 3:9ef9b4c66648 403
benkatz 3:9ef9b4c66648 404
benkatz 3:9ef9b4c66648 405 void control()
benkatz 3:9ef9b4c66648 406 {
benkatz 3:9ef9b4c66648 407
benkatz 3:9ef9b4c66648 408 if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){
benkatz 3:9ef9b4c66648 409 enabled = 1;
benkatz 3:9ef9b4c66648 410 EnterMotorMode(&a1_can);
benkatz 3:9ef9b4c66648 411 can1.write(a1_can);
benkatz 3:9ef9b4c66648 412 EnterMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 413 can2.write(a2_can);
benkatz 3:9ef9b4c66648 414 EnterMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 415 can1.write(k1_can);
benkatz 3:9ef9b4c66648 416 EnterMotorMode(&k2_can);
benkatz 3:9ef9b4c66648 417 can2.write(k2_can);
benkatz 3:9ef9b4c66648 418 EnterMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 419 can1.write(h1_can);
benkatz 3:9ef9b4c66648 420 EnterMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 421 can2.write(h2_can);
benkatz 3:9ef9b4c66648 422 printf("e\n\r");
benkatz 3:9ef9b4c66648 423 return;
benkatz 3:9ef9b4c66648 424 }
benkatz 3:9ef9b4c66648 425 else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){
benkatz 3:9ef9b4c66648 426 enabled = 0;
benkatz 3:9ef9b4c66648 427 ExitMotorMode(&a1_can);
benkatz 3:9ef9b4c66648 428 can1.write(a1_can);
benkatz 3:9ef9b4c66648 429 ExitMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 430 can2.write(a2_can);
benkatz 3:9ef9b4c66648 431 ExitMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 432 can1.write(h1_can);
benkatz 3:9ef9b4c66648 433 ExitMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 434 can2.write(h2_can);
benkatz 3:9ef9b4c66648 435 ExitMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 436 can1.write(k1_can);
benkatz 3:9ef9b4c66648 437 ExitMotorMode(&k2_can);
benkatz 3:9ef9b4c66648 438 can2.write(k2_can);
benkatz 3:9ef9b4c66648 439 printf("x\n\r");
benkatz 3:9ef9b4c66648 440 return;
benkatz 3:9ef9b4c66648 441 }
benkatz 3:9ef9b4c66648 442
benkatz 3:9ef9b4c66648 443 spi_data.q_abad[0] = l1_state.a.p;
benkatz 3:9ef9b4c66648 444 spi_data.q_hip[0] = l1_state.h.p;
benkatz 3:9ef9b4c66648 445 spi_data.q_knee[0] = l1_state.k.p;
benkatz 3:9ef9b4c66648 446 spi_data.qd_abad[0] = l1_state.a.v;
benkatz 3:9ef9b4c66648 447 spi_data.qd_hip[0] = l1_state.h.v;
benkatz 3:9ef9b4c66648 448 spi_data.qd_knee[0] = l1_state.k.v;
benkatz 3:9ef9b4c66648 449
benkatz 3:9ef9b4c66648 450 spi_data.q_abad[1] = l2_state.a.p;
benkatz 3:9ef9b4c66648 451 spi_data.q_hip[1] = l2_state.h.p;
benkatz 3:9ef9b4c66648 452 spi_data.q_knee[1] = l2_state.k.p;
benkatz 3:9ef9b4c66648 453 spi_data.qd_abad[1] = l2_state.a.v;
benkatz 3:9ef9b4c66648 454 spi_data.qd_hip[1] = l2_state.h.v;
benkatz 3:9ef9b4c66648 455 spi_data.qd_knee[1] = l2_state.k.v;
benkatz 3:9ef9b4c66648 456
benkatz 3:9ef9b4c66648 457
benkatz 3:9ef9b4c66648 458
Ikhee 10:54e95428798b 459 // if(estop==0){
Ikhee 10:54e95428798b 460 // //printf("estopped!!!!\n\r");
Ikhee 10:54e95428798b 461 // memset(&l1_control, 0, sizeof(l1_control));
Ikhee 10:54e95428798b 462 // memset(&l2_control, 0, sizeof(l2_control));
Ikhee 10:54e95428798b 463 // spi_data.flags[0] = 0xdead;
Ikhee 10:54e95428798b 464 // spi_data.flags[1] = 0xdead;
Ikhee 10:54e95428798b 465 // led = 1;
Ikhee 10:54e95428798b 466 // }
Ikhee 10:54e95428798b 467 //
Ikhee 10:54e95428798b 468 // else{
benkatz 3:9ef9b4c66648 469 led = 0;
benkatz 3:9ef9b4c66648 470
benkatz 3:9ef9b4c66648 471 memset(&l1_control, 0, sizeof(l1_control));
benkatz 3:9ef9b4c66648 472 memset(&l2_control, 0, sizeof(l2_control));
benkatz 3:9ef9b4c66648 473
benkatz 3:9ef9b4c66648 474 l1_control.a.p_des = spi_command.q_des_abad[0];
benkatz 3:9ef9b4c66648 475 l1_control.a.v_des = spi_command.qd_des_abad[0];
benkatz 3:9ef9b4c66648 476 l1_control.a.kp = spi_command.kp_abad[0];
benkatz 3:9ef9b4c66648 477 l1_control.a.kd = spi_command.kd_abad[0];
benkatz 3:9ef9b4c66648 478 l1_control.a.t_ff = spi_command.tau_abad_ff[0];
benkatz 3:9ef9b4c66648 479
benkatz 3:9ef9b4c66648 480 l1_control.h.p_des = spi_command.q_des_hip[0];
benkatz 3:9ef9b4c66648 481 l1_control.h.v_des = spi_command.qd_des_hip[0];
benkatz 3:9ef9b4c66648 482 l1_control.h.kp = spi_command.kp_hip[0];
benkatz 3:9ef9b4c66648 483 l1_control.h.kd = spi_command.kd_hip[0];
benkatz 3:9ef9b4c66648 484 l1_control.h.t_ff = spi_command.tau_hip_ff[0];
benkatz 3:9ef9b4c66648 485
benkatz 3:9ef9b4c66648 486 l1_control.k.p_des = spi_command.q_des_knee[0];
benkatz 3:9ef9b4c66648 487 l1_control.k.v_des = spi_command.qd_des_knee[0];
benkatz 3:9ef9b4c66648 488 l1_control.k.kp = spi_command.kp_knee[0];
benkatz 3:9ef9b4c66648 489 l1_control.k.kd = spi_command.kd_knee[0];
benkatz 3:9ef9b4c66648 490 l1_control.k.t_ff = spi_command.tau_knee_ff[0];
benkatz 3:9ef9b4c66648 491
benkatz 3:9ef9b4c66648 492 l2_control.a.p_des = spi_command.q_des_abad[1];
benkatz 3:9ef9b4c66648 493 l2_control.a.v_des = spi_command.qd_des_abad[1];
benkatz 3:9ef9b4c66648 494 l2_control.a.kp = spi_command.kp_abad[1];
benkatz 3:9ef9b4c66648 495 l2_control.a.kd = spi_command.kd_abad[1];
benkatz 3:9ef9b4c66648 496 l2_control.a.t_ff = spi_command.tau_abad_ff[1];
benkatz 3:9ef9b4c66648 497
benkatz 3:9ef9b4c66648 498 l2_control.h.p_des = spi_command.q_des_hip[1];
benkatz 3:9ef9b4c66648 499 l2_control.h.v_des = spi_command.qd_des_hip[1];
benkatz 3:9ef9b4c66648 500 l2_control.h.kp = spi_command.kp_hip[1];
benkatz 3:9ef9b4c66648 501 l2_control.h.kd = spi_command.kd_hip[1];
benkatz 3:9ef9b4c66648 502 l2_control.h.t_ff = spi_command.tau_hip_ff[1];
benkatz 3:9ef9b4c66648 503
benkatz 3:9ef9b4c66648 504 l2_control.k.p_des = spi_command.q_des_knee[1];
benkatz 3:9ef9b4c66648 505 l2_control.k.v_des = spi_command.qd_des_knee[1];
benkatz 3:9ef9b4c66648 506 l2_control.k.kp = spi_command.kp_knee[1];
benkatz 3:9ef9b4c66648 507 l2_control.k.kd = spi_command.kd_knee[1];
benkatz 3:9ef9b4c66648 508 l2_control.k.t_ff = spi_command.tau_knee_ff[1];
benkatz 3:9ef9b4c66648 509
benkatz 3:9ef9b4c66648 510
benkatz 3:9ef9b4c66648 511 spi_data.flags[0] = 0;
benkatz 3:9ef9b4c66648 512 spi_data.flags[1] = 0;
benkatz 3:9ef9b4c66648 513 spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
benkatz 3:9ef9b4c66648 514 spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
benkatz 3:9ef9b4c66648 515 //spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2;
benkatz 3:9ef9b4c66648 516 spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
benkatz 3:9ef9b4c66648 517 spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
benkatz 3:9ef9b4c66648 518 //spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2;
benkatz 3:9ef9b4c66648 519
benkatz 3:9ef9b4c66648 520 //spi_data.flags[0] = 0xbeef;
benkatz 3:9ef9b4c66648 521 //spi_data.flags[1] = 0xbeef;
benkatz 3:9ef9b4c66648 522 //PackAll();
benkatz 3:9ef9b4c66648 523 //WriteAll();
Ikhee 10:54e95428798b 524 // }
benkatz 3:9ef9b4c66648 525 spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
benkatz 3:9ef9b4c66648 526 for(int i = 0; i < DATA_LEN; i++){
benkatz 3:9ef9b4c66648 527 tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
benkatz 3:9ef9b4c66648 528
benkatz 3:9ef9b4c66648 529 }
benkatz 3:9ef9b4c66648 530
benkatz 3:9ef9b4c66648 531
benkatz 3:9ef9b4c66648 532 void test_control()
benkatz 3:9ef9b4c66648 533 {
benkatz 3:9ef9b4c66648 534 for(int i = 0; i < 2; i++)
benkatz 3:9ef9b4c66648 535 {
benkatz 3:9ef9b4c66648 536 spi_data.q_abad[i] = spi_command.q_des_abad[i] + 1.f;
benkatz 3:9ef9b4c66648 537 spi_data.q_knee[i] = spi_command.q_des_knee[i] + 1.f;
benkatz 3:9ef9b4c66648 538 spi_data.q_hip[i] = spi_command.q_des_hip[i] + 1.f;
benkatz 3:9ef9b4c66648 539
benkatz 3:9ef9b4c66648 540 spi_data.qd_abad[i] = spi_command.qd_des_abad[i] + 1.f;
benkatz 3:9ef9b4c66648 541 spi_data.qd_knee[i] = spi_command.qd_des_knee[i] + 1.f;
benkatz 3:9ef9b4c66648 542 spi_data.qd_hip[i] = spi_command.qd_des_hip[i] + 1.f;
benkatz 3:9ef9b4c66648 543 }
benkatz 3:9ef9b4c66648 544
benkatz 3:9ef9b4c66648 545 spi_data.flags[0] = 0xdead;
benkatz 3:9ef9b4c66648 546 //spi_data.flags[1] = 0xbeef;
benkatz 3:9ef9b4c66648 547
benkatz 3:9ef9b4c66648 548 // only do first 56 bytes of message.
benkatz 3:9ef9b4c66648 549 spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
benkatz 3:9ef9b4c66648 550
benkatz 3:9ef9b4c66648 551 for(int i = 0; i < DATA_LEN; i++)
benkatz 3:9ef9b4c66648 552 tx_buff[i] = ((uint16_t*)(&spi_data))[i];
benkatz 3:9ef9b4c66648 553 }
benkatz 3:9ef9b4c66648 554
benkatz 3:9ef9b4c66648 555 void init_spi(void){
benkatz 3:9ef9b4c66648 556 SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
benkatz 3:9ef9b4c66648 557 spi->format(16, 0);
benkatz 3:9ef9b4c66648 558 spi->frequency(12000000);
benkatz 3:9ef9b4c66648 559 spi->reply(0x0);
benkatz 3:9ef9b4c66648 560 cs.fall(&spi_isr);
benkatz 3:9ef9b4c66648 561 printf("done\n\r");
benkatz 3:9ef9b4c66648 562 }
benkatz 3:9ef9b4c66648 563
benkatz 3:9ef9b4c66648 564
benkatz 3:9ef9b4c66648 565 int main() {
benkatz 9:56747e76c9c1 566 wait(1);
benkatz 3:9ef9b4c66648 567 //led = 1;
benkatz 3:9ef9b4c66648 568 pc.baud(921600);
benkatz 3:9ef9b4c66648 569 pc.attach(&serial_isr);
Ikhee 10:54e95428798b 570 // estop.mode(PullUp);
benkatz 3:9ef9b4c66648 571 //spi.format(16, 0);
benkatz 3:9ef9b4c66648 572 //spi.frequency(1000000);
benkatz 3:9ef9b4c66648 573 //spi.reply(0x0);
benkatz 3:9ef9b4c66648 574 //cs.fall(&spi_isr);
benkatz 3:9ef9b4c66648 575
benkatz 7:f10513577d4c 576 //can1.frequency(1000000); // set bit rate to 1Mbps
benkatz 3:9ef9b4c66648 577 //can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler
benkatz 3:9ef9b4c66648 578 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 7:f10513577d4c 579 //can2.frequency(1000000); // set bit rate to 1Mbps
benkatz 3:9ef9b4c66648 580 //can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler
benkatz 3:9ef9b4c66648 581 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 3:9ef9b4c66648 582
benkatz 3:9ef9b4c66648 583 memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
benkatz 3:9ef9b4c66648 584 memset(&spi_data, 0, sizeof(spi_data_t));
benkatz 3:9ef9b4c66648 585 memset(&spi_command,0,sizeof(spi_command_t));
benkatz 3:9ef9b4c66648 586
benkatz 3:9ef9b4c66648 587
benkatz 3:9ef9b4c66648 588 NVIC_SetPriority(TIM5_IRQn, 1);
benkatz 3:9ef9b4c66648 589 //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
benkatz 3:9ef9b4c66648 590 //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
benkatz 3:9ef9b4c66648 591
benkatz 3:9ef9b4c66648 592 printf("\n\r SPIne\n\r");
benkatz 3:9ef9b4c66648 593 //printf("%d\n\r", RX_ID << 18);
benkatz 3:9ef9b4c66648 594
benkatz 3:9ef9b4c66648 595 a1_can.len = 8; //transmit 8 bytes
benkatz 3:9ef9b4c66648 596 a2_can.len = 8; //transmit 8 bytes
benkatz 3:9ef9b4c66648 597 h1_can.len = 8;
benkatz 3:9ef9b4c66648 598 h2_can.len = 8;
benkatz 3:9ef9b4c66648 599 k1_can.len = 8;
benkatz 3:9ef9b4c66648 600 k2_can.len = 8;
benkatz 3:9ef9b4c66648 601 rxMsg1.len = 6; //receive 6 bytes
benkatz 3:9ef9b4c66648 602 rxMsg2.len = 6; //receive 6 bytes
benkatz 3:9ef9b4c66648 603
benkatz 3:9ef9b4c66648 604 a1_can.id = 0x1;
benkatz 3:9ef9b4c66648 605 a2_can.id = 0x1;
benkatz 3:9ef9b4c66648 606 h1_can.id = 0x2;
benkatz 3:9ef9b4c66648 607 h2_can.id = 0x2;
benkatz 3:9ef9b4c66648 608 k1_can.id = 0x3;
benkatz 3:9ef9b4c66648 609 k2_can.id = 0x3;
benkatz 3:9ef9b4c66648 610
benkatz 3:9ef9b4c66648 611 pack_cmd(&a1_can, l1_control.a);
benkatz 3:9ef9b4c66648 612 pack_cmd(&a2_can, l2_control.a);
benkatz 3:9ef9b4c66648 613 pack_cmd(&h1_can, l1_control.h);
benkatz 3:9ef9b4c66648 614 pack_cmd(&h2_can, l2_control.h);
benkatz 3:9ef9b4c66648 615 pack_cmd(&k1_can, l1_control.k);
benkatz 3:9ef9b4c66648 616 pack_cmd(&k2_can, l2_control.k);
benkatz 3:9ef9b4c66648 617 WriteAll();
benkatz 2:25837cbaee98 618
benkatz 2:25837cbaee98 619
benkatz 3:9ef9b4c66648 620 // SPI doesn't work if enabled while the CS pin is pulled low
benkatz 3:9ef9b4c66648 621 // Wait for CS to not be low, then enable SPI
benkatz 3:9ef9b4c66648 622 if(!spi_enabled){
benkatz 3:9ef9b4c66648 623 while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
benkatz 3:9ef9b4c66648 624 init_spi();
benkatz 3:9ef9b4c66648 625 spi_enabled = 1;
benkatz 3:9ef9b4c66648 626 }
benkatz 3:9ef9b4c66648 627
benkatz 3:9ef9b4c66648 628 while(1) {
benkatz 3:9ef9b4c66648 629 counter++;
benkatz 3:9ef9b4c66648 630 can2.read(rxMsg2);
benkatz 3:9ef9b4c66648 631 unpack_reply(rxMsg2, &l2_state);
benkatz 3:9ef9b4c66648 632 can1.read(rxMsg1); // read message into Rx message storage
benkatz 3:9ef9b4c66648 633 unpack_reply(rxMsg1, &l1_state);
benkatz 3:9ef9b4c66648 634 wait_us(10);
benkatz 2:25837cbaee98 635
Ikhee 10:54e95428798b 636 // counter++;
Ikhee 10:54e95428798b 637 // can1.read(rxMsg1);
Ikhee 10:54e95428798b 638 // unpack_reply(rxMsg1, &l1_state);
Ikhee 10:54e95428798b 639 // can2.read(rxMsg2); // read message into Rx message storage
Ikhee 10:54e95428798b 640 // unpack_reply(rxMsg2, &l2_state);
Ikhee 10:54e95428798b 641 // wait_us(10);
Ikhee 10:54e95428798b 642
benkatz 3:9ef9b4c66648 643 }
benkatz 2:25837cbaee98 644
benkatz 0:d6186b8990c5 645
benkatz 3:9ef9b4c66648 646
benkatz 0:d6186b8990c5 647
benkatz 0:d6186b8990c5 648 }
benkatz 0:d6186b8990c5 649
benkatz 0:d6186b8990c5 650
benkatz 0:d6186b8990c5 651
benkatz 0:d6186b8990c5 652
benkatz 0:d6186b8990c5 653