Ben Katz / SPIne

SPI to quad CAN firmware

Dependencies:   mbed-dev_spine

Fork of Teleop_Controller by Ben Katz

main.cpp@7:f10513577d4c, 2018-07-15 (annotated)

Committer:
benkatz
Date:
Sun Jul 15 21:55:27 2018 +0000
Revision:
7:f10513577d4c
Parent:
3:9ef9b4c66648
Child:
8:10162f709930
Child:
9:56747e76c9c1
new robot version

Who changed what in which revision?

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