Aditya Mehrotra / SPINE_FIDDY_TEST

test code for SPI communications with any board

Dependencies:   mbed-dev

main.cpp@14:8a7fd1ebc209, 2021-08-30 (annotated)

Committer:
adimmit
Date:
Mon Aug 30 15:47:18 2021 +0000
Revision:
14:8a7fd1ebc209
Parent:
13:ad3ca70bf929
Child:
15:d3c93f01a02b
updated torque limits to the correct ones

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