Aditya Mehrotra / SPIne_Plus_v2

Spine 18 motor board for Biomimetic Robotics Lab

Dependencies:   mbed-dev

main.cpp@15:56a06363a698, 2022-09-28 (annotated)

Committer:
adimmit
Date:
Wed Sep 28 16:32:46 2022 +0000
Revision:
15:56a06363a698
Parent:
14:6fec697f62ef 
updated to work with checksum and with the 2-motor dyno;

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 15:56a06363a698 13 #define RX_LEN 52 //CHECK THESE BUFFER LENGHTS
adimmit 15:56a06363a698 14 #define TX_LEN 52 //CHECK THESE BUFFER LENGHTS
adimmit 0:76c761d3caf1 15
adimmit 0:76c761d3caf1 16 // length of outgoing/incoming messages
adimmit 15:56a06363a698 17 #define DATA_LEN 36 //CHECK THESE BUFFER LENGHTS
adimmit 15:56a06363a698 18 #define CMD_LEN 52 //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 13:87e7ce883f6a 51 spi_command_t tmp_crc_chk;
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 15:56a06363a698 67 CANMessage q11_can, q21_can; //q12_can, q13_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 15:56a06363a698 80 grouped_act_state g1_state, g2_state; // g3_state;
adimmit 15:56a06363a698 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 0:76c761d3caf1 167 }
adimmit 0:76c761d3caf1 168
adimmit 0:76c761d3caf1 169 void rxISR1() {
adimmit 0:76c761d3caf1 170 can1.read(rxMsg1); // read message into Rx message storage
adimmit 0:76c761d3caf1 171 unpack_reply(rxMsg1, &g1_state);
adimmit 0:76c761d3caf1 172 }
adimmit 0:76c761d3caf1 173 void rxISR2(){
adimmit 0:76c761d3caf1 174 can2.read(rxMsg2);
adimmit 0:76c761d3caf1 175 unpack_reply(rxMsg2, &g2_state);
adimmit 0:76c761d3caf1 176 }
adimmit 0:76c761d3caf1 177
adimmit 0:76c761d3caf1 178 void PackAll(){
adimmit 5:f1703165ca7e 179 //actuators on the CAN1 bus
adimmit 6:077e37d5c2da 180 pack_cmd(&q11_can, g1_control.a1);
adimmit 5:f1703165ca7e 181 //actuators on the CAN2 bus
adimmit 6:077e37d5c2da 182 pack_cmd(&q21_can, g2_control.a1);
adimmit 0:76c761d3caf1 183 }
adimmit 0:76c761d3caf1 184 void WriteAll(){
adimmit 0:76c761d3caf1 185 //toggle = 1;
adimmit 5:f1703165ca7e 186 //ID = 1 actuators
adimmit 0:76c761d3caf1 187 can1.write(q11_can);
adimmit 0:76c761d3caf1 188 wait(.00002);
adimmit 5:f1703165ca7e 189 can2.write(q21_can);
adimmit 0:76c761d3caf1 190 wait(.00002);
adimmit 0:76c761d3caf1 191 }
adimmit 0:76c761d3caf1 192
adimmit 0:76c761d3caf1 193 void sendCMD(){
adimmit 0:76c761d3caf1 194 counter ++;
adimmit 0:76c761d3caf1 195
adimmit 0:76c761d3caf1 196 PackAll();
adimmit 0:76c761d3caf1 197
adimmit 0:76c761d3caf1 198 if(counter>100){
adimmit 15:56a06363a698 199 pc.printf("%.3f %.3f\n\r", g1_state.a1.p, g2_state.a1.p);
adimmit 0:76c761d3caf1 200 counter = 0 ;
adimmit 0:76c761d3caf1 201 }
adimmit 0:76c761d3caf1 202
adimmit 0:76c761d3caf1 203 WriteAll();
adimmit 0:76c761d3caf1 204 }
adimmit 0:76c761d3caf1 205
adimmit 0:76c761d3caf1 206
adimmit 0:76c761d3caf1 207
adimmit 0:76c761d3caf1 208
adimmit 0:76c761d3caf1 209 void Zero(CANMessage * msg){
adimmit 0:76c761d3caf1 210 msg->data[0] = 0xFF;
adimmit 0:76c761d3caf1 211 msg->data[1] = 0xFF;
adimmit 0:76c761d3caf1 212 msg->data[2] = 0xFF;
adimmit 0:76c761d3caf1 213 msg->data[3] = 0xFF;
adimmit 0:76c761d3caf1 214 msg->data[4] = 0xFF;
adimmit 0:76c761d3caf1 215 msg->data[5] = 0xFF;
adimmit 0:76c761d3caf1 216 msg->data[6] = 0xFF;
adimmit 0:76c761d3caf1 217 msg->data[7] = 0xFE;
adimmit 8:450fbccaf4f6 218 //WriteAll();
adimmit 0:76c761d3caf1 219 }
adimmit 0:76c761d3caf1 220
adimmit 0:76c761d3caf1 221 void EnterMotorMode(CANMessage * msg){
adimmit 0:76c761d3caf1 222 msg->data[0] = 0xFF;
adimmit 0:76c761d3caf1 223 msg->data[1] = 0xFF;
adimmit 0:76c761d3caf1 224 msg->data[2] = 0xFF;
adimmit 0:76c761d3caf1 225 msg->data[3] = 0xFF;
adimmit 0:76c761d3caf1 226 msg->data[4] = 0xFF;
adimmit 0:76c761d3caf1 227 msg->data[5] = 0xFF;
adimmit 0:76c761d3caf1 228 msg->data[6] = 0xFF;
adimmit 0:76c761d3caf1 229 msg->data[7] = 0xFC;
adimmit 8:450fbccaf4f6 230 //WriteAll();
adimmit 0:76c761d3caf1 231 }
adimmit 0:76c761d3caf1 232
adimmit 0:76c761d3caf1 233 void ExitMotorMode(CANMessage * msg){
adimmit 0:76c761d3caf1 234 msg->data[0] = 0xFF;
adimmit 0:76c761d3caf1 235 msg->data[1] = 0xFF;
adimmit 0:76c761d3caf1 236 msg->data[2] = 0xFF;
adimmit 0:76c761d3caf1 237 msg->data[3] = 0xFF;
adimmit 0:76c761d3caf1 238 msg->data[4] = 0xFF;
adimmit 0:76c761d3caf1 239 msg->data[5] = 0xFF;
adimmit 0:76c761d3caf1 240 msg->data[6] = 0xFF;
adimmit 0:76c761d3caf1 241 msg->data[7] = 0xFD;
adimmit 8:450fbccaf4f6 242 //WriteAll();
adimmit 0:76c761d3caf1 243 }
adimmit 5:f1703165ca7e 244
adimmit 8:450fbccaf4f6 245
adimmit 0:76c761d3caf1 246 void serial_isr(){
adimmit 0:76c761d3caf1 247 /// handle keyboard commands from the serial terminal ///
adimmit 0:76c761d3caf1 248 while(pc.readable()){
adimmit 0:76c761d3caf1 249 char c = pc.getc();
adimmit 0:76c761d3caf1 250 //led = !led;
adimmit 0:76c761d3caf1 251 switch(c){
adimmit 0:76c761d3caf1 252 case(27):
adimmit 0:76c761d3caf1 253 //loop.detach();
adimmit 1:aa253b5f5b65 254 pc.printf("\n\r exiting motor mode \n\r");
adimmit 5:f1703165ca7e 255 //CAN BUS 1
adimmit 0:76c761d3caf1 256 ExitMotorMode(&q11_can);
adimmit 5:f1703165ca7e 257 //CAN BUS 2
adimmit 0:76c761d3caf1 258 ExitMotorMode(&q21_can);
adimmit 5:f1703165ca7e 259 //DISABLE FLAG
adimmit 0:76c761d3caf1 260 enabled = 0;
adimmit 0:76c761d3caf1 261 break;
adimmit 0:76c761d3caf1 262 case('m'):
adimmit 1:aa253b5f5b65 263 pc.printf("\n\r entering motor mode \n\r");
adimmit 5:f1703165ca7e 264 //CAN BUS 1
adimmit 0:76c761d3caf1 265 EnterMotorMode(&q11_can);
adimmit 5:f1703165ca7e 266 //CAN BUS 2
adimmit 0:76c761d3caf1 267 EnterMotorMode(&q21_can);
adimmit 5:f1703165ca7e 268 //WAIT FOR ENABLE
adimmit 0:76c761d3caf1 269 wait(.5);
adimmit 5:f1703165ca7e 270 //ENABLE FLAG
adimmit 0:76c761d3caf1 271 enabled = 1;
adimmit 0:76c761d3caf1 272 //loop.attach(&sendCMD, .001);
adimmit 0:76c761d3caf1 273 break;
adimmit 0:76c761d3caf1 274 case('s'):
adimmit 1:aa253b5f5b65 275 pc.printf("\n\r standing \n\r");
adimmit 0:76c761d3caf1 276 counter2 = 0;
adimmit 0:76c761d3caf1 277 is_standing = 1;
adimmit 0:76c761d3caf1 278 //stand();
adimmit 0:76c761d3caf1 279 break;
adimmit 0:76c761d3caf1 280 case('z'):
adimmit 1:aa253b5f5b65 281 pc.printf("\n\r zeroing \n\r");
adimmit 5:f1703165ca7e 282 //CAN BUS 1
adimmit 0:76c761d3caf1 283 Zero(&q11_can);
adimmit 15:56a06363a698 284 //Zero(&q12_can);
adimmit 15:56a06363a698 285 //Zero(&q13_can);
adimmit 5:f1703165ca7e 286 //CAN BUS 2
adimmit 5:f1703165ca7e 287 Zero(&q21_can);
adimmit 15:56a06363a698 288 //Zero(&q22_can);
adimmit 15:56a06363a698 289 //Zero(&q23_can);
adimmit 5:f1703165ca7e 290 //CAN BUS 3
adimmit 15:56a06363a698 291 //Zero(&q31_can);
adimmit 15:56a06363a698 292 //Zero(&q32_can);
adimmit 15:56a06363a698 293 //Zero(&q33_can);
adimmit 0:76c761d3caf1 294 break;
adimmit 0:76c761d3caf1 295 }
adimmit 0:76c761d3caf1 296 }
adimmit 0:76c761d3caf1 297 WriteAll();
adimmit 0:76c761d3caf1 298
adimmit 0:76c761d3caf1 299 }
adimmit 8:450fbccaf4f6 300
adimmit 0:76c761d3caf1 301
adimmit 0:76c761d3caf1 302 uint32_t xor_checksum(uint32_t* data, size_t len)
adimmit 0:76c761d3caf1 303 {
adimmit 0:76c761d3caf1 304 uint32_t t = 0;
adimmit 0:76c761d3caf1 305 for(int i = 0; i < len; i++)
adimmit 0:76c761d3caf1 306 t = t ^ data[i];
adimmit 0:76c761d3caf1 307 return t;
adimmit 0:76c761d3caf1 308 }
adimmit 0:76c761d3caf1 309
adimmit 2:32f69175c78e 310
adimmit 2:32f69175c78e 311
adimmit 2:32f69175c78e 312 void print_SPI_command() {
adimmit 2:32f69175c78e 313 pc.printf("SPI MESSAGE RECIEVED:\n");
adimmit 15:56a06363a698 314 //CAN ID ONE
adimmit 5:f1703165ca7e 315 pc.printf("MOTOR 1-1 Q: %f\n", spi_command.q_des_1s[0]);
adimmit 5:f1703165ca7e 316 pc.printf("MOTOR 1-1 Qd: %f\n", spi_command.qd_des_1s[0]);
adimmit 5:f1703165ca7e 317 pc.printf("MOTOR 1-1 Kp: %f\n", spi_command.kp_1s[0]);
adimmit 5:f1703165ca7e 318 pc.printf("MOTOR 1-1 Kd: %f\n", spi_command.kd_1s[0]);
adimmit 5:f1703165ca7e 319 pc.printf("MOTOR 1-1 T_FF: %f\n", spi_command.tau_1s_ff[0]);
adimmit 2:32f69175c78e 320
adimmit 5:f1703165ca7e 321 pc.printf("MOTOR 2-1 Q: %f\n", spi_command.q_des_1s[1]);
adimmit 5:f1703165ca7e 322 pc.printf("MOTOR 2-1 Qd: %f\n", spi_command.qd_des_1s[1]);
adimmit 5:f1703165ca7e 323 pc.printf("MOTOR 2-1 Kp: %f\n", spi_command.kp_1s[1]);
adimmit 5:f1703165ca7e 324 pc.printf("MOTOR 2-1 Kd: %f\n", spi_command.kd_1s[1]);
adimmit 5:f1703165ca7e 325 pc.printf("MOTOR 2-1_FF: %f\n", spi_command.tau_1s_ff[1]);
adimmit 2:32f69175c78e 326
adimmit 2:32f69175c78e 327 }
adimmit 2:32f69175c78e 328
adimmit 2:32f69175c78e 329
adimmit 2:32f69175c78e 330
adimmit 2:32f69175c78e 331
adimmit 9:16a550d36425 332 void print_SPI_data() {
adimmit 9:16a550d36425 333 pc.printf("SPI MESSAGE SENT:\n");
adimmit 15:56a06363a698 334 //CAN ID ONES
adimmit 9:16a550d36425 335 pc.printf("MOTOR 1-1 Q: %f\n", spi_data.q_1s[0]);
adimmit 9:16a550d36425 336 pc.printf("MOTOR 1-1 Qd: %f\n", spi_data.qd_1s[0]);
adimmit 9:16a550d36425 337
adimmit 9:16a550d36425 338 pc.printf("MOTOR 2-1 Q: %f\n", spi_data.q_1s[1]);
adimmit 9:16a550d36425 339 pc.printf("MOTOR 2-1 Qd: %f\n", spi_data.qd_1s[1]);
adimmit 9:16a550d36425 340
adimmit 9:16a550d36425 341 }
adimmit 9:16a550d36425 342
adimmit 9:16a550d36425 343
adimmit 9:16a550d36425 344
adimmit 12:5f97bcab6c50 345 void process()
adimmit 12:5f97bcab6c50 346 {
adimmit 13:87e7ce883f6a 347 //pc.printf("%f\n", spi_command.q_des_2s[0]);
adimmit 12:5f97bcab6c50 348 // update qs
adimmit 12:5f97bcab6c50 349 spi_data.q_1s[0] = spi_command.q_des_1s[0]+1.0;
adimmit 12:5f97bcab6c50 350 spi_data.q_1s[1] = spi_command.q_des_1s[1]+1.0;
adimmit 12:5f97bcab6c50 351 // update qds
adimmit 12:5f97bcab6c50 352 spi_data.qd_1s[0] = spi_command.qd_des_1s[0]+1.0;
adimmit 12:5f97bcab6c50 353 spi_data.qd_1s[1] = spi_command.qd_des_1s[1]+1.0;
adimmit 12:5f97bcab6c50 354 // update taus
adimmit 12:5f97bcab6c50 355 spi_data.tau_1s[0] = spi_command.tau_1s_ff[0]+1.0;
adimmit 12:5f97bcab6c50 356 spi_data.tau_1s[1] = spi_command.tau_1s_ff[1]+1.0;
adimmit 12:5f97bcab6c50 357 // UDPATE FLAGS
adimmit 12:5f97bcab6c50 358 spi_data.flags[0] = 0;
adimmit 12:5f97bcab6c50 359 spi_data.flags[1] = 0;
adimmit 12:5f97bcab6c50 360 // UPDATE CHECKSUM
adimmit 15:56a06363a698 361 spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8); //DONT CRC THE CRC!
adimmit 12:5f97bcab6c50 362 for(int i = 0; i < DATA_LEN; i++){
adimmit 12:5f97bcab6c50 363 tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
adimmit 12:5f97bcab6c50 364 }
adimmit 12:5f97bcab6c50 365
adimmit 9:16a550d36425 366
adimmit 2:32f69175c78e 367
adimmit 0:76c761d3caf1 368 void spi_isr(void)
adimmit 0:76c761d3caf1 369 {
adimmit 1:aa253b5f5b65 370 //pc.printf("CS ACTIVE\n");
adimmit 0:76c761d3caf1 371 GPIOC->ODR |= (1 << 8);
adimmit 0:76c761d3caf1 372 GPIOC->ODR &= ~(1 << 8);
adimmit 0:76c761d3caf1 373 int bytecount = 0;
adimmit 0:76c761d3caf1 374 SPI1->DR = tx_buff[0];
adimmit 0:76c761d3caf1 375 while(cs == 0) {
adimmit 0:76c761d3caf1 376 if(SPI1->SR&0x1) {
adimmit 1:aa253b5f5b65 377 rx_buff[bytecount] = SPI1->DR;
adimmit 0:76c761d3caf1 378 bytecount++;
adimmit 0:76c761d3caf1 379 if(bytecount<TX_LEN) {
adimmit 0:76c761d3caf1 380 SPI1->DR = tx_buff[bytecount];
adimmit 0:76c761d3caf1 381 }
adimmit 0:76c761d3caf1 382 }
adimmit 0:76c761d3caf1 383 }
adimmit 2:32f69175c78e 384 //pc.printf("RECIEVED: %d BYTES\n", bytecount);
adimmit 0:76c761d3caf1 385
adimmit 13:87e7ce883f6a 386 //update crc_chk from buffer
adimmit 13:87e7ce883f6a 387 for(int i = 0; i < RX_LEN; i++) {((uint16_t*)(&tmp_crc_chk))[i] = rx_buff[i];}
adimmit 13:87e7ce883f6a 388 // CHECK THE CHECKSUM
adimmit 15:56a06363a698 389 uint32_t _crc = xor_checksum((uint32_t*)&tmp_crc_chk, 12); //DONT CRC THE CRC!!!
adimmit 13:87e7ce883f6a 390 // READ CHECKSUM
adimmit 13:87e7ce883f6a 391 uint32_t _rx_crc = tmp_crc_chk.checksum;
adimmit 0:76c761d3caf1 392
adimmit 13:87e7ce883f6a 393 if(_crc == _rx_crc) {
adimmit 13:87e7ce883f6a 394 //pc.printf("CHECKSUM PASSED...");
adimmit 13:87e7ce883f6a 395 //update crc_chk from buffer
adimmit 13:87e7ce883f6a 396 spi_command = tmp_crc_chk;
adimmit 13:87e7ce883f6a 397 //do math on the input
adimmit 14:6fec697f62ef 398 //process();
adimmit 14:6fec697f62ef 399 control();
adimmit 14:6fec697f62ef 400 PackAll();
adimmit 15:56a06363a698 401 WriteAll();
adimmit 13:87e7ce883f6a 402 //new_command = 1;
adimmit 15:56a06363a698 403 //print_SPI_command();
adimmit 13:87e7ce883f6a 404 }
adimmit 13:87e7ce883f6a 405 else{pc.printf("bigger F in the chat...\n");}
adimmit 0:76c761d3caf1 406 }
adimmit 0:76c761d3caf1 407
adimmit 0:76c761d3caf1 408 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
adimmit 9:16a550d36425 409 /*
adimmit 0:76c761d3caf1 410 if((state.p)>=limit_p){
adimmit 0:76c761d3caf1 411 //control->p_des = limit_p;
adimmit 0:76c761d3caf1 412 control->v_des = 0.0f;
adimmit 0:76c761d3caf1 413 control->kp = 0;
adimmit 0:76c761d3caf1 414 control->kd = KD_SOFTSTOP;
adimmit 0:76c761d3caf1 415 control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
adimmit 0:76c761d3caf1 416 return 1;
adimmit 0:76c761d3caf1 417 }
adimmit 0:76c761d3caf1 418 else if((state.p)<=limit_n){
adimmit 0:76c761d3caf1 419 //control->p_des = limit_n;
adimmit 0:76c761d3caf1 420 control->v_des = 0.0f;
adimmit 0:76c761d3caf1 421 control->kp = 0;
adimmit 0:76c761d3caf1 422 control->kd = KD_SOFTSTOP;
adimmit 0:76c761d3caf1 423 control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
adimmit 0:76c761d3caf1 424 return 1;
adimmit 0:76c761d3caf1 425 }
adimmit 9:16a550d36425 426 */
adimmit 0:76c761d3caf1 427 return 0;
adimmit 0:76c761d3caf1 428
adimmit 0:76c761d3caf1 429 }
adimmit 0:76c761d3caf1 430
adimmit 0:76c761d3caf1 431
adimmit 0:76c761d3caf1 432 void control()
adimmit 0:76c761d3caf1 433 {
adimmit 0:76c761d3caf1 434
adimmit 0:76c761d3caf1 435 if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){
adimmit 0:76c761d3caf1 436 enabled = 1;
adimmit 0:76c761d3caf1 437 //BUS ONE
adimmit 0:76c761d3caf1 438 EnterMotorMode(&q11_can);
adimmit 0:76c761d3caf1 439 can1.write(q11_can);
adimmit 5:f1703165ca7e 440 //BUS TWO
adimmit 0:76c761d3caf1 441 EnterMotorMode(&q21_can);
adimmit 5:f1703165ca7e 442 can2.write(q21_can);
adimmit 8:450fbccaf4f6 443 //pc.printf("e\n\r");
adimmit 0:76c761d3caf1 444 return;
adimmit 0:76c761d3caf1 445 }
adimmit 0:76c761d3caf1 446 else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){
adimmit 0:76c761d3caf1 447 enabled = 0;
adimmit 0:76c761d3caf1 448 //BUS ONE
adimmit 0:76c761d3caf1 449 ExitMotorMode(&q11_can);
adimmit 0:76c761d3caf1 450 can1.write(q11_can);
adimmit 5:f1703165ca7e 451 //BUS TWO
adimmit 0:76c761d3caf1 452 ExitMotorMode(&q21_can);
adimmit 5:f1703165ca7e 453 can2.write(q21_can);
adimmit 0:76c761d3caf1 454 return;
adimmit 0:76c761d3caf1 455 }
adimmit 0:76c761d3caf1 456
adimmit 5:f1703165ca7e 457 //BUS 1 DATA
adimmit 5:f1703165ca7e 458 spi_data.q_1s[0] = g1_state.a1.p;
adimmit 5:f1703165ca7e 459 spi_data.qd_1s[0] = g1_state.a1.v;
adimmit 10:42438194e82b 460 spi_data.tau_1s[0] = g1_state.a1.t;
adimmit 5:f1703165ca7e 461 //BUS 2 DATA
adimmit 9:16a550d36425 462 spi_data.q_1s[1] = g2_state.a1.p;
adimmit 9:16a550d36425 463 spi_data.qd_1s[1] = g2_state.a1.v;
adimmit 10:42438194e82b 464 spi_data.tau_1s[1] = g2_state.a1.t;
adimmit 0:76c761d3caf1 465
adimmit 0:76c761d3caf1 466 if(estop==0){
adimmit 5:f1703165ca7e 467 printf("estopped!!!!\n\r");
adimmit 6:077e37d5c2da 468 memset(&g1_control, 0, sizeof(g1_control));
adimmit 6:077e37d5c2da 469 memset(&g2_control, 0, sizeof(g2_control));
adimmit 15:56a06363a698 470 //memset(&g3_control, 0, sizeof(g3_control));
adimmit 0:76c761d3caf1 471 spi_data.flags[0] = 0xdead;
adimmit 0:76c761d3caf1 472 spi_data.flags[1] = 0xdead;
adimmit 15:56a06363a698 473 //spi_data.flags[2] = 0xdead;
adimmit 0:76c761d3caf1 474 led = 1;
adimmit 0:76c761d3caf1 475 }
adimmit 0:76c761d3caf1 476
adimmit 0:76c761d3caf1 477 else{
adimmit 0:76c761d3caf1 478 led = 0;
adimmit 6:077e37d5c2da 479 memset(&g1_control, 0, sizeof(g1_control));
adimmit 6:077e37d5c2da 480 memset(&g2_control, 0, sizeof(g2_control));
adimmit 15:56a06363a698 481 //memset(&g3_control, 0, sizeof(g3_control));
adimmit 0:76c761d3caf1 482
adimmit 5:f1703165ca7e 483 //TRANSLATE SPI TO ACTUATOR COMMANNDS
adimmit 5:f1703165ca7e 484 //CAN1
adimmit 5:f1703165ca7e 485 //CAN1 MOTOR1
adimmit 5:f1703165ca7e 486 g1_control.a1.p_des = spi_command.q_des_1s[0];
adimmit 5:f1703165ca7e 487 g1_control.a1.v_des = spi_command.qd_des_1s[0];
adimmit 5:f1703165ca7e 488 g1_control.a1.kp = spi_command.kp_1s[0];
adimmit 5:f1703165ca7e 489 g1_control.a1.kd = spi_command.kd_1s[0];
adimmit 5:f1703165ca7e 490 g1_control.a1.t_ff = spi_command.tau_1s_ff[0];
adimmit 5:f1703165ca7e 491 //CAN2 MOTOR1
adimmit 5:f1703165ca7e 492 g2_control.a1.p_des = spi_command.q_des_1s[1];
adimmit 5:f1703165ca7e 493 g2_control.a1.v_des = spi_command.qd_des_1s[1];
adimmit 5:f1703165ca7e 494 g2_control.a1.kp = spi_command.kp_1s[1];
adimmit 5:f1703165ca7e 495 g2_control.a1.kd = spi_command.kd_1s[1];
adimmit 5:f1703165ca7e 496 g2_control.a1.t_ff = spi_command.tau_1s_ff[1];
adimmit 7:2f785cb39f11 497 //SPI FLAGS RETURN //IMPLEMENTS THE JOINT SOFT STOP RIGHT HERE
adimmit 0:76c761d3caf1 498 spi_data.flags[0] = 0;
adimmit 0:76c761d3caf1 499 spi_data.flags[1] = 0;
adimmit 0:76c761d3caf1 500 }
adimmit 15:56a06363a698 501 spi_data.checksum = xor_checksum((uint32_t*)&spi_data, 8);
adimmit 0:76c761d3caf1 502 for(int i = 0; i < DATA_LEN; i++){
adimmit 0:76c761d3caf1 503 tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
adimmit 0:76c761d3caf1 504
adimmit 0:76c761d3caf1 505 }
adimmit 0:76c761d3caf1 506
adimmit 0:76c761d3caf1 507
adimmit 0:76c761d3caf1 508 void test_control()
adimmit 0:76c761d3caf1 509 {
adimmit 0:76c761d3caf1 510 for(int i = 0; i < 3; i++)
adimmit 0:76c761d3caf1 511 {
adimmit 0:76c761d3caf1 512 spi_data.q_1s[i] = spi_command.q_des_1s[i] + 1.f;
adimmit 0:76c761d3caf1 513
adimmit 0:76c761d3caf1 514 spi_data.qd_1s[i] = spi_command.qd_des_1s[i] + 1.f;
adimmit 0:76c761d3caf1 515 }
adimmit 0:76c761d3caf1 516
adimmit 0:76c761d3caf1 517 spi_data.flags[0] = 0xdead;
adimmit 0:76c761d3caf1 518 //spi_data.flags[1] = 0xbeef;
adimmit 0:76c761d3caf1 519
adimmit 0:76c761d3caf1 520 // only do first 56 bytes of message.
adimmit 15:56a06363a698 521 spi_data.checksum = xor_checksum((uint32_t*)&spi_data,12);
adimmit 0:76c761d3caf1 522
adimmit 0:76c761d3caf1 523 for(int i = 0; i < DATA_LEN; i++)
adimmit 0:76c761d3caf1 524 tx_buff[i] = ((uint16_t*)(&spi_data))[i];
adimmit 0:76c761d3caf1 525 }
adimmit 0:76c761d3caf1 526
adimmit 0:76c761d3caf1 527 void init_spi(void){
adimmit 0:76c761d3caf1 528 SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
adimmit 0:76c761d3caf1 529 spi->format(16, 0);
adimmit 15:56a06363a698 530 spi->frequency(500000);
adimmit 0:76c761d3caf1 531 spi->reply(0x0);
adimmit 0:76c761d3caf1 532 cs.fall(&spi_isr);
adimmit 1:aa253b5f5b65 533 pc.printf("done\n\r");
adimmit 0:76c761d3caf1 534 }
adimmit 0:76c761d3caf1 535
adimmit 0:76c761d3caf1 536 int main() {
adimmit 0:76c761d3caf1 537 wait(1);
adimmit 0:76c761d3caf1 538 //led = 1;
adimmit 1:aa253b5f5b65 539 pc.baud(115200); //MAYBE CHANGE THIS IF NEEDED
adimmit 0:76c761d3caf1 540 pc.attach(&serial_isr);
adimmit 0:76c761d3caf1 541 estop.mode(PullUp);
adimmit 0:76c761d3caf1 542
adimmit 0:76c761d3caf1 543 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
adimmit 0:76c761d3caf1 544 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
adimmit 0:76c761d3caf1 545
adimmit 0:76c761d3caf1 546 memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
adimmit 0:76c761d3caf1 547 memset(&spi_data, 0, sizeof(spi_data_t));
adimmit 0:76c761d3caf1 548 memset(&spi_command,0,sizeof(spi_command_t));
adimmit 0:76c761d3caf1 549
adimmit 0:76c761d3caf1 550
adimmit 0:76c761d3caf1 551 NVIC_SetPriority(TIM5_IRQn, 1);
adimmit 0:76c761d3caf1 552 //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
adimmit 0:76c761d3caf1 553 //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
adimmit 0:76c761d3caf1 554
adimmit 0:76c761d3caf1 555 pc.printf("\n\r SPIne\n\r");
adimmit 0:76c761d3caf1 556 //printf("%d\n\r", RX_ID << 18);
adimmit 0:76c761d3caf1 557
adimmit 5:f1703165ca7e 558 //CAN 1 BUS
adimmit 0:76c761d3caf1 559 q11_can.len = 8; //transmit 8 bytes
adimmit 5:f1703165ca7e 560 //CAN 2 BUS
adimmit 0:76c761d3caf1 561 q21_can.len = 8; //transmit 8 bytes
adimmit 5:f1703165ca7e 562 //RECIEVE
adimmit 0:76c761d3caf1 563 rxMsg1.len = 6; //receive 6 bytes
adimmit 0:76c761d3caf1 564 rxMsg2.len = 6;
adimmit 5:f1703165ca7e 565
adimmit 5:f1703165ca7e 566 //CAN 1 BUS
adimmit 0:76c761d3caf1 567 q11_can.id = 0x1;
adimmit 5:f1703165ca7e 568 //CAN 2 BUS
adimmit 5:f1703165ca7e 569 q21_can.id = 0x1;
adimmit 0:76c761d3caf1 570
adimmit 5:f1703165ca7e 571 //actuators on the CAN1 bus
adimmit 6:077e37d5c2da 572 pack_cmd(&q11_can, g1_control.a1);
adimmit 5:f1703165ca7e 573 //actuators on the CAN2 bus
adimmit 6:077e37d5c2da 574 pack_cmd(&q21_can, g2_control.a1);
adimmit 5:f1703165ca7e 575 //WRITE THE INITIAL COMMAND
adimmit 0:76c761d3caf1 576 WriteAll();
adimmit 1:aa253b5f5b65 577
adimmit 1:aa253b5f5b65 578 //just debugging things
adimmit 1:aa253b5f5b65 579 pc.printf("SETUP VARS ALL DONE\n");
adimmit 0:76c761d3caf1 580
adimmit 0:76c761d3caf1 581
adimmit 0:76c761d3caf1 582 // SPI doesn't work if enabled while the CS pin is pulled low
adimmit 0:76c761d3caf1 583 // Wait for CS to not be low, then enable SPI
adimmit 0:76c761d3caf1 584 if(!spi_enabled){
adimmit 1:aa253b5f5b65 585 while((spi_enabled==0) && (cs.read() ==0)){pc.printf("waiting for CS Pin\n"); wait_us(10);}
adimmit 0:76c761d3caf1 586 init_spi();
adimmit 0:76c761d3caf1 587 spi_enabled = 1;
adimmit 1:aa253b5f5b65 588 pc.printf("SPI ENABLED AND READY\n");
adimmit 0:76c761d3caf1 589 }
adimmit 0:76c761d3caf1 590
adimmit 0:76c761d3caf1 591 //spi_command=set the thing here...
adimmit 0:76c761d3caf1 592
adimmit 0:76c761d3caf1 593 while(1) {
adimmit 15:56a06363a698 594 //pc.printf("heartbeat...\n");
adimmit 0:76c761d3caf1 595 counter++;
adimmit 13:87e7ce883f6a 596 can2.read(rxMsg2);
adimmit 13:87e7ce883f6a 597 unpack_reply(rxMsg2, &g2_state);
adimmit 13:87e7ce883f6a 598 can1.read(rxMsg1); // read message into Rx message storage
adimmit 13:87e7ce883f6a 599 unpack_reply(rxMsg1, &g1_state);
adimmit 15:56a06363a698 600 ///wait(0.01);
adimmit 15:56a06363a698 601 }
adimmit 2:32f69175c78e 602
adimmit 0:76c761d3caf1 603 }