Aditya Mehrotra / SPIne_Plus_Dyno_v2

Updated for checksum

Dependencies:   mbed-dev

main.cpp@5:f1703165ca7e, 2021-04-01 (annotated)

Committer:
adimmit
Date:
Thu Apr 01 21:28:37 2021 +0000
Revision:
5:f1703165ca7e
Parent:
4:e8c1b3f8fc6c
Child:
6:077e37d5c2da
updated structs and re-mapped busses to correct actuators + things

Who changed what in which revision?

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