Updated for checksum
main.cpp@12:5f97bcab6c50, 2022-02-03 (annotated)
- Committer:
- adimmit
- Date:
- Thu Feb 03 19:10:54 2022 +0000
- Revision:
- 12:5f97bcab6c50
- Parent:
- 11:40d5d9b5d2ba
- Child:
- 13:87e7ce883f6a
idk man;
Who changed what in which revision?
User | Revision | Line number | New 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 | 11:40d5d9b5d2ba | 17 | #define DATA_LEN 62 //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 | 7:2f785cb39f11 | 37 | #define A1_LIM_P 1.5f |
adimmit | 7:2f785cb39f11 | 38 | #define A1_LIM_N -1.5f |
adimmit | 7:2f785cb39f11 | 39 | #define A2_LIM_P 1.5f |
adimmit | 7:2f785cb39f11 | 40 | #define A2_LIM_N -1.5f |
adimmit | 7:2f785cb39f11 | 41 | #define A3_LIM_P 1.5f |
adimmit | 7:2f785cb39f11 | 42 | #define A3_LIM_N -1.5f |
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 | 6:077e37d5c2da | 193 | pack_cmd(&q11_can, g1_control.a1); |
adimmit | 6:077e37d5c2da | 194 | pack_cmd(&q12_can, g1_control.a2); |
adimmit | 6:077e37d5c2da | 195 | pack_cmd(&q13_can, g1_control.a3); |
adimmit | 5:f1703165ca7e | 196 | //actuators on the CAN2 bus |
adimmit | 6:077e37d5c2da | 197 | pack_cmd(&q21_can, g2_control.a1); |
adimmit | 6:077e37d5c2da | 198 | pack_cmd(&q22_can, g2_control.a2); |
adimmit | 6:077e37d5c2da | 199 | pack_cmd(&q23_can, g2_control.a3); |
adimmit | 5:f1703165ca7e | 200 | //actuators on the CAN3 bus |
adimmit | 6:077e37d5c2da | 201 | pack_cmd(&q31_can, g3_control.a1); |
adimmit | 6:077e37d5c2da | 202 | pack_cmd(&q32_can, g3_control.a2); |
adimmit | 6:077e37d5c2da | 203 | pack_cmd(&q33_can, g3_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 | 6:077e37d5c2da | 237 | pc.printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", g1_state.a1.p, g1_state.a2.p, g1_state.a3.p, g2_state.a1.p, g2_state.a2.p, g2_state.a3.p, g3_state.a1.p, g3_state.a2.p, g3_state.a3.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 | 8:450fbccaf4f6 | 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 | 8:450fbccaf4f6 | 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 | 8:450fbccaf4f6 | 280 | //WriteAll(); |
adimmit | 0:76c761d3caf1 | 281 | } |
adimmit | 5:f1703165ca7e | 282 | |
adimmit | 8:450fbccaf4f6 | 283 | |
adimmit | 0:76c761d3caf1 | 284 | void serial_isr(){ |
adimmit | 0:76c761d3caf1 | 285 | /// handle keyboard commands from the serial terminal /// |
adimmit | 0:76c761d3caf1 | 286 | while(pc.readable()){ |
adimmit | 0:76c761d3caf1 | 287 | char c = pc.getc(); |
adimmit | 0:76c761d3caf1 | 288 | //led = !led; |
adimmit | 0:76c761d3caf1 | 289 | switch(c){ |
adimmit | 0:76c761d3caf1 | 290 | case(27): |
adimmit | 0:76c761d3caf1 | 291 | //loop.detach(); |
adimmit | 1:aa253b5f5b65 | 292 | pc.printf("\n\r exiting motor mode \n\r"); |
adimmit | 5:f1703165ca7e | 293 | //CAN BUS 1 |
adimmit | 0:76c761d3caf1 | 294 | ExitMotorMode(&q11_can); |
adimmit | 5:f1703165ca7e | 295 | ExitMotorMode(&q12_can); |
adimmit | 5:f1703165ca7e | 296 | ExitMotorMode(&q13_can); |
adimmit | 5:f1703165ca7e | 297 | //CAN BUS 2 |
adimmit | 0:76c761d3caf1 | 298 | ExitMotorMode(&q21_can); |
adimmit | 0:76c761d3caf1 | 299 | ExitMotorMode(&q22_can); |
adimmit | 5:f1703165ca7e | 300 | ExitMotorMode(&q23_can); |
adimmit | 5:f1703165ca7e | 301 | //CAN BUS 3 |
adimmit | 5:f1703165ca7e | 302 | ExitMotorMode(&q31_can); |
adimmit | 0:76c761d3caf1 | 303 | ExitMotorMode(&q32_can); |
adimmit | 0:76c761d3caf1 | 304 | ExitMotorMode(&q33_can); |
adimmit | 5:f1703165ca7e | 305 | //DISABLE FLAG |
adimmit | 0:76c761d3caf1 | 306 | enabled = 0; |
adimmit | 0:76c761d3caf1 | 307 | break; |
adimmit | 0:76c761d3caf1 | 308 | case('m'): |
adimmit | 1:aa253b5f5b65 | 309 | pc.printf("\n\r entering motor mode \n\r"); |
adimmit | 5:f1703165ca7e | 310 | //CAN BUS 1 |
adimmit | 0:76c761d3caf1 | 311 | EnterMotorMode(&q11_can); |
adimmit | 5:f1703165ca7e | 312 | EnterMotorMode(&q12_can); |
adimmit | 5:f1703165ca7e | 313 | EnterMotorMode(&q13_can); |
adimmit | 5:f1703165ca7e | 314 | //CAN BUS 2 |
adimmit | 0:76c761d3caf1 | 315 | EnterMotorMode(&q21_can); |
adimmit | 0:76c761d3caf1 | 316 | EnterMotorMode(&q22_can); |
adimmit | 5:f1703165ca7e | 317 | EnterMotorMode(&q23_can); |
adimmit | 5:f1703165ca7e | 318 | //CAN BUS 3 |
adimmit | 5:f1703165ca7e | 319 | EnterMotorMode(&q31_can); |
adimmit | 0:76c761d3caf1 | 320 | EnterMotorMode(&q32_can); |
adimmit | 0:76c761d3caf1 | 321 | EnterMotorMode(&q33_can); |
adimmit | 5:f1703165ca7e | 322 | //WAIT FOR ENABLE |
adimmit | 0:76c761d3caf1 | 323 | wait(.5); |
adimmit | 5:f1703165ca7e | 324 | //ENABLE FLAG |
adimmit | 0:76c761d3caf1 | 325 | enabled = 1; |
adimmit | 0:76c761d3caf1 | 326 | //loop.attach(&sendCMD, .001); |
adimmit | 0:76c761d3caf1 | 327 | break; |
adimmit | 0:76c761d3caf1 | 328 | case('s'): |
adimmit | 1:aa253b5f5b65 | 329 | pc.printf("\n\r standing \n\r"); |
adimmit | 0:76c761d3caf1 | 330 | counter2 = 0; |
adimmit | 0:76c761d3caf1 | 331 | is_standing = 1; |
adimmit | 0:76c761d3caf1 | 332 | //stand(); |
adimmit | 0:76c761d3caf1 | 333 | break; |
adimmit | 0:76c761d3caf1 | 334 | case('z'): |
adimmit | 1:aa253b5f5b65 | 335 | pc.printf("\n\r zeroing \n\r"); |
adimmit | 5:f1703165ca7e | 336 | //CAN BUS 1 |
adimmit | 0:76c761d3caf1 | 337 | Zero(&q11_can); |
adimmit | 0:76c761d3caf1 | 338 | Zero(&q12_can); |
adimmit | 5:f1703165ca7e | 339 | Zero(&q13_can); |
adimmit | 5:f1703165ca7e | 340 | //CAN BUS 2 |
adimmit | 5:f1703165ca7e | 341 | Zero(&q21_can); |
adimmit | 0:76c761d3caf1 | 342 | Zero(&q22_can); |
adimmit | 5:f1703165ca7e | 343 | Zero(&q23_can); |
adimmit | 5:f1703165ca7e | 344 | //CAN BUS 3 |
adimmit | 5:f1703165ca7e | 345 | Zero(&q31_can); |
adimmit | 0:76c761d3caf1 | 346 | Zero(&q32_can); |
adimmit | 0:76c761d3caf1 | 347 | Zero(&q33_can); |
adimmit | 0:76c761d3caf1 | 348 | break; |
adimmit | 0:76c761d3caf1 | 349 | } |
adimmit | 0:76c761d3caf1 | 350 | } |
adimmit | 0:76c761d3caf1 | 351 | WriteAll(); |
adimmit | 0:76c761d3caf1 | 352 | |
adimmit | 0:76c761d3caf1 | 353 | } |
adimmit | 8:450fbccaf4f6 | 354 | |
adimmit | 0:76c761d3caf1 | 355 | |
adimmit | 0:76c761d3caf1 | 356 | uint32_t xor_checksum(uint32_t* data, size_t len) |
adimmit | 0:76c761d3caf1 | 357 | { |
adimmit | 0:76c761d3caf1 | 358 | uint32_t t = 0; |
adimmit | 0:76c761d3caf1 | 359 | for(int i = 0; i < len; i++) |
adimmit | 0:76c761d3caf1 | 360 | t = t ^ data[i]; |
adimmit | 0:76c761d3caf1 | 361 | return t; |
adimmit | 0:76c761d3caf1 | 362 | } |
adimmit | 0:76c761d3caf1 | 363 | |
adimmit | 2:32f69175c78e | 364 | |
adimmit | 2:32f69175c78e | 365 | |
adimmit | 2:32f69175c78e | 366 | void print_SPI_command() { |
adimmit | 2:32f69175c78e | 367 | pc.printf("SPI MESSAGE RECIEVED:\n"); |
adimmit | 5:f1703165ca7e | 368 | //CAN ONE |
adimmit | 5:f1703165ca7e | 369 | pc.printf("MOTOR 1-1 Q: %f\n", spi_command.q_des_1s[0]); |
adimmit | 5:f1703165ca7e | 370 | pc.printf("MOTOR 1-1 Qd: %f\n", spi_command.qd_des_1s[0]); |
adimmit | 5:f1703165ca7e | 371 | pc.printf("MOTOR 1-1 Kp: %f\n", spi_command.kp_1s[0]); |
adimmit | 5:f1703165ca7e | 372 | pc.printf("MOTOR 1-1 Kd: %f\n", spi_command.kd_1s[0]); |
adimmit | 5:f1703165ca7e | 373 | pc.printf("MOTOR 1-1 T_FF: %f\n", spi_command.tau_1s_ff[0]); |
adimmit | 2:32f69175c78e | 374 | |
adimmit | 5:f1703165ca7e | 375 | pc.printf("MOTOR 1-2 Q: %f\n", spi_command.q_des_2s[0]); |
adimmit | 5:f1703165ca7e | 376 | pc.printf("MOTOR 1-2 Qd: %f\n", spi_command.qd_des_2s[0]); |
adimmit | 5:f1703165ca7e | 377 | pc.printf("MOTOR 1-2 Kp: %f\n", spi_command.kp_2s[0]); |
adimmit | 5:f1703165ca7e | 378 | pc.printf("MOTOR 1-2 Kd: %f\n", spi_command.kd_2s[0]); |
adimmit | 5:f1703165ca7e | 379 | pc.printf("MOTOR 1-2 T_FF: %f\n", spi_command.tau_2s_ff[0]); |
adimmit | 2:32f69175c78e | 380 | |
adimmit | 5:f1703165ca7e | 381 | pc.printf("MOTOR 1-3 Q: %f\n", spi_command.q_des_3s[0]); |
adimmit | 5:f1703165ca7e | 382 | pc.printf("MOTOR 1-3 Qd: %f\n", spi_command.qd_des_3s[0]); |
adimmit | 5:f1703165ca7e | 383 | pc.printf("MOTOR 1-3 Kp: %f\n", spi_command.kp_3s[0]); |
adimmit | 5:f1703165ca7e | 384 | pc.printf("MOTOR 1-3 Kd: %f\n", spi_command.kd_3s[0]); |
adimmit | 5:f1703165ca7e | 385 | pc.printf("MOTOR 1-3 T_FF: %f\n", spi_command.tau_3s_ff[0]); |
adimmit | 2:32f69175c78e | 386 | |
adimmit | 5:f1703165ca7e | 387 | pc.printf("MOTOR 2-1 Q: %f\n", spi_command.q_des_1s[1]); |
adimmit | 5:f1703165ca7e | 388 | pc.printf("MOTOR 2-1 Qd: %f\n", spi_command.qd_des_1s[1]); |
adimmit | 5:f1703165ca7e | 389 | pc.printf("MOTOR 2-1 Kp: %f\n", spi_command.kp_1s[1]); |
adimmit | 5:f1703165ca7e | 390 | pc.printf("MOTOR 2-1 Kd: %f\n", spi_command.kd_1s[1]); |
adimmit | 5:f1703165ca7e | 391 | pc.printf("MOTOR 2-1_FF: %f\n", spi_command.tau_1s_ff[1]); |
adimmit | 2:32f69175c78e | 392 | |
adimmit | 5:f1703165ca7e | 393 | pc.printf("MOTOR 2-2 Q: %f\n", spi_command.q_des_2s[1]); |
adimmit | 5:f1703165ca7e | 394 | pc.printf("MOTOR 2-2 Qd: %f\n", spi_command.qd_des_2s[1]); |
adimmit | 5:f1703165ca7e | 395 | pc.printf("MOTOR 2-2 Kp: %f\n", spi_command.kp_2s[1]); |
adimmit | 5:f1703165ca7e | 396 | pc.printf("MOTOR 2-2 Kd: %f\n", spi_command.kd_2s[1]); |
adimmit | 5:f1703165ca7e | 397 | pc.printf("MOTOR 2-2 T_FF: %f\n", spi_command.tau_2s_ff[1]); |
adimmit | 2:32f69175c78e | 398 | |
adimmit | 5:f1703165ca7e | 399 | pc.printf("MOTOR 2-3 Q: %f\n", spi_command.q_des_3s[1]); |
adimmit | 5:f1703165ca7e | 400 | pc.printf("MOTOR 2-3 Qd: %f\n", spi_command.qd_des_3s[1]); |
adimmit | 5:f1703165ca7e | 401 | pc.printf("MOTOR 2-3 Kp: %f\n", spi_command.kp_3s[1]); |
adimmit | 5:f1703165ca7e | 402 | pc.printf("MOTOR 2-3 Kd: %f\n", spi_command.kd_3s[1]); |
adimmit | 5:f1703165ca7e | 403 | pc.printf("MOTOR 2-3 T_FF: %f\n", spi_command.tau_3s_ff[1]); |
adimmit | 2:32f69175c78e | 404 | |
adimmit | 5:f1703165ca7e | 405 | pc.printf("MOTOR 3-1 Q: %f\n", spi_command.q_des_1s[2]); |
adimmit | 5:f1703165ca7e | 406 | pc.printf("MOTOR 3-1 Qd: %f\n", spi_command.qd_des_1s[2]); |
adimmit | 5:f1703165ca7e | 407 | pc.printf("MOTOR 3-1 Kp: %f\n", spi_command.kp_1s[2]); |
adimmit | 5:f1703165ca7e | 408 | pc.printf("MOTOR 3-1 Kd: %f\n", spi_command.kd_1s[2]); |
adimmit | 5:f1703165ca7e | 409 | pc.printf("MOTOR 3-1 T_FF: %f\n", spi_command.tau_1s_ff[2]); |
adimmit | 2:32f69175c78e | 410 | |
adimmit | 5:f1703165ca7e | 411 | pc.printf("MOTOR 3-2 Q: %f\n", spi_command.q_des_2s[2]); |
adimmit | 5:f1703165ca7e | 412 | pc.printf("MOTOR 3-2 Qd: %f\n", spi_command.qd_des_2s[2]); |
adimmit | 5:f1703165ca7e | 413 | pc.printf("MOTOR 3-2 Kp: %f\n", spi_command.kp_2s[2]); |
adimmit | 5:f1703165ca7e | 414 | pc.printf("MOTOR 3-2 Kd: %f\n", spi_command.kd_2s[2]); |
adimmit | 5:f1703165ca7e | 415 | pc.printf("MOTOR 3-2 T_FF: %f\n", spi_command.tau_2s_ff[2]); |
adimmit | 2:32f69175c78e | 416 | |
adimmit | 5:f1703165ca7e | 417 | pc.printf("MOTOR 3-3 Q: %f\n", spi_command.q_des_3s[2]); |
adimmit | 5:f1703165ca7e | 418 | pc.printf("MOTOR 3-3 Qd: %f\n", spi_command.qd_des_3s[2]); |
adimmit | 5:f1703165ca7e | 419 | pc.printf("MOTOR 3-3 Kp: %f\n", spi_command.kp_3s[2]); |
adimmit | 5:f1703165ca7e | 420 | pc.printf("MOTOR 3-3 Kd: %f\n", spi_command.kd_3s[2]); |
adimmit | 5:f1703165ca7e | 421 | pc.printf("MOTOR 3-3 T_FF: %f\n", spi_command.tau_3s_ff[2]); |
adimmit | 2:32f69175c78e | 422 | |
adimmit | 2:32f69175c78e | 423 | } |
adimmit | 2:32f69175c78e | 424 | |
adimmit | 2:32f69175c78e | 425 | |
adimmit | 2:32f69175c78e | 426 | |
adimmit | 2:32f69175c78e | 427 | |
adimmit | 9:16a550d36425 | 428 | void print_SPI_data() { |
adimmit | 9:16a550d36425 | 429 | pc.printf("SPI MESSAGE SENT:\n"); |
adimmit | 9:16a550d36425 | 430 | //CAN ONE |
adimmit | 9:16a550d36425 | 431 | pc.printf("MOTOR 1-1 Q: %f\n", spi_data.q_1s[0]); |
adimmit | 9:16a550d36425 | 432 | pc.printf("MOTOR 1-1 Qd: %f\n", spi_data.qd_1s[0]); |
adimmit | 9:16a550d36425 | 433 | |
adimmit | 9:16a550d36425 | 434 | pc.printf("MOTOR 1-2 Q: %f\n", spi_data.q_2s[0]); |
adimmit | 9:16a550d36425 | 435 | pc.printf("MOTOR 1-2 Qd: %f\n", spi_data.qd_2s[0]); |
adimmit | 9:16a550d36425 | 436 | |
adimmit | 9:16a550d36425 | 437 | pc.printf("MOTOR 1-3 Q: %f\n", spi_data.q_3s[0]); |
adimmit | 9:16a550d36425 | 438 | pc.printf("MOTOR 1-3 Qd: %f\n", spi_data.qd_3s[0]); |
adimmit | 9:16a550d36425 | 439 | |
adimmit | 9:16a550d36425 | 440 | pc.printf("MOTOR 2-1 Q: %f\n", spi_data.q_1s[1]); |
adimmit | 9:16a550d36425 | 441 | pc.printf("MOTOR 2-1 Qd: %f\n", spi_data.qd_1s[1]); |
adimmit | 9:16a550d36425 | 442 | |
adimmit | 9:16a550d36425 | 443 | pc.printf("MOTOR 2-2 Q: %f\n", spi_data.q_2s[1]); |
adimmit | 9:16a550d36425 | 444 | pc.printf("MOTOR 2-2 Qd: %f\n", spi_data.qd_2s[1]); |
adimmit | 9:16a550d36425 | 445 | |
adimmit | 9:16a550d36425 | 446 | pc.printf("MOTOR 2-3 Q: %f\n", spi_data.q_3s[1]); |
adimmit | 9:16a550d36425 | 447 | pc.printf("MOTOR 2-3 Qd: %f\n", spi_data.qd_3s[1]); |
adimmit | 9:16a550d36425 | 448 | |
adimmit | 9:16a550d36425 | 449 | pc.printf("MOTOR 3-1 Q: %f\n", spi_data.q_1s[2]); |
adimmit | 9:16a550d36425 | 450 | pc.printf("MOTOR 3-1 Qd: %f\n", spi_data.qd_1s[2]); |
adimmit | 9:16a550d36425 | 451 | |
adimmit | 9:16a550d36425 | 452 | pc.printf("MOTOR 3-2 Q: %f\n", spi_data.q_2s[2]); |
adimmit | 9:16a550d36425 | 453 | pc.printf("MOTOR 3-2 Qd: %f\n", spi_data.qd_2s[2]); |
adimmit | 9:16a550d36425 | 454 | |
adimmit | 9:16a550d36425 | 455 | pc.printf("MOTOR 3-3 Q: %f\n", spi_data.q_3s[2]); |
adimmit | 9:16a550d36425 | 456 | pc.printf("MOTOR 3-3 Qd: %f\n", spi_data.qd_3s[2]); |
adimmit | 9:16a550d36425 | 457 | |
adimmit | 9:16a550d36425 | 458 | } |
adimmit | 9:16a550d36425 | 459 | |
adimmit | 9:16a550d36425 | 460 | |
adimmit | 9:16a550d36425 | 461 | |
adimmit | 12:5f97bcab6c50 | 462 | void process() |
adimmit | 12:5f97bcab6c50 | 463 | { |
adimmit | 12:5f97bcab6c50 | 464 | // update qs |
adimmit | 12:5f97bcab6c50 | 465 | spi_data.q_1s[0] = spi_command.q_des_1s[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 466 | spi_data.q_2s[0] = spi_command.q_des_2s[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 467 | spi_data.q_3s[0] = spi_command.q_des_3s[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 468 | spi_data.q_1s[1] = spi_command.q_des_1s[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 469 | spi_data.q_2s[1] = spi_command.q_des_2s[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 470 | spi_data.q_3s[1] = spi_command.q_des_3s[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 471 | spi_data.q_1s[2] = spi_command.q_des_1s[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 472 | spi_data.q_2s[2] = spi_command.q_des_2s[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 473 | spi_data.q_3s[2] = spi_command.q_des_3s[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 474 | // update qds |
adimmit | 12:5f97bcab6c50 | 475 | spi_data.qd_1s[0] = spi_command.qd_des_1s[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 476 | spi_data.qd_2s[0] = spi_command.qd_des_2s[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 477 | spi_data.qd_3s[0] = spi_command.qd_des_3s[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 478 | spi_data.qd_1s[1] = spi_command.qd_des_1s[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 479 | spi_data.qd_2s[1] = spi_command.qd_des_2s[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 480 | spi_data.qd_3s[1] = spi_command.qd_des_3s[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 481 | spi_data.qd_1s[2] = spi_command.qd_des_1s[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 482 | spi_data.qd_2s[2] = spi_command.qd_des_2s[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 483 | spi_data.qd_3s[2] = spi_command.qd_des_3s[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 484 | // update taus |
adimmit | 12:5f97bcab6c50 | 485 | spi_data.tau_1s[0] = spi_command.tau_1s_ff[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 486 | spi_data.tau_2s[0] = spi_command.tau_2s_ff[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 487 | spi_data.tau_3s[0] = spi_command.tau_3s_ff[0]+1.0; |
adimmit | 12:5f97bcab6c50 | 488 | spi_data.tau_1s[1] = spi_command.tau_1s_ff[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 489 | spi_data.tau_2s[1] = spi_command.tau_2s_ff[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 490 | spi_data.tau_3s[1] = spi_command.tau_3s_ff[1]+1.0; |
adimmit | 12:5f97bcab6c50 | 491 | spi_data.tau_1s[2] = spi_command.tau_1s_ff[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 492 | spi_data.tau_2s[2] = spi_command.tau_2s_ff[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 493 | spi_data.tau_3s[2] = spi_command.tau_3s_ff[2]+1.0; |
adimmit | 12:5f97bcab6c50 | 494 | // UDPATE FLAGS |
adimmit | 12:5f97bcab6c50 | 495 | spi_data.flags[0] = 0; |
adimmit | 12:5f97bcab6c50 | 496 | spi_data.flags[1] = 0; |
adimmit | 12:5f97bcab6c50 | 497 | spi_data.flags[2] = 0; |
adimmit | 12:5f97bcab6c50 | 498 | // UPDATE CHECKSUM |
adimmit | 12:5f97bcab6c50 | 499 | spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 30); //NOTE, CHECK THIS WE WANT TO DO IT ON THE FIRST 16 SPI BYTES |
adimmit | 12:5f97bcab6c50 | 500 | for(int i = 0; i < DATA_LEN; i++){ |
adimmit | 12:5f97bcab6c50 | 501 | tx_buff[i] = ((uint16_t*)(&spi_data))[i];} |
adimmit | 12:5f97bcab6c50 | 502 | } |
adimmit | 12:5f97bcab6c50 | 503 | |
adimmit | 9:16a550d36425 | 504 | |
adimmit | 2:32f69175c78e | 505 | |
adimmit | 0:76c761d3caf1 | 506 | void spi_isr(void) |
adimmit | 0:76c761d3caf1 | 507 | { |
adimmit | 1:aa253b5f5b65 | 508 | //pc.printf("CS ACTIVE\n"); |
adimmit | 0:76c761d3caf1 | 509 | GPIOC->ODR |= (1 << 8); |
adimmit | 0:76c761d3caf1 | 510 | GPIOC->ODR &= ~(1 << 8); |
adimmit | 0:76c761d3caf1 | 511 | int bytecount = 0; |
adimmit | 0:76c761d3caf1 | 512 | SPI1->DR = tx_buff[0]; |
adimmit | 0:76c761d3caf1 | 513 | while(cs == 0) { |
adimmit | 0:76c761d3caf1 | 514 | if(SPI1->SR&0x1) { |
adimmit | 1:aa253b5f5b65 | 515 | rx_buff[bytecount] = SPI1->DR; |
adimmit | 0:76c761d3caf1 | 516 | bytecount++; |
adimmit | 0:76c761d3caf1 | 517 | if(bytecount<TX_LEN) { |
adimmit | 0:76c761d3caf1 | 518 | SPI1->DR = tx_buff[bytecount]; |
adimmit | 0:76c761d3caf1 | 519 | } |
adimmit | 0:76c761d3caf1 | 520 | } |
adimmit | 0:76c761d3caf1 | 521 | } |
adimmit | 2:32f69175c78e | 522 | //pc.printf("RECIEVED: %d BYTES\n", bytecount); |
adimmit | 0:76c761d3caf1 | 523 | |
adimmit | 1:aa253b5f5b65 | 524 | //pc.printf("HERE'S A SPI COMMAND:\n"); |
adimmit | 0:76c761d3caf1 | 525 | // after reading, save into spi_command |
adimmit | 0:76c761d3caf1 | 526 | // should probably check checksum first! |
adimmit | 12:5f97bcab6c50 | 527 | uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,48); |
adimmit | 0:76c761d3caf1 | 528 | for(int i = 0; i < CMD_LEN; i++) |
adimmit | 0:76c761d3caf1 | 529 | { |
adimmit | 0:76c761d3caf1 | 530 | ((uint16_t*)(&spi_command))[i] = rx_buff[i]; |
adimmit | 9:16a550d36425 | 531 | //pc.printf("BYTE %d SENT: %d\n", i, tx_buff[i]); |
adimmit | 2:32f69175c78e | 532 | //pc.printf("WORD %d RECIEVED: %d\n", i, rx_buff[i]); |
adimmit | 0:76c761d3caf1 | 533 | } |
adimmit | 6:077e37d5c2da | 534 | //print_SPI_command(); |
adimmit | 0:76c761d3caf1 | 535 | |
adimmit | 0:76c761d3caf1 | 536 | // run control, which fills in tx_buff for the next iteration |
adimmit | 2:32f69175c78e | 537 | /* |
adimmit | 0:76c761d3caf1 | 538 | if(calc_checksum != spi_command.checksum){ |
adimmit | 1:aa253b5f5b65 | 539 | spi_data.flags[1] = 0xdead; |
adimmit | 2:32f69175c78e | 540 | pc.printf("FAILED CHECKSUM\n"); |
adimmit | 2:32f69175c78e | 541 | pc.printf("ACTUAL: %d\n", calc_checksum); |
adimmit | 2:32f69175c78e | 542 | pc.printf("CURRENT: %d\n", spi_command.checksum);} |
adimmit | 2:32f69175c78e | 543 | */ |
adimmit | 5:f1703165ca7e | 544 | |
adimmit | 9:16a550d36425 | 545 | //print_SPI_data(); |
adimmit | 12:5f97bcab6c50 | 546 | process(); |
adimmit | 12:5f97bcab6c50 | 547 | //control(); |
adimmit | 12:5f97bcab6c50 | 548 | //PackAll(); |
adimmit | 12:5f97bcab6c50 | 549 | //WriteAll(); |
adimmit | 0:76c761d3caf1 | 550 | |
adimmit | 0:76c761d3caf1 | 551 | |
adimmit | 0:76c761d3caf1 | 552 | //for (int i = 0; i<TX_LEN; i++) { |
adimmit | 0:76c761d3caf1 | 553 | // tx_buff[i] = 2*rx_buff[i]; |
adimmit | 0:76c761d3caf1 | 554 | //} |
adimmit | 0:76c761d3caf1 | 555 | // for (int i=0; i<TX_LEN; i++) { |
adimmit | 0:76c761d3caf1 | 556 | // //printf("%d ", rx_buff[i]); |
adimmit | 0:76c761d3caf1 | 557 | // } |
adimmit | 0:76c761d3caf1 | 558 | //printf("\n\r"); |
adimmit | 0:76c761d3caf1 | 559 | } |
adimmit | 0:76c761d3caf1 | 560 | |
adimmit | 0:76c761d3caf1 | 561 | int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){ |
adimmit | 9:16a550d36425 | 562 | /* |
adimmit | 0:76c761d3caf1 | 563 | if((state.p)>=limit_p){ |
adimmit | 0:76c761d3caf1 | 564 | //control->p_des = limit_p; |
adimmit | 0:76c761d3caf1 | 565 | control->v_des = 0.0f; |
adimmit | 0:76c761d3caf1 | 566 | control->kp = 0; |
adimmit | 0:76c761d3caf1 | 567 | control->kd = KD_SOFTSTOP; |
adimmit | 0:76c761d3caf1 | 568 | control->t_ff += KP_SOFTSTOP*(limit_p - state.p); |
adimmit | 0:76c761d3caf1 | 569 | return 1; |
adimmit | 0:76c761d3caf1 | 570 | } |
adimmit | 0:76c761d3caf1 | 571 | else if((state.p)<=limit_n){ |
adimmit | 0:76c761d3caf1 | 572 | //control->p_des = limit_n; |
adimmit | 0:76c761d3caf1 | 573 | control->v_des = 0.0f; |
adimmit | 0:76c761d3caf1 | 574 | control->kp = 0; |
adimmit | 0:76c761d3caf1 | 575 | control->kd = KD_SOFTSTOP; |
adimmit | 0:76c761d3caf1 | 576 | control->t_ff += KP_SOFTSTOP*(limit_n - state.p); |
adimmit | 0:76c761d3caf1 | 577 | return 1; |
adimmit | 0:76c761d3caf1 | 578 | } |
adimmit | 9:16a550d36425 | 579 | */ |
adimmit | 0:76c761d3caf1 | 580 | return 0; |
adimmit | 0:76c761d3caf1 | 581 | |
adimmit | 0:76c761d3caf1 | 582 | } |
adimmit | 0:76c761d3caf1 | 583 | |
adimmit | 0:76c761d3caf1 | 584 | |
adimmit | 0:76c761d3caf1 | 585 | void control() |
adimmit | 0:76c761d3caf1 | 586 | { |
adimmit | 0:76c761d3caf1 | 587 | |
adimmit | 0:76c761d3caf1 | 588 | if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){ |
adimmit | 0:76c761d3caf1 | 589 | enabled = 1; |
adimmit | 0:76c761d3caf1 | 590 | //BUS ONE |
adimmit | 0:76c761d3caf1 | 591 | EnterMotorMode(&q11_can); |
adimmit | 0:76c761d3caf1 | 592 | can1.write(q11_can); |
adimmit | 5:f1703165ca7e | 593 | EnterMotorMode(&q12_can); |
adimmit | 5:f1703165ca7e | 594 | can1.write(q12_can); |
adimmit | 5:f1703165ca7e | 595 | EnterMotorMode(&q13_can); |
adimmit | 5:f1703165ca7e | 596 | can1.write(q13_can); |
adimmit | 5:f1703165ca7e | 597 | //BUS TWO |
adimmit | 0:76c761d3caf1 | 598 | EnterMotorMode(&q21_can); |
adimmit | 5:f1703165ca7e | 599 | can2.write(q21_can); |
adimmit | 0:76c761d3caf1 | 600 | EnterMotorMode(&q22_can); |
adimmit | 0:76c761d3caf1 | 601 | can2.write(q22_can); |
adimmit | 5:f1703165ca7e | 602 | EnterMotorMode(&q23_can); |
adimmit | 5:f1703165ca7e | 603 | can2.write(q23_can); |
adimmit | 0:76c761d3caf1 | 604 | //BUS THREE |
adimmit | 5:f1703165ca7e | 605 | EnterMotorMode(&q31_can); |
adimmit | 5:f1703165ca7e | 606 | can3.write(q31_can); |
adimmit | 5:f1703165ca7e | 607 | EnterMotorMode(&q32_can); |
adimmit | 5:f1703165ca7e | 608 | can3.write(q32_can); |
adimmit | 0:76c761d3caf1 | 609 | EnterMotorMode(&q33_can); |
adimmit | 0:76c761d3caf1 | 610 | can3.write(q33_can); |
adimmit | 8:450fbccaf4f6 | 611 | //WRITE THE COMMANDS |
adimmit | 8:450fbccaf4f6 | 612 | //WriteAll(); |
adimmit | 5:f1703165ca7e | 613 | //SERIAL TO USER |
adimmit | 8:450fbccaf4f6 | 614 | //pc.printf("e\n\r"); |
adimmit | 0:76c761d3caf1 | 615 | return; |
adimmit | 0:76c761d3caf1 | 616 | } |
adimmit | 0:76c761d3caf1 | 617 | else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){ |
adimmit | 0:76c761d3caf1 | 618 | enabled = 0; |
adimmit | 0:76c761d3caf1 | 619 | //BUS ONE |
adimmit | 0:76c761d3caf1 | 620 | ExitMotorMode(&q11_can); |
adimmit | 0:76c761d3caf1 | 621 | can1.write(q11_can); |
adimmit | 5:f1703165ca7e | 622 | ExitMotorMode(&q12_can); |
adimmit | 5:f1703165ca7e | 623 | can1.write(q12_can); |
adimmit | 5:f1703165ca7e | 624 | ExitMotorMode(&q13_can); |
adimmit | 5:f1703165ca7e | 625 | can1.write(q13_can); |
adimmit | 5:f1703165ca7e | 626 | //BUS TWO |
adimmit | 0:76c761d3caf1 | 627 | ExitMotorMode(&q21_can); |
adimmit | 5:f1703165ca7e | 628 | can2.write(q21_can); |
adimmit | 0:76c761d3caf1 | 629 | ExitMotorMode(&q22_can); |
adimmit | 0:76c761d3caf1 | 630 | can2.write(q22_can); |
adimmit | 5:f1703165ca7e | 631 | ExitMotorMode(&q23_can); |
adimmit | 5:f1703165ca7e | 632 | can2.write(q23_can); |
adimmit | 0:76c761d3caf1 | 633 | //BUS THREE |
adimmit | 5:f1703165ca7e | 634 | ExitMotorMode(&q31_can); |
adimmit | 5:f1703165ca7e | 635 | can3.write(q31_can); |
adimmit | 5:f1703165ca7e | 636 | ExitMotorMode(&q32_can); |
adimmit | 5:f1703165ca7e | 637 | can3.write(q32_can); |
adimmit | 0:76c761d3caf1 | 638 | ExitMotorMode(&q33_can); |
adimmit | 0:76c761d3caf1 | 639 | can3.write(q33_can); |
adimmit | 8:450fbccaf4f6 | 640 | //WRITE THE COMMANDS |
adimmit | 8:450fbccaf4f6 | 641 | //WriteAll(); |
adimmit | 5:f1703165ca7e | 642 | //SERIAL TO USER |
adimmit | 8:450fbccaf4f6 | 643 | //pc.printf("x\n\r"); |
adimmit | 0:76c761d3caf1 | 644 | return; |
adimmit | 0:76c761d3caf1 | 645 | } |
adimmit | 0:76c761d3caf1 | 646 | |
adimmit | 5:f1703165ca7e | 647 | //BUS 1 DATA |
adimmit | 5:f1703165ca7e | 648 | spi_data.q_1s[0] = g1_state.a1.p; |
adimmit | 5:f1703165ca7e | 649 | spi_data.q_2s[0] = g1_state.a2.p; |
adimmit | 5:f1703165ca7e | 650 | spi_data.q_3s[0] = g1_state.a3.p; |
adimmit | 5:f1703165ca7e | 651 | spi_data.qd_1s[0] = g1_state.a1.v; |
adimmit | 5:f1703165ca7e | 652 | spi_data.qd_2s[0] = g1_state.a2.v; |
adimmit | 5:f1703165ca7e | 653 | spi_data.qd_3s[0] = g1_state.a3.v; |
adimmit | 10:42438194e82b | 654 | spi_data.tau_1s[0] = g1_state.a1.t; |
adimmit | 10:42438194e82b | 655 | spi_data.tau_2s[0] = g1_state.a2.t; |
adimmit | 10:42438194e82b | 656 | spi_data.tau_3s[0] = g1_state.a3.t; |
adimmit | 5:f1703165ca7e | 657 | //BUS 2 DATA |
adimmit | 9:16a550d36425 | 658 | spi_data.q_1s[1] = g2_state.a1.p; |
adimmit | 9:16a550d36425 | 659 | spi_data.q_2s[1] = g2_state.a2.p; |
adimmit | 9:16a550d36425 | 660 | spi_data.q_3s[1] = g2_state.a3.p; |
adimmit | 9:16a550d36425 | 661 | spi_data.qd_1s[1] = g2_state.a1.v; |
adimmit | 9:16a550d36425 | 662 | spi_data.qd_2s[1] = g2_state.a2.v; |
adimmit | 9:16a550d36425 | 663 | spi_data.qd_3s[1] = g2_state.a3.v; |
adimmit | 10:42438194e82b | 664 | spi_data.tau_1s[1] = g2_state.a1.t; |
adimmit | 10:42438194e82b | 665 | spi_data.tau_2s[1] = g2_state.a2.t; |
adimmit | 10:42438194e82b | 666 | spi_data.tau_3s[1] = g2_state.a3.t; |
adimmit | 5:f1703165ca7e | 667 | //BUS 3 DATA |
adimmit | 9:16a550d36425 | 668 | spi_data.q_1s[2] = g3_state.a1.p; |
adimmit | 9:16a550d36425 | 669 | spi_data.q_2s[2] = g3_state.a2.p; |
adimmit | 9:16a550d36425 | 670 | spi_data.q_3s[2] = g3_state.a3.p; |
adimmit | 9:16a550d36425 | 671 | spi_data.qd_1s[2] = g3_state.a1.v; |
adimmit | 9:16a550d36425 | 672 | spi_data.qd_2s[2] = g3_state.a2.v; |
adimmit | 9:16a550d36425 | 673 | spi_data.qd_3s[2] = g3_state.a3.v; |
adimmit | 10:42438194e82b | 674 | spi_data.tau_1s[2] = g3_state.a1.t; |
adimmit | 10:42438194e82b | 675 | spi_data.tau_2s[2] = g3_state.a2.t; |
adimmit | 10:42438194e82b | 676 | spi_data.tau_3s[2] = g3_state.a3.t; |
adimmit | 0:76c761d3caf1 | 677 | |
adimmit | 0:76c761d3caf1 | 678 | if(estop==0){ |
adimmit | 5:f1703165ca7e | 679 | printf("estopped!!!!\n\r"); |
adimmit | 6:077e37d5c2da | 680 | memset(&g1_control, 0, sizeof(g1_control)); |
adimmit | 6:077e37d5c2da | 681 | memset(&g2_control, 0, sizeof(g2_control)); |
adimmit | 6:077e37d5c2da | 682 | memset(&g3_control, 0, sizeof(g3_control)); |
adimmit | 0:76c761d3caf1 | 683 | spi_data.flags[0] = 0xdead; |
adimmit | 0:76c761d3caf1 | 684 | spi_data.flags[1] = 0xdead; |
adimmit | 0:76c761d3caf1 | 685 | spi_data.flags[2] = 0xdead; |
adimmit | 0:76c761d3caf1 | 686 | led = 1; |
adimmit | 0:76c761d3caf1 | 687 | } |
adimmit | 0:76c761d3caf1 | 688 | |
adimmit | 0:76c761d3caf1 | 689 | else{ |
adimmit | 0:76c761d3caf1 | 690 | led = 0; |
adimmit | 6:077e37d5c2da | 691 | memset(&g1_control, 0, sizeof(g1_control)); |
adimmit | 6:077e37d5c2da | 692 | memset(&g2_control, 0, sizeof(g2_control)); |
adimmit | 6:077e37d5c2da | 693 | memset(&g3_control, 0, sizeof(g3_control)); |
adimmit | 0:76c761d3caf1 | 694 | |
adimmit | 5:f1703165ca7e | 695 | //TRANSLATE SPI TO ACTUATOR COMMANNDS |
adimmit | 5:f1703165ca7e | 696 | //CAN1 |
adimmit | 5:f1703165ca7e | 697 | //CAN1 MOTOR1 |
adimmit | 5:f1703165ca7e | 698 | g1_control.a1.p_des = spi_command.q_des_1s[0]; |
adimmit | 5:f1703165ca7e | 699 | g1_control.a1.v_des = spi_command.qd_des_1s[0]; |
adimmit | 5:f1703165ca7e | 700 | g1_control.a1.kp = spi_command.kp_1s[0]; |
adimmit | 5:f1703165ca7e | 701 | g1_control.a1.kd = spi_command.kd_1s[0]; |
adimmit | 5:f1703165ca7e | 702 | g1_control.a1.t_ff = spi_command.tau_1s_ff[0]; |
adimmit | 5:f1703165ca7e | 703 | //CAN1 MOTOR 2 |
adimmit | 5:f1703165ca7e | 704 | g1_control.a2.p_des = spi_command.q_des_2s[0]; |
adimmit | 5:f1703165ca7e | 705 | g1_control.a2.v_des = spi_command.qd_des_2s[0]; |
adimmit | 5:f1703165ca7e | 706 | g1_control.a2.kp = spi_command.kp_2s[0]; |
adimmit | 5:f1703165ca7e | 707 | g1_control.a2.kd = spi_command.kd_2s[0]; |
adimmit | 5:f1703165ca7e | 708 | g1_control.a2.t_ff = spi_command.tau_2s_ff[0]; |
adimmit | 5:f1703165ca7e | 709 | //CAN1 MOTOR 3 |
adimmit | 5:f1703165ca7e | 710 | g1_control.a3.p_des = spi_command.q_des_3s[0]; |
adimmit | 5:f1703165ca7e | 711 | g1_control.a3.v_des = spi_command.qd_des_3s[0]; |
adimmit | 5:f1703165ca7e | 712 | g1_control.a3.kp = spi_command.kp_3s[0]; |
adimmit | 5:f1703165ca7e | 713 | g1_control.a3.kd = spi_command.kd_3s[0]; |
adimmit | 5:f1703165ca7e | 714 | g1_control.a3.t_ff = spi_command.tau_3s_ff[0]; |
adimmit | 5:f1703165ca7e | 715 | //CAN2 |
adimmit | 5:f1703165ca7e | 716 | //CAN2 MOTOR1 |
adimmit | 5:f1703165ca7e | 717 | g2_control.a1.p_des = spi_command.q_des_1s[1]; |
adimmit | 5:f1703165ca7e | 718 | g2_control.a1.v_des = spi_command.qd_des_1s[1]; |
adimmit | 5:f1703165ca7e | 719 | g2_control.a1.kp = spi_command.kp_1s[1]; |
adimmit | 5:f1703165ca7e | 720 | g2_control.a1.kd = spi_command.kd_1s[1]; |
adimmit | 5:f1703165ca7e | 721 | g2_control.a1.t_ff = spi_command.tau_1s_ff[1]; |
adimmit | 5:f1703165ca7e | 722 | //CAN2 MOTOR 2 |
adimmit | 5:f1703165ca7e | 723 | g2_control.a2.p_des = spi_command.q_des_2s[1]; |
adimmit | 5:f1703165ca7e | 724 | g2_control.a2.v_des = spi_command.qd_des_2s[1]; |
adimmit | 5:f1703165ca7e | 725 | g2_control.a2.kp = spi_command.kp_2s[1]; |
adimmit | 5:f1703165ca7e | 726 | g2_control.a2.kd = spi_command.kd_2s[1]; |
adimmit | 5:f1703165ca7e | 727 | g2_control.a2.t_ff = spi_command.tau_2s_ff[1]; |
adimmit | 5:f1703165ca7e | 728 | //CAN2 MOTOR 3 |
adimmit | 5:f1703165ca7e | 729 | g2_control.a3.p_des = spi_command.q_des_3s[1]; |
adimmit | 5:f1703165ca7e | 730 | g2_control.a3.v_des = spi_command.qd_des_3s[1]; |
adimmit | 5:f1703165ca7e | 731 | g2_control.a3.kp = spi_command.kp_3s[1]; |
adimmit | 5:f1703165ca7e | 732 | g2_control.a3.kd = spi_command.kd_3s[1]; |
adimmit | 5:f1703165ca7e | 733 | g2_control.a3.t_ff = spi_command.tau_3s_ff[1]; |
adimmit | 5:f1703165ca7e | 734 | //CAN3 |
adimmit | 5:f1703165ca7e | 735 | //CAN3 MOTOR1 |
adimmit | 5:f1703165ca7e | 736 | g3_control.a1.p_des = spi_command.q_des_1s[2]; |
adimmit | 5:f1703165ca7e | 737 | g3_control.a1.v_des = spi_command.qd_des_1s[2]; |
adimmit | 5:f1703165ca7e | 738 | g3_control.a1.kp = spi_command.kp_1s[2]; |
adimmit | 5:f1703165ca7e | 739 | g3_control.a1.kd = spi_command.kd_1s[2]; |
adimmit | 5:f1703165ca7e | 740 | g3_control.a1.t_ff = spi_command.tau_1s_ff[2]; |
adimmit | 5:f1703165ca7e | 741 | //CAN3 MOTOR 2 |
adimmit | 5:f1703165ca7e | 742 | g3_control.a2.p_des = spi_command.q_des_2s[2]; |
adimmit | 5:f1703165ca7e | 743 | g3_control.a2.v_des = spi_command.qd_des_2s[2]; |
adimmit | 5:f1703165ca7e | 744 | g3_control.a2.kp = spi_command.kp_2s[2]; |
adimmit | 5:f1703165ca7e | 745 | g3_control.a2.kd = spi_command.kd_2s[2]; |
adimmit | 5:f1703165ca7e | 746 | g3_control.a2.t_ff = spi_command.tau_2s_ff[2]; |
adimmit | 5:f1703165ca7e | 747 | //CAN3 MOTOR 3 |
adimmit | 5:f1703165ca7e | 748 | g3_control.a3.p_des = spi_command.q_des_3s[2]; |
adimmit | 5:f1703165ca7e | 749 | g3_control.a3.v_des = spi_command.qd_des_3s[2]; |
adimmit | 5:f1703165ca7e | 750 | g3_control.a3.kp = spi_command.kp_3s[2]; |
adimmit | 5:f1703165ca7e | 751 | g3_control.a3.kd = spi_command.kd_3s[2]; |
adimmit | 5:f1703165ca7e | 752 | g3_control.a3.t_ff = spi_command.tau_3s_ff[2]; |
adimmit | 0:76c761d3caf1 | 753 | |
adimmit | 7:2f785cb39f11 | 754 | //SPI FLAGS RETURN //IMPLEMENTS THE JOINT SOFT STOP RIGHT HERE |
adimmit | 0:76c761d3caf1 | 755 | spi_data.flags[0] = 0; |
adimmit | 0:76c761d3caf1 | 756 | spi_data.flags[1] = 0; |
adimmit | 0:76c761d3caf1 | 757 | spi_data.flags[2] = 0; |
adimmit | 9:16a550d36425 | 758 | //spi_data.flags[0] |= softstop_joint(g1_state.a1, &g1_control.a1, A1_LIM_P, A1_LIM_N); |
adimmit | 9:16a550d36425 | 759 | //spi_data.flags[0] |= (softstop_joint(g1_state.a2, &g1_control.a2, A2_LIM_P, A2_LIM_N))<<1; |
adimmit | 9:16a550d36425 | 760 | //spi_data.flags[0] |= (softstop_joint(g1_state.a3, &g1_control.a3, A3_LIM_P, A3_LIM_N))<<2; |
adimmit | 9:16a550d36425 | 761 | //spi_data.flags[1] |= softstop_joint(g2_state.a1, &g2_control.a1, A1_LIM_P, A1_LIM_N); |
adimmit | 9:16a550d36425 | 762 | //spi_data.flags[1] |= (softstop_joint(g2_state.a2, &g2_control.a2, A2_LIM_P, A2_LIM_N))<<1; |
adimmit | 9:16a550d36425 | 763 | //spi_data.flags[1] |= (softstop_joint(g2_state.a3, &g2_control.a3, A3_LIM_P, A3_LIM_N))<<2; |
adimmit | 9:16a550d36425 | 764 | //spi_data.flags[2] |= softstop_joint(g3_state.a1, &g3_control.a1, A1_LIM_P, A1_LIM_N); |
adimmit | 9:16a550d36425 | 765 | //spi_data.flags[2] |= (softstop_joint(g3_state.a2, &g3_control.a2, A2_LIM_P, A2_LIM_N))<<1; |
adimmit | 9:16a550d36425 | 766 | //spi_data.flags[2] |= (softstop_joint(g3_state.a3, &g3_control.a3, A3_LIM_P, A3_LIM_N))<<2; |
adimmit | 0:76c761d3caf1 | 767 | |
adimmit | 0:76c761d3caf1 | 768 | //spi_data.flags[0] = 0xbeef; |
adimmit | 0:76c761d3caf1 | 769 | //spi_data.flags[1] = 0xbeef; |
adimmit | 0:76c761d3caf1 | 770 | //PackAll(); |
adimmit | 0:76c761d3caf1 | 771 | //WriteAll(); |
adimmit | 0:76c761d3caf1 | 772 | } |
adimmit | 11:40d5d9b5d2ba | 773 | spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14); //OK THIS LINE ISN'T SETTING THE CHECKSUM B/C SETTINNG IT TO 10 DID NOTHIGN |
adimmit | 0:76c761d3caf1 | 774 | for(int i = 0; i < DATA_LEN; i++){ |
adimmit | 0:76c761d3caf1 | 775 | tx_buff[i] = ((uint16_t*)(&spi_data))[i];} |
adimmit | 0:76c761d3caf1 | 776 | |
adimmit | 0:76c761d3caf1 | 777 | } |
adimmit | 0:76c761d3caf1 | 778 | |
adimmit | 0:76c761d3caf1 | 779 | |
adimmit | 0:76c761d3caf1 | 780 | void test_control() |
adimmit | 0:76c761d3caf1 | 781 | { |
adimmit | 0:76c761d3caf1 | 782 | for(int i = 0; i < 3; i++) |
adimmit | 0:76c761d3caf1 | 783 | { |
adimmit | 0:76c761d3caf1 | 784 | spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f; |
adimmit | 0:76c761d3caf1 | 785 | spi_data.q_2s[i] = spi_command.q_des_2s[i] + 1.f; |
adimmit | 0:76c761d3caf1 | 786 | spi_data.q_3s[i] = spi_command.q_des_3s[i] + 1.f; |
adimmit | 0:76c761d3caf1 | 787 | |
adimmit | 0:76c761d3caf1 | 788 | spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f; |
adimmit | 0:76c761d3caf1 | 789 | spi_data.qd_2s[i] = spi_command.qd_des_2s[i] + 1.f; |
adimmit | 0:76c761d3caf1 | 790 | spi_data.qd_3s[i] = spi_command.qd_des_3s[i] + 1.f; |
adimmit | 0:76c761d3caf1 | 791 | } |
adimmit | 0:76c761d3caf1 | 792 | |
adimmit | 0:76c761d3caf1 | 793 | spi_data.flags[0] = 0xdead; |
adimmit | 0:76c761d3caf1 | 794 | //spi_data.flags[1] = 0xbeef; |
adimmit | 0:76c761d3caf1 | 795 | |
adimmit | 0:76c761d3caf1 | 796 | // only do first 56 bytes of message. |
adimmit | 0:76c761d3caf1 | 797 | spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14); |
adimmit | 0:76c761d3caf1 | 798 | |
adimmit | 0:76c761d3caf1 | 799 | for(int i = 0; i < DATA_LEN; i++) |
adimmit | 0:76c761d3caf1 | 800 | tx_buff[i] = ((uint16_t*)(&spi_data))[i]; |
adimmit | 0:76c761d3caf1 | 801 | } |
adimmit | 0:76c761d3caf1 | 802 | |
adimmit | 0:76c761d3caf1 | 803 | void init_spi(void){ |
adimmit | 0:76c761d3caf1 | 804 | SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4); |
adimmit | 0:76c761d3caf1 | 805 | spi->format(16, 0); |
adimmit | 5:f1703165ca7e | 806 | spi->frequency(6000000); |
adimmit | 0:76c761d3caf1 | 807 | spi->reply(0x0); |
adimmit | 0:76c761d3caf1 | 808 | cs.fall(&spi_isr); |
adimmit | 1:aa253b5f5b65 | 809 | pc.printf("done\n\r"); |
adimmit | 0:76c761d3caf1 | 810 | } |
adimmit | 0:76c761d3caf1 | 811 | |
adimmit | 0:76c761d3caf1 | 812 | int main() { |
adimmit | 0:76c761d3caf1 | 813 | wait(1); |
adimmit | 0:76c761d3caf1 | 814 | //led = 1; |
adimmit | 1:aa253b5f5b65 | 815 | pc.baud(115200); //MAYBE CHANGE THIS IF NEEDED |
adimmit | 0:76c761d3caf1 | 816 | pc.attach(&serial_isr); |
adimmit | 0:76c761d3caf1 | 817 | estop.mode(PullUp); |
adimmit | 0:76c761d3caf1 | 818 | //spi.format(16, 0); |
adimmit | 0:76c761d3caf1 | 819 | //spi.frequency(1000000); |
adimmit | 0:76c761d3caf1 | 820 | //spi.reply(0x0); |
adimmit | 0:76c761d3caf1 | 821 | //cs.fall(&spi_isr); |
adimmit | 0:76c761d3caf1 | 822 | |
adimmit | 0:76c761d3caf1 | 823 | //can1.frequency(1000000); // set bit rate to 1Mbps |
adimmit | 0:76c761d3caf1 | 824 | //can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler |
adimmit | 0:76c761d3caf1 | 825 | can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter |
adimmit | 0:76c761d3caf1 | 826 | //can2.frequency(1000000); // set bit rate to 1Mbps |
adimmit | 0:76c761d3caf1 | 827 | //can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler |
adimmit | 0:76c761d3caf1 | 828 | can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter |
adimmit | 0:76c761d3caf1 | 829 | can3.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter |
adimmit | 0:76c761d3caf1 | 830 | |
adimmit | 0:76c761d3caf1 | 831 | memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t)); |
adimmit | 0:76c761d3caf1 | 832 | memset(&spi_data, 0, sizeof(spi_data_t)); |
adimmit | 0:76c761d3caf1 | 833 | memset(&spi_command,0,sizeof(spi_command_t)); |
adimmit | 0:76c761d3caf1 | 834 | |
adimmit | 0:76c761d3caf1 | 835 | |
adimmit | 0:76c761d3caf1 | 836 | NVIC_SetPriority(TIM5_IRQn, 1); |
adimmit | 0:76c761d3caf1 | 837 | //NVIC_SetPriority(CAN1_RX0_IRQn, 3); |
adimmit | 0:76c761d3caf1 | 838 | //NVIC_SetPriority(CAN2_RX0_IRQn, 3); |
adimmit | 0:76c761d3caf1 | 839 | |
adimmit | 0:76c761d3caf1 | 840 | pc.printf("\n\r SPIne\n\r"); |
adimmit | 0:76c761d3caf1 | 841 | //printf("%d\n\r", RX_ID << 18); |
adimmit | 0:76c761d3caf1 | 842 | |
adimmit | 5:f1703165ca7e | 843 | //CAN 1 BUS |
adimmit | 0:76c761d3caf1 | 844 | q11_can.len = 8; //transmit 8 bytes |
adimmit | 5:f1703165ca7e | 845 | q12_can.len = 8; //transmit 8 bytes |
adimmit | 5:f1703165ca7e | 846 | q13_can.len = 8; |
adimmit | 5:f1703165ca7e | 847 | //CAN 2 BUS |
adimmit | 0:76c761d3caf1 | 848 | q21_can.len = 8; //transmit 8 bytes |
adimmit | 0:76c761d3caf1 | 849 | q22_can.len = 8; //transmit 8 bytes |
adimmit | 5:f1703165ca7e | 850 | q23_can.len = 8; |
adimmit | 5:f1703165ca7e | 851 | //CAN 3 BUS |
adimmit | 5:f1703165ca7e | 852 | q31_can.len = 8; //transmit 8 bytes |
adimmit | 5:f1703165ca7e | 853 | q32_can.len = 8; //transmit 8 bytes |
adimmit | 0:76c761d3caf1 | 854 | q33_can.len = 8; |
adimmit | 5:f1703165ca7e | 855 | //RECIEVE |
adimmit | 0:76c761d3caf1 | 856 | rxMsg1.len = 6; //receive 6 bytes |
adimmit | 0:76c761d3caf1 | 857 | rxMsg2.len = 6; |
adimmit | 0:76c761d3caf1 | 858 | rxMsg3.len = 6; |
adimmit | 5:f1703165ca7e | 859 | |
adimmit | 5:f1703165ca7e | 860 | //CAN 1 BUS |
adimmit | 0:76c761d3caf1 | 861 | q11_can.id = 0x1; |
adimmit | 5:f1703165ca7e | 862 | q12_can.id = 0x2; |
adimmit | 5:f1703165ca7e | 863 | q13_can.id = 0x3; |
adimmit | 5:f1703165ca7e | 864 | //CAN 2 BUS |
adimmit | 5:f1703165ca7e | 865 | q21_can.id = 0x1; |
adimmit | 0:76c761d3caf1 | 866 | q22_can.id = 0x2; |
adimmit | 5:f1703165ca7e | 867 | q23_can.id = 0x3; |
adimmit | 5:f1703165ca7e | 868 | //CAN 3 BUS |
adimmit | 5:f1703165ca7e | 869 | q31_can.id = 0x1; |
adimmit | 5:f1703165ca7e | 870 | q32_can.id = 0x2; |
adimmit | 5:f1703165ca7e | 871 | q33_can.id = 0x3; |
adimmit | 0:76c761d3caf1 | 872 | |
adimmit | 5:f1703165ca7e | 873 | //actuators on the CAN1 bus |
adimmit | 6:077e37d5c2da | 874 | pack_cmd(&q11_can, g1_control.a1); |
adimmit | 6:077e37d5c2da | 875 | pack_cmd(&q12_can, g1_control.a2); |
adimmit | 6:077e37d5c2da | 876 | pack_cmd(&q13_can, g1_control.a3); |
adimmit | 5:f1703165ca7e | 877 | //actuators on the CAN2 bus |
adimmit | 6:077e37d5c2da | 878 | pack_cmd(&q21_can, g2_control.a1); |
adimmit | 6:077e37d5c2da | 879 | pack_cmd(&q22_can, g2_control.a2); |
adimmit | 6:077e37d5c2da | 880 | pack_cmd(&q23_can, g2_control.a3); |
adimmit | 5:f1703165ca7e | 881 | //actuators on the CAN3 bus |
adimmit | 6:077e37d5c2da | 882 | pack_cmd(&q31_can, g3_control.a1); |
adimmit | 6:077e37d5c2da | 883 | pack_cmd(&q32_can, g3_control.a2); |
adimmit | 6:077e37d5c2da | 884 | pack_cmd(&q33_can, g3_control.a3); |
adimmit | 5:f1703165ca7e | 885 | //WRITE THE INITIAL COMMAND |
adimmit | 0:76c761d3caf1 | 886 | WriteAll(); |
adimmit | 1:aa253b5f5b65 | 887 | |
adimmit | 1:aa253b5f5b65 | 888 | //just debugging things |
adimmit | 1:aa253b5f5b65 | 889 | pc.printf("SETUP VARS ALL DONE\n"); |
adimmit | 0:76c761d3caf1 | 890 | |
adimmit | 0:76c761d3caf1 | 891 | |
adimmit | 0:76c761d3caf1 | 892 | // SPI doesn't work if enabled while the CS pin is pulled low |
adimmit | 0:76c761d3caf1 | 893 | // Wait for CS to not be low, then enable SPI |
adimmit | 0:76c761d3caf1 | 894 | if(!spi_enabled){ |
adimmit | 1:aa253b5f5b65 | 895 | while((spi_enabled==0) && (cs.read() ==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);} |
adimmit | 0:76c761d3caf1 | 896 | init_spi(); |
adimmit | 0:76c761d3caf1 | 897 | spi_enabled = 1; |
adimmit | 1:aa253b5f5b65 | 898 | pc.printf("SPI ENABLED AND READY\n"); |
adimmit | 0:76c761d3caf1 | 899 | } |
adimmit | 0:76c761d3caf1 | 900 | |
adimmit | 0:76c761d3caf1 | 901 | //spi_command=set the thing here... |
adimmit | 0:76c761d3caf1 | 902 | |
adimmit | 0:76c761d3caf1 | 903 | while(1) { |
adimmit | 1:aa253b5f5b65 | 904 | //pc.printf("test, of SPINE\r\n"); |
adimmit | 0:76c761d3caf1 | 905 | counter++; |
adimmit | 12:5f97bcab6c50 | 906 | //can2.read(rxMsg2); |
adimmit | 12:5f97bcab6c50 | 907 | //unpack_reply(rxMsg2, &g2_state); |
adimmit | 12:5f97bcab6c50 | 908 | //can1.read(rxMsg1); // read message into Rx message storage |
adimmit | 12:5f97bcab6c50 | 909 | //unpack_reply(rxMsg1, &g1_state); |
adimmit | 12:5f97bcab6c50 | 910 | //can3.read(rxMsg3); // read message into Rx message storage |
adimmit | 12:5f97bcab6c50 | 911 | //unpack_reply(rxMsg3, &g3_state); |
adimmit | 0:76c761d3caf1 | 912 | wait_us(10); |
adimmit | 1:aa253b5f5b65 | 913 | |
adimmit | 1:aa253b5f5b65 | 914 | //print heatbeat (always will print message 0) |
adimmit | 2:32f69175c78e | 915 | /* |
adimmit | 1:aa253b5f5b65 | 916 | if (counter3 == 100000) { //for debugging purposes |
adimmit | 1:aa253b5f5b65 | 917 | pc.printf("HEARTBEAT\n"); |
adimmit | 1:aa253b5f5b65 | 918 | counter3 = 0; |
adimmit | 1:aa253b5f5b65 | 919 | } |
adimmit | 1:aa253b5f5b65 | 920 | counter3++; |
adimmit | 2:32f69175c78e | 921 | */ |
adimmit | 0:76c761d3caf1 | 922 | } |
adimmit | 2:32f69175c78e | 923 | |
adimmit | 0:76c761d3caf1 | 924 | } |