WANG YUCHAO / Mbed 2 deprecated spine_2020_9_8

mit

Dependencies:   mbed

main.cpp@10:b8582775d466, 2020-09-15 (annotated)

Committer:
shaorui
Date:
Tue Sep 15 09:02:03 2020 +0000
Revision:
10:b8582775d466
Parent:
9:56747e76c9c1
Child:
11:2404f2218040
1;

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