Aditya Mehrotra / SPIne_Plus_Dyno_v2

Updated for checksum

Dependencies:   mbed-dev

main.cpp@0:76c761d3caf1, 2021-03-20 (annotated)

Committer:
adimmit
Date:
Sat Mar 20 18:38:51 2021 +0000
Revision:
0:76c761d3caf1
Child:
1:aa253b5f5b65
updated to 18 motors

Who changed what in which revision?

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