Induk Choi / MCP_SPIne

MCP with wheel

Dependencies:   mbed-dev_spine

main.cpp@12:2aa99d54af80, 2020-11-13 (annotated)

Committer:
indolcid
Date:
Fri Nov 13 02:38:14 2020 +0000
Revision:
12:2aa99d54af80
Parent:
11:58a70b97c750 
Modified velocity control command;   - Desired Velocity + Torque F/F

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 9:56747e76c9c1 23 #define V_MIN -65.0f
benkatz 9:56747e76c9c1 24 #define V_MAX 65.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 3:9ef9b4c66648 31 /// Joint Soft Stops ///
benkatz 3:9ef9b4c66648 32 #define A_LIM_P 1.5f
benkatz 3:9ef9b4c66648 33 #define A_LIM_N -1.5f
benkatz 3:9ef9b4c66648 34 #define H_LIM_P 5.0f
benkatz 3:9ef9b4c66648 35 #define H_LIM_N -5.0f
benkatz 3:9ef9b4c66648 36 #define K_LIM_P 0.2f
benkatz 3:9ef9b4c66648 37 #define K_LIM_N 7.7f
benkatz 3:9ef9b4c66648 38 #define KP_SOFTSTOP 100.0f
benkatz 3:9ef9b4c66648 39 #define KD_SOFTSTOP 0.4f;
benkatz 3:9ef9b4c66648 40
benkatz 3:9ef9b4c66648 41 #define ENABLE_CMD 0xFFFF
benkatz 3:9ef9b4c66648 42 #define DISABLE_CMD 0x1F1F
benkatz 3:9ef9b4c66648 43
benkatz 3:9ef9b4c66648 44 spi_data_t spi_data; // data from spine to up
benkatz 3:9ef9b4c66648 45 spi_command_t spi_command; // data from up to spine
benkatz 3:9ef9b4c66648 46
benkatz 3:9ef9b4c66648 47 // spi buffers
benkatz 3:9ef9b4c66648 48 uint16_t rx_buff[RX_LEN];
benkatz 3:9ef9b4c66648 49 uint16_t tx_buff[TX_LEN];
benkatz 3:9ef9b4c66648 50
benkatz 3:9ef9b4c66648 51 DigitalOut led(PC_5);
benkatz 3:9ef9b4c66648 52
benkatz 3:9ef9b4c66648 53
Ikhee 10:54e95428798b 54 //Serial pc(PA_2, PA_3);
Ikhee 10:54e95428798b 55 Serial pc(PA_9, PA_10);
Ikhee 10:54e95428798b 56 CAN can1(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
Ikhee 10:54e95428798b 57 CAN can2(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name
Ikhee 10:54e95428798b 58 //CAN can1(PB_12, PB_13, 1000000); // CAN Rx pin name, CAN Tx pin name
Ikhee 10:54e95428798b 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);
Ikhee 10:54e95428798b 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 }
indolcid 11:58a70b97c750 134
indolcid 11:58a70b97c750 135 void pack_cmd2(CANMessage * msg, joint_control joint){
indolcid 11:58a70b97c750 136
indolcid 12:2aa99d54af80 137 int16_t v_des = (int16_t)fminf(fmaxf(-32738.0f, joint.v_des * 128.0f), 32767.0f);
indolcid 12:2aa99d54af80 138 int16_t t_ff = (int16_t)fminf(fmaxf(-32738.0f, joint.t_ff * 128.0f), 32767.0f);
indolcid 12:2aa99d54af80 139 uint8_t kp = (uint8_t)fminf(fmaxf(0.0f, joint.kp * 16.0f), 255.0f);
indolcid 12:2aa99d54af80 140 uint8_t ki = (uint8_t)fminf(fmaxf(0.0f, joint.kd * 16.0f), 255.0f);
indolcid 11:58a70b97c750 141
indolcid 12:2aa99d54af80 142 memcpy(msg->data, &v_des, 2);
indolcid 12:2aa99d54af80 143 memcpy(msg->data + 2, &t_ff, 2);
indolcid 12:2aa99d54af80 144 memcpy(msg->data + 4, &kp, 1);
indolcid 12:2aa99d54af80 145 memcpy(msg->data + 5, &ki, 1);
indolcid 11:58a70b97c750 146 }
benkatz 0:d6186b8990c5 147
benkatz 0:d6186b8990c5 148 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 149 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 150 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 151 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 152 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 153 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 154 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 155 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 156 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 157 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 158 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 159
benkatz 3:9ef9b4c66648 160 void unpack_reply(CANMessage msg, leg_state * leg){
benkatz 0:d6186b8990c5 161 /// unpack ints from can buffer ///
indolcid 11:58a70b97c750 162 if(msg.id == 6) {
indolcid 11:58a70b97c750 163 float p, v;
indolcid 11:58a70b97c750 164 memcpy(&v, msg.data, 4);
indolcid 11:58a70b97c750 165 memcpy(&p, msg.data + 4, 4);
indolcid 11:58a70b97c750 166 leg->k.v = v;
indolcid 11:58a70b97c750 167 leg->k.p = p;
indolcid 11:58a70b97c750 168 leg->k.t = 0.0f;
indolcid 11:58a70b97c750 169 } else {
indolcid 11:58a70b97c750 170 uint16_t id = msg.data[0];
indolcid 11:58a70b97c750 171 uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
indolcid 11:58a70b97c750 172 uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
indolcid 11:58a70b97c750 173 uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
indolcid 11:58a70b97c750 174 /// convert uints to floats ///
indolcid 11:58a70b97c750 175 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
indolcid 11:58a70b97c750 176 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
indolcid 11:58a70b97c750 177 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
indolcid 11:58a70b97c750 178
indolcid 11:58a70b97c750 179 if(id == 1) {
indolcid 11:58a70b97c750 180 leg->a.p = p;
indolcid 11:58a70b97c750 181 leg->a.v = v;
indolcid 11:58a70b97c750 182 leg->a.t = t;
indolcid 11:58a70b97c750 183 } else if(id == 2) {
indolcid 11:58a70b97c750 184 leg->h.p = p;
indolcid 11:58a70b97c750 185 leg->h.v = v;
indolcid 11:58a70b97c750 186 leg->h.t = t;
indolcid 11:58a70b97c750 187 } else if(id == 3) {
indolcid 11:58a70b97c750 188 leg->k.p = p;
indolcid 11:58a70b97c750 189 leg->k.v = v;
indolcid 11:58a70b97c750 190 leg->k.t = t;
benkatz 2:25837cbaee98 191 }
indolcid 11:58a70b97c750 192 }
indolcid 11:58a70b97c750 193 }
benkatz 3:9ef9b4c66648 194
benkatz 1:79e0d4791936 195 void rxISR1() {
benkatz 1:79e0d4791936 196 can1.read(rxMsg1); // read message into Rx message storage
benkatz 3:9ef9b4c66648 197 unpack_reply(rxMsg1, &l1_state);
benkatz 0:d6186b8990c5 198 }
benkatz 1:79e0d4791936 199 void rxISR2(){
benkatz 1:79e0d4791936 200 can2.read(rxMsg2);
benkatz 3:9ef9b4c66648 201 unpack_reply(rxMsg2, &l2_state);
benkatz 1:79e0d4791936 202 }
benkatz 3:9ef9b4c66648 203 void PackAll(){
indolcid 11:58a70b97c750 204 // pack_cmd(&a1_can, l1_control.a);
indolcid 11:58a70b97c750 205 // pack_cmd(&a2_can, l2_control.a);
benkatz 3:9ef9b4c66648 206 pack_cmd(&h1_can, l1_control.h);
benkatz 3:9ef9b4c66648 207 pack_cmd(&h2_can, l2_control.h);
indolcid 11:58a70b97c750 208 pack_cmd2(&k1_can, l1_control.k);
indolcid 11:58a70b97c750 209 pack_cmd2(&k2_can, l2_control.k);
benkatz 3:9ef9b4c66648 210
benkatz 3:9ef9b4c66648 211 }
benkatz 1:79e0d4791936 212 void WriteAll(){
benkatz 2:25837cbaee98 213 //toggle = 1;
indolcid 11:58a70b97c750 214 /*
benkatz 3:9ef9b4c66648 215 can1.write(a1_can);
benkatz 3:9ef9b4c66648 216 wait(.00002);
benkatz 3:9ef9b4c66648 217 can2.write(a2_can);
benkatz 3:9ef9b4c66648 218 wait(.00002);
indolcid 11:58a70b97c750 219 */
benkatz 3:9ef9b4c66648 220 can1.write(h1_can);
benkatz 3:9ef9b4c66648 221 wait(.00002);
benkatz 3:9ef9b4c66648 222 can2.write(h2_can);
benkatz 3:9ef9b4c66648 223 wait(.00002);
benkatz 3:9ef9b4c66648 224 can1.write(k1_can);
benkatz 3:9ef9b4c66648 225 wait(.00002);
benkatz 3:9ef9b4c66648 226 can2.write(k2_can);
benkatz 3:9ef9b4c66648 227 wait(.00002);
benkatz 2:25837cbaee98 228 //toggle = 0;
Ikhee 10:54e95428798b 229
Ikhee 10:54e95428798b 230
Ikhee 10:54e95428798b 231 // //toggle = 1;
Ikhee 10:54e95428798b 232 // can1.write(a1_can);
Ikhee 10:54e95428798b 233 // wait(.00002);
Ikhee 10:54e95428798b 234 // can1.write(h1_can);
Ikhee 10:54e95428798b 235 // wait(.00002);
Ikhee 10:54e95428798b 236 // can1.write(k1_can);
Ikhee 10:54e95428798b 237 // wait(.00002);
Ikhee 10:54e95428798b 238 //
Ikhee 10:54e95428798b 239 // can2.write(a2_can);
Ikhee 10:54e95428798b 240 // wait(.00002);
Ikhee 10:54e95428798b 241 // can2.write(h2_can);
Ikhee 10:54e95428798b 242 // wait(.00002);
Ikhee 10:54e95428798b 243 // can2.write(k2_can);
Ikhee 10:54e95428798b 244 // wait(.00002);
Ikhee 10:54e95428798b 245 // //toggle = 0;
Ikhee 10:54e95428798b 246
benkatz 1:79e0d4791936 247 }
benkatz 0:d6186b8990c5 248
benkatz 0:d6186b8990c5 249 void sendCMD(){
benkatz 1:79e0d4791936 250 counter ++;
benkatz 3:9ef9b4c66648 251
benkatz 3:9ef9b4c66648 252 PackAll();
benkatz 3:9ef9b4c66648 253
benkatz 3:9ef9b4c66648 254 if(counter>100){
benkatz 3:9ef9b4c66648 255 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 256 counter = 0 ;
benkatz 3:9ef9b4c66648 257 }
benkatz 1:79e0d4791936 258
benkatz 3:9ef9b4c66648 259 WriteAll();
benkatz 2:25837cbaee98 260
benkatz 3:9ef9b4c66648 261 }
benkatz 2:25837cbaee98 262
benkatz 3:9ef9b4c66648 263
benkatz 3:9ef9b4c66648 264
benkatz 0:d6186b8990c5 265
benkatz 1:79e0d4791936 266 void Zero(CANMessage * msg){
benkatz 1:79e0d4791936 267 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 268 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 269 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 270 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 271 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 272 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 273 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 274 msg->data[7] = 0xFE;
indolcid 11:58a70b97c750 275 //WriteAll();
benkatz 1:79e0d4791936 276 }
benkatz 1:79e0d4791936 277
benkatz 1:79e0d4791936 278 void EnterMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 279 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 280 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 281 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 282 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 283 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 284 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 285 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 286 msg->data[7] = 0xFC;
benkatz 3:9ef9b4c66648 287 //WriteAll();
benkatz 1:79e0d4791936 288 }
indolcid 11:58a70b97c750 289
indolcid 11:58a70b97c750 290 void EnterVelCtrlMode(CANMessage * msg){
indolcid 11:58a70b97c750 291 msg->data[0] = 0xFF;
indolcid 11:58a70b97c750 292 msg->data[1] = 0xFF;
indolcid 11:58a70b97c750 293 msg->data[2] = 0xFF;
indolcid 11:58a70b97c750 294 msg->data[3] = 0xFF;
indolcid 11:58a70b97c750 295 msg->data[4] = 0xFF;
indolcid 11:58a70b97c750 296 msg->data[5] = 0xFF;
indolcid 11:58a70b97c750 297 msg->data[6] = 0xFF;
indolcid 11:58a70b97c750 298 msg->data[7] = 0xFB;
indolcid 11:58a70b97c750 299 //WriteAll();
indolcid 11:58a70b97c750 300 }
benkatz 1:79e0d4791936 301
benkatz 1:79e0d4791936 302 void ExitMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 303 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 304 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 305 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 306 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 307 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 308 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 309 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 310 msg->data[7] = 0xFD;
benkatz 3:9ef9b4c66648 311 //WriteAll();
benkatz 1:79e0d4791936 312 }
indolcid 11:58a70b97c750 313
benkatz 0:d6186b8990c5 314 void serial_isr(){
benkatz 1:79e0d4791936 315 /// handle keyboard commands from the serial terminal ///
benkatz 0:d6186b8990c5 316 while(pc.readable()){
benkatz 0:d6186b8990c5 317 char c = pc.getc();
benkatz 3:9ef9b4c66648 318 //led = !led;
benkatz 0:d6186b8990c5 319 switch(c){
benkatz 0:d6186b8990c5 320 case(27):
benkatz 3:9ef9b4c66648 321 //loop.detach();
benkatz 0:d6186b8990c5 322 printf("\n\r exiting motor mode \n\r");
indolcid 11:58a70b97c750 323 // ExitMotorMode(&a1_can);
indolcid 11:58a70b97c750 324 // ExitMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 325 ExitMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 326 ExitMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 327 ExitMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 328 ExitMotorMode(&k2_can);
indolcid 11:58a70b97c750 329 l1_control.k.v_des = 0.0f;
indolcid 11:58a70b97c750 330 l2_control.k.v_des = 0.0f;
benkatz 1:79e0d4791936 331 enabled = 0;
benkatz 0:d6186b8990c5 332 break;
benkatz 0:d6186b8990c5 333 case('m'):
benkatz 0:d6186b8990c5 334 printf("\n\r entering motor mode \n\r");
indolcid 11:58a70b97c750 335 // EnterMotorMode(&a1_can);
indolcid 11:58a70b97c750 336 // EnterMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 337 EnterMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 338 EnterMotorMode(&h2_can);
indolcid 11:58a70b97c750 339 EnterVelCtrlMode(&k1_can);
indolcid 11:58a70b97c750 340 EnterVelCtrlMode(&k2_can);
benkatz 1:79e0d4791936 341 wait(.5);
indolcid 11:58a70b97c750 342 enabled = 0xC;
benkatz 3:9ef9b4c66648 343 //loop.attach(&sendCMD, .001);
benkatz 3:9ef9b4c66648 344 break;
benkatz 3:9ef9b4c66648 345 case('s'):
benkatz 3:9ef9b4c66648 346 printf("\n\r standing \n\r");
benkatz 3:9ef9b4c66648 347 counter2 = 0;
benkatz 3:9ef9b4c66648 348 is_standing = 1;
benkatz 3:9ef9b4c66648 349 //stand();
benkatz 0:d6186b8990c5 350 break;
benkatz 0:d6186b8990c5 351 case('z'):
benkatz 0:d6186b8990c5 352 printf("\n\r zeroing \n\r");
indolcid 11:58a70b97c750 353 // Zero(&a1_can);
indolcid 11:58a70b97c750 354 // Zero(&a2_can);
indolcid 11:58a70b97c750 355 Zero(&h1_can);
indolcid 11:58a70b97c750 356 Zero(&h2_can);
indolcid 11:58a70b97c750 357 l1_control.k.v_des = 0.0f;
indolcid 11:58a70b97c750 358 l2_control.k.v_des = 0.0f;
indolcid 11:58a70b97c750 359 pack_cmd2(&k1_can, l1_control.k);
indolcid 11:58a70b97c750 360 pack_cmd2(&k2_can, l2_control.k);
indolcid 11:58a70b97c750 361 break;
indolcid 11:58a70b97c750 362 case('+'):
indolcid 11:58a70b97c750 363 printf("+");
benkatz 3:9ef9b4c66648 364 Zero(&h1_can);
benkatz 3:9ef9b4c66648 365 Zero(&h2_can);
indolcid 12:2aa99d54af80 366
indolcid 12:2aa99d54af80 367 l1_control.k.p_des = 0.0f;
indolcid 11:58a70b97c750 368 l1_control.k.v_des += PI * 2.0f * 0.1f;
indolcid 12:2aa99d54af80 369 l1_control.k.kp = 4.0f;
indolcid 12:2aa99d54af80 370 l1_control.k.kd = 0.5f;
indolcid 12:2aa99d54af80 371 l1_control.k.t_ff = 0.0f;
indolcid 12:2aa99d54af80 372
indolcid 12:2aa99d54af80 373 l2_control.k.p_des = PI * 2.0f * 0.1f;
indolcid 11:58a70b97c750 374 l2_control.k.v_des += PI * 2.0f * 0.1f;
indolcid 12:2aa99d54af80 375 l2_control.k.kp = PI * 4.0f;
indolcid 12:2aa99d54af80 376 l2_control.k.kd = PI * 0.5f;
indolcid 12:2aa99d54af80 377 l2_control.k.t_ff = PI * 2.0f * 0.1f;
indolcid 12:2aa99d54af80 378
indolcid 11:58a70b97c750 379 pack_cmd2(&k1_can, l1_control.k);
indolcid 11:58a70b97c750 380 pack_cmd2(&k2_can, l2_control.k);
indolcid 11:58a70b97c750 381 break;
indolcid 11:58a70b97c750 382 case('-'):
indolcid 11:58a70b97c750 383 printf("-");
indolcid 11:58a70b97c750 384 Zero(&h1_can);
indolcid 11:58a70b97c750 385 Zero(&h2_can);
indolcid 12:2aa99d54af80 386
indolcid 12:2aa99d54af80 387 l1_control.k.p_des = 0.0f;
indolcid 11:58a70b97c750 388 l1_control.k.v_des -= PI * 2.0f * 0.1f;
indolcid 12:2aa99d54af80 389 l1_control.k.kp = 4.0f;
indolcid 12:2aa99d54af80 390 l1_control.k.kd = 0.5f;
indolcid 12:2aa99d54af80 391 l1_control.k.t_ff = 0.0f;
indolcid 12:2aa99d54af80 392
indolcid 12:2aa99d54af80 393 l2_control.k.p_des = 0.0f;
indolcid 11:58a70b97c750 394 l2_control.k.v_des -= PI * 2.0f * 0.1f;
indolcid 12:2aa99d54af80 395 l2_control.k.kp = 4.0f;
indolcid 12:2aa99d54af80 396 l2_control.k.kd = 0.5f;
indolcid 12:2aa99d54af80 397 l2_control.k.t_ff = 0.0f;
indolcid 12:2aa99d54af80 398
indolcid 11:58a70b97c750 399 pack_cmd2(&k1_can, l1_control.k);
indolcid 11:58a70b97c750 400 pack_cmd2(&k2_can, l2_control.k);
benkatz 2:25837cbaee98 401 break;
benkatz 0:d6186b8990c5 402 }
benkatz 0:d6186b8990c5 403 }
benkatz 1:79e0d4791936 404 WriteAll();
benkatz 0:d6186b8990c5 405
benkatz 0:d6186b8990c5 406 }
benkatz 0:d6186b8990c5 407
benkatz 3:9ef9b4c66648 408 uint32_t xor_checksum(uint32_t* data, size_t len)
benkatz 3:9ef9b4c66648 409 {
benkatz 3:9ef9b4c66648 410 uint32_t t = 0;
benkatz 3:9ef9b4c66648 411 for(int i = 0; i < len; i++)
benkatz 3:9ef9b4c66648 412 t = t ^ data[i];
benkatz 3:9ef9b4c66648 413 return t;
benkatz 3:9ef9b4c66648 414 }
benkatz 3:9ef9b4c66648 415
benkatz 3:9ef9b4c66648 416 void spi_isr(void)
benkatz 3:9ef9b4c66648 417 {
benkatz 3:9ef9b4c66648 418 GPIOC->ODR |= (1 << 8);
benkatz 3:9ef9b4c66648 419 GPIOC->ODR &= ~(1 << 8);
benkatz 3:9ef9b4c66648 420 int bytecount = 0;
benkatz 3:9ef9b4c66648 421 SPI1->DR = tx_buff[0];
benkatz 3:9ef9b4c66648 422 while(cs == 0) {
benkatz 3:9ef9b4c66648 423 if(SPI1->SR&0x1) {
benkatz 3:9ef9b4c66648 424 rx_buff[bytecount] = SPI1->DR;
benkatz 3:9ef9b4c66648 425 bytecount++;
benkatz 3:9ef9b4c66648 426 if(bytecount<TX_LEN) {
benkatz 3:9ef9b4c66648 427 SPI1->DR = tx_buff[bytecount];
benkatz 3:9ef9b4c66648 428 }
benkatz 3:9ef9b4c66648 429 }
benkatz 3:9ef9b4c66648 430
benkatz 3:9ef9b4c66648 431 }
benkatz 2:25837cbaee98 432
benkatz 3:9ef9b4c66648 433 // after reading, save into spi_command
benkatz 3:9ef9b4c66648 434 // should probably check checksum first!
benkatz 3:9ef9b4c66648 435 uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
benkatz 3:9ef9b4c66648 436 for(int i = 0; i < CMD_LEN; i++)
benkatz 3:9ef9b4c66648 437 {
benkatz 3:9ef9b4c66648 438 ((uint16_t*)(&spi_command))[i] = rx_buff[i];
benkatz 3:9ef9b4c66648 439 }
benkatz 1:79e0d4791936 440
benkatz 3:9ef9b4c66648 441 // run control, which fills in tx_buff for the next iteration
benkatz 3:9ef9b4c66648 442 if(calc_checksum != spi_command.checksum){
benkatz 3:9ef9b4c66648 443 spi_data.flags[1] = 0xdead;}
benkatz 3:9ef9b4c66648 444
benkatz 3:9ef9b4c66648 445 //test_control();
benkatz 3:9ef9b4c66648 446 //spi_data.q_abad[0] = 12.0f;
benkatz 3:9ef9b4c66648 447 control();
benkatz 3:9ef9b4c66648 448 PackAll();
benkatz 3:9ef9b4c66648 449 WriteAll();
benkatz 1:79e0d4791936 450
benkatz 3:9ef9b4c66648 451 //for (int i = 0; i<TX_LEN; i++) {
benkatz 3:9ef9b4c66648 452 // tx_buff[i] = 2*rx_buff[i];
benkatz 3:9ef9b4c66648 453 //}
benkatz 3:9ef9b4c66648 454 // for (int i=0; i<TX_LEN; i++) {
benkatz 3:9ef9b4c66648 455 // //printf("%d ", rx_buff[i]);
benkatz 3:9ef9b4c66648 456 // }
benkatz 3:9ef9b4c66648 457 //printf("\n\r");
benkatz 3:9ef9b4c66648 458 }
benkatz 3:9ef9b4c66648 459
benkatz 3:9ef9b4c66648 460 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
benkatz 3:9ef9b4c66648 461 if((state.p)>=limit_p){
benkatz 3:9ef9b4c66648 462 //control->p_des = limit_p;
benkatz 3:9ef9b4c66648 463 control->v_des = 0.0f;
benkatz 3:9ef9b4c66648 464 control->kp = 0;
benkatz 3:9ef9b4c66648 465 control->kd = KD_SOFTSTOP;
benkatz 3:9ef9b4c66648 466 control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
benkatz 3:9ef9b4c66648 467 return 1;
benkatz 3:9ef9b4c66648 468 }
benkatz 3:9ef9b4c66648 469 else if((state.p)<=limit_n){
benkatz 3:9ef9b4c66648 470 //control->p_des = limit_n;
benkatz 3:9ef9b4c66648 471 control->v_des = 0.0f;
benkatz 3:9ef9b4c66648 472 control->kp = 0;
benkatz 3:9ef9b4c66648 473 control->kd = KD_SOFTSTOP;
benkatz 3:9ef9b4c66648 474 control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
benkatz 3:9ef9b4c66648 475 return 1;
benkatz 3:9ef9b4c66648 476 }
benkatz 3:9ef9b4c66648 477 return 0;
benkatz 3:9ef9b4c66648 478
benkatz 3:9ef9b4c66648 479 }
benkatz 3:9ef9b4c66648 480
benkatz 3:9ef9b4c66648 481
benkatz 3:9ef9b4c66648 482 void control()
benkatz 3:9ef9b4c66648 483 {
indolcid 11:58a70b97c750 484 int32_t en_flag = spi_command.flags[0];
indolcid 11:58a70b97c750 485
indolcid 11:58a70b97c750 486 if((en_flag&0x1) && !(enabled&0x1)) {
indolcid 11:58a70b97c750 487 enabled |= 0x1;
benkatz 3:9ef9b4c66648 488 EnterMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 489 can1.write(h1_can);
indolcid 11:58a70b97c750 490 }
indolcid 11:58a70b97c750 491
indolcid 11:58a70b97c750 492 if((en_flag&0x2) && !(enabled&0x2)) {
indolcid 11:58a70b97c750 493 enabled |= 0x2;
benkatz 3:9ef9b4c66648 494 EnterMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 495 can2.write(h2_can);
indolcid 11:58a70b97c750 496 wait(.00002);
indolcid 11:58a70b97c750 497 }
indolcid 11:58a70b97c750 498
indolcid 11:58a70b97c750 499 if((en_flag&0x4) && !(enabled&0x4)) {
indolcid 11:58a70b97c750 500 enabled |= 0x4;
indolcid 11:58a70b97c750 501 EnterVelCtrlMode(&k1_can);
indolcid 11:58a70b97c750 502 can1.write(k1_can);
indolcid 11:58a70b97c750 503 wait(.00002);
benkatz 3:9ef9b4c66648 504 }
indolcid 11:58a70b97c750 505
indolcid 11:58a70b97c750 506 if((en_flag&0x8) && !(enabled&0x8)) {
indolcid 11:58a70b97c750 507 enabled |= 0x8;
indolcid 11:58a70b97c750 508 EnterVelCtrlMode(&k2_can);
indolcid 11:58a70b97c750 509 can2.write(k2_can);
indolcid 11:58a70b97c750 510 wait(.00002);
indolcid 11:58a70b97c750 511 }
indolcid 11:58a70b97c750 512
indolcid 11:58a70b97c750 513 if(!(en_flag&0x1) && (enabled&0x1)) {
indolcid 11:58a70b97c750 514 enabled &= ~0x1;
benkatz 3:9ef9b4c66648 515 ExitMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 516 can1.write(h1_can);
indolcid 11:58a70b97c750 517 wait(.00002);
indolcid 11:58a70b97c750 518 }
indolcid 11:58a70b97c750 519
indolcid 11:58a70b97c750 520 if(!(en_flag&0x2) && (enabled&0x2)) {
indolcid 11:58a70b97c750 521 enabled &= ~0x2;
benkatz 3:9ef9b4c66648 522 ExitMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 523 can2.write(h2_can);
indolcid 11:58a70b97c750 524 wait(.00002);
indolcid 11:58a70b97c750 525 }
indolcid 11:58a70b97c750 526
indolcid 11:58a70b97c750 527 if(!(en_flag&0x4) && (enabled&0x4)) {
indolcid 11:58a70b97c750 528 enabled &= ~0x4;
benkatz 3:9ef9b4c66648 529 ExitMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 530 can1.write(k1_can);
indolcid 11:58a70b97c750 531 wait(.00002);
indolcid 11:58a70b97c750 532 }
indolcid 11:58a70b97c750 533
indolcid 11:58a70b97c750 534 if(!(en_flag&0x8) && (enabled&0x8)) {
indolcid 11:58a70b97c750 535 enabled &= ~0x8;
benkatz 3:9ef9b4c66648 536 ExitMotorMode(&k2_can);
benkatz 3:9ef9b4c66648 537 can2.write(k2_can);
indolcid 11:58a70b97c750 538 wait(.00002);
indolcid 11:58a70b97c750 539 }
indolcid 11:58a70b97c750 540
indolcid 11:58a70b97c750 541 // spi_data.q_abad[0] = l1_state.a.p;
benkatz 3:9ef9b4c66648 542 spi_data.q_hip[0] = l1_state.h.p;
benkatz 3:9ef9b4c66648 543 spi_data.q_knee[0] = l1_state.k.p;
indolcid 11:58a70b97c750 544 // spi_data.qd_abad[0] = l1_state.a.v;
benkatz 3:9ef9b4c66648 545 spi_data.qd_hip[0] = l1_state.h.v;
benkatz 3:9ef9b4c66648 546 spi_data.qd_knee[0] = l1_state.k.v;
benkatz 3:9ef9b4c66648 547
indolcid 11:58a70b97c750 548 // spi_data.q_abad[1] = l2_state.a.p;
benkatz 3:9ef9b4c66648 549 spi_data.q_hip[1] = l2_state.h.p;
benkatz 3:9ef9b4c66648 550 spi_data.q_knee[1] = l2_state.k.p;
indolcid 11:58a70b97c750 551 // spi_data.qd_abad[1] = l2_state.a.v;
benkatz 3:9ef9b4c66648 552 spi_data.qd_hip[1] = l2_state.h.v;
benkatz 3:9ef9b4c66648 553 spi_data.qd_knee[1] = l2_state.k.v;
benkatz 3:9ef9b4c66648 554
benkatz 3:9ef9b4c66648 555
benkatz 3:9ef9b4c66648 556
Ikhee 10:54e95428798b 557 // if(estop==0){
Ikhee 10:54e95428798b 558 // //printf("estopped!!!!\n\r");
Ikhee 10:54e95428798b 559 // memset(&l1_control, 0, sizeof(l1_control));
Ikhee 10:54e95428798b 560 // memset(&l2_control, 0, sizeof(l2_control));
Ikhee 10:54e95428798b 561 // spi_data.flags[0] = 0xdead;
Ikhee 10:54e95428798b 562 // spi_data.flags[1] = 0xdead;
Ikhee 10:54e95428798b 563 // led = 1;
Ikhee 10:54e95428798b 564 // }
Ikhee 10:54e95428798b 565 //
Ikhee 10:54e95428798b 566 // else{
benkatz 3:9ef9b4c66648 567 led = 0;
benkatz 3:9ef9b4c66648 568
benkatz 3:9ef9b4c66648 569 memset(&l1_control, 0, sizeof(l1_control));
benkatz 3:9ef9b4c66648 570 memset(&l2_control, 0, sizeof(l2_control));
indolcid 11:58a70b97c750 571
indolcid 11:58a70b97c750 572 // l1_control.a.p_des = spi_command.q_des_abad[0];
indolcid 11:58a70b97c750 573 // l1_control.a.v_des = spi_command.qd_des_abad[0];
indolcid 11:58a70b97c750 574 // l1_control.a.kp = spi_command.kp_abad[0];
indolcid 11:58a70b97c750 575 // l1_control.a.kd = spi_command.kd_abad[0];
indolcid 11:58a70b97c750 576 // l1_control.a.t_ff = spi_command.tau_abad_ff[0];
indolcid 11:58a70b97c750 577
benkatz 3:9ef9b4c66648 578 l1_control.h.p_des = spi_command.q_des_hip[0];
benkatz 3:9ef9b4c66648 579 l1_control.h.v_des = spi_command.qd_des_hip[0];
benkatz 3:9ef9b4c66648 580 l1_control.h.kp = spi_command.kp_hip[0];
benkatz 3:9ef9b4c66648 581 l1_control.h.kd = spi_command.kd_hip[0];
benkatz 3:9ef9b4c66648 582 l1_control.h.t_ff = spi_command.tau_hip_ff[0];
indolcid 11:58a70b97c750 583
benkatz 3:9ef9b4c66648 584 l1_control.k.p_des = spi_command.q_des_knee[0];
benkatz 3:9ef9b4c66648 585 l1_control.k.v_des = spi_command.qd_des_knee[0];
benkatz 3:9ef9b4c66648 586 l1_control.k.kp = spi_command.kp_knee[0];
benkatz 3:9ef9b4c66648 587 l1_control.k.kd = spi_command.kd_knee[0];
benkatz 3:9ef9b4c66648 588 l1_control.k.t_ff = spi_command.tau_knee_ff[0];
indolcid 11:58a70b97c750 589
indolcid 11:58a70b97c750 590 // l2_control.a.p_des = spi_command.q_des_abad[1];
indolcid 11:58a70b97c750 591 // l2_control.a.v_des = spi_command.qd_des_abad[1];
indolcid 11:58a70b97c750 592 // l2_control.a.kp = spi_command.kp_abad[1];
indolcid 11:58a70b97c750 593 // l2_control.a.kd = spi_command.kd_abad[1];
indolcid 11:58a70b97c750 594 // l2_control.a.t_ff = spi_command.tau_abad_ff[1];
indolcid 11:58a70b97c750 595
benkatz 3:9ef9b4c66648 596 l2_control.h.p_des = spi_command.q_des_hip[1];
benkatz 3:9ef9b4c66648 597 l2_control.h.v_des = spi_command.qd_des_hip[1];
benkatz 3:9ef9b4c66648 598 l2_control.h.kp = spi_command.kp_hip[1];
benkatz 3:9ef9b4c66648 599 l2_control.h.kd = spi_command.kd_hip[1];
benkatz 3:9ef9b4c66648 600 l2_control.h.t_ff = spi_command.tau_hip_ff[1];
indolcid 11:58a70b97c750 601
benkatz 3:9ef9b4c66648 602 l2_control.k.p_des = spi_command.q_des_knee[1];
benkatz 3:9ef9b4c66648 603 l2_control.k.v_des = spi_command.qd_des_knee[1];
benkatz 3:9ef9b4c66648 604 l2_control.k.kp = spi_command.kp_knee[1];
benkatz 3:9ef9b4c66648 605 l2_control.k.kd = spi_command.kd_knee[1];
benkatz 3:9ef9b4c66648 606 l2_control.k.t_ff = spi_command.tau_knee_ff[1];
indolcid 11:58a70b97c750 607
indolcid 11:58a70b97c750 608
benkatz 3:9ef9b4c66648 609 spi_data.flags[0] = 0;
benkatz 3:9ef9b4c66648 610 spi_data.flags[1] = 0;
indolcid 11:58a70b97c750 611 //spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
indolcid 11:58a70b97c750 612 //spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
benkatz 3:9ef9b4c66648 613 //spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2;
indolcid 11:58a70b97c750 614 //spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
indolcid 11:58a70b97c750 615 //spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
benkatz 3:9ef9b4c66648 616 //spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2;
benkatz 3:9ef9b4c66648 617
benkatz 3:9ef9b4c66648 618 //spi_data.flags[0] = 0xbeef;
benkatz 3:9ef9b4c66648 619 //spi_data.flags[1] = 0xbeef;
benkatz 3:9ef9b4c66648 620 //PackAll();
benkatz 3:9ef9b4c66648 621 //WriteAll();
Ikhee 10:54e95428798b 622 // }
benkatz 3:9ef9b4c66648 623 spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
benkatz 3:9ef9b4c66648 624 for(int i = 0; i < DATA_LEN; i++){
indolcid 11:58a70b97c750 625 tx_buff[i] = ((uint16_t*)(&spi_data))[i];
indolcid 11:58a70b97c750 626 }
benkatz 3:9ef9b4c66648 627
benkatz 3:9ef9b4c66648 628 }
benkatz 3:9ef9b4c66648 629
benkatz 3:9ef9b4c66648 630
benkatz 3:9ef9b4c66648 631 void test_control()
benkatz 3:9ef9b4c66648 632 {
benkatz 3:9ef9b4c66648 633 for(int i = 0; i < 2; i++)
benkatz 3:9ef9b4c66648 634 {
benkatz 3:9ef9b4c66648 635 spi_data.q_abad[i] = spi_command.q_des_abad[i] + 1.f;
benkatz 3:9ef9b4c66648 636 spi_data.q_knee[i] = spi_command.q_des_knee[i] + 1.f;
benkatz 3:9ef9b4c66648 637 spi_data.q_hip[i] = spi_command.q_des_hip[i] + 1.f;
benkatz 3:9ef9b4c66648 638
benkatz 3:9ef9b4c66648 639 spi_data.qd_abad[i] = spi_command.qd_des_abad[i] + 1.f;
benkatz 3:9ef9b4c66648 640 spi_data.qd_knee[i] = spi_command.qd_des_knee[i] + 1.f;
benkatz 3:9ef9b4c66648 641 spi_data.qd_hip[i] = spi_command.qd_des_hip[i] + 1.f;
benkatz 3:9ef9b4c66648 642 }
benkatz 3:9ef9b4c66648 643
benkatz 3:9ef9b4c66648 644 spi_data.flags[0] = 0xdead;
benkatz 3:9ef9b4c66648 645 //spi_data.flags[1] = 0xbeef;
benkatz 3:9ef9b4c66648 646
benkatz 3:9ef9b4c66648 647 // only do first 56 bytes of message.
benkatz 3:9ef9b4c66648 648 spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
benkatz 3:9ef9b4c66648 649
benkatz 3:9ef9b4c66648 650 for(int i = 0; i < DATA_LEN; i++)
benkatz 3:9ef9b4c66648 651 tx_buff[i] = ((uint16_t*)(&spi_data))[i];
benkatz 3:9ef9b4c66648 652 }
benkatz 3:9ef9b4c66648 653
benkatz 3:9ef9b4c66648 654 void init_spi(void){
benkatz 3:9ef9b4c66648 655 SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
benkatz 3:9ef9b4c66648 656 spi->format(16, 0);
benkatz 3:9ef9b4c66648 657 spi->frequency(12000000);
benkatz 3:9ef9b4c66648 658 spi->reply(0x0);
benkatz 3:9ef9b4c66648 659 cs.fall(&spi_isr);
benkatz 3:9ef9b4c66648 660 printf("done\n\r");
benkatz 3:9ef9b4c66648 661 }
indolcid 11:58a70b97c750 662
benkatz 3:9ef9b4c66648 663 int main() {
benkatz 9:56747e76c9c1 664 wait(1);
benkatz 3:9ef9b4c66648 665 //led = 1;
benkatz 3:9ef9b4c66648 666 pc.baud(921600);
benkatz 3:9ef9b4c66648 667 pc.attach(&serial_isr);
Ikhee 10:54e95428798b 668 // estop.mode(PullUp);
benkatz 3:9ef9b4c66648 669 //spi.format(16, 0);
benkatz 3:9ef9b4c66648 670 //spi.frequency(1000000);
benkatz 3:9ef9b4c66648 671 //spi.reply(0x0);
benkatz 3:9ef9b4c66648 672 //cs.fall(&spi_isr);
benkatz 3:9ef9b4c66648 673
benkatz 7:f10513577d4c 674 //can1.frequency(1000000); // set bit rate to 1Mbps
benkatz 3:9ef9b4c66648 675 //can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler
indolcid 11:58a70b97c750 676 can1.filter(0, 0x7F8, CANStandard, 0); //set up can filter
benkatz 7:f10513577d4c 677 //can2.frequency(1000000); // set bit rate to 1Mbps
benkatz 3:9ef9b4c66648 678 //can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler
indolcid 11:58a70b97c750 679 can2.filter(0, 0x7F8, CANStandard, 0); //set up can filter
benkatz 3:9ef9b4c66648 680
benkatz 3:9ef9b4c66648 681 memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
benkatz 3:9ef9b4c66648 682 memset(&spi_data, 0, sizeof(spi_data_t));
benkatz 3:9ef9b4c66648 683 memset(&spi_command,0,sizeof(spi_command_t));
benkatz 3:9ef9b4c66648 684
benkatz 3:9ef9b4c66648 685
benkatz 3:9ef9b4c66648 686 NVIC_SetPriority(TIM5_IRQn, 1);
benkatz 3:9ef9b4c66648 687 //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
benkatz 3:9ef9b4c66648 688 //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
benkatz 3:9ef9b4c66648 689
benkatz 3:9ef9b4c66648 690 printf("\n\r SPIne\n\r");
benkatz 3:9ef9b4c66648 691 //printf("%d\n\r", RX_ID << 18);
benkatz 3:9ef9b4c66648 692
benkatz 3:9ef9b4c66648 693 a1_can.len = 8; //transmit 8 bytes
benkatz 3:9ef9b4c66648 694 a2_can.len = 8; //transmit 8 bytes
benkatz 3:9ef9b4c66648 695 h1_can.len = 8;
benkatz 3:9ef9b4c66648 696 h2_can.len = 8;
benkatz 3:9ef9b4c66648 697 k1_can.len = 8;
benkatz 3:9ef9b4c66648 698 k2_can.len = 8;
benkatz 3:9ef9b4c66648 699 rxMsg1.len = 6; //receive 6 bytes
benkatz 3:9ef9b4c66648 700 rxMsg2.len = 6; //receive 6 bytes
benkatz 3:9ef9b4c66648 701
benkatz 3:9ef9b4c66648 702 a1_can.id = 0x1;
benkatz 3:9ef9b4c66648 703 a2_can.id = 0x1;
benkatz 3:9ef9b4c66648 704 h1_can.id = 0x2;
benkatz 3:9ef9b4c66648 705 h2_can.id = 0x2;
benkatz 3:9ef9b4c66648 706 k1_can.id = 0x3;
benkatz 3:9ef9b4c66648 707 k2_can.id = 0x3;
benkatz 3:9ef9b4c66648 708
indolcid 11:58a70b97c750 709 // pack_cmd(&a1_can, l1_control.a);
indolcid 11:58a70b97c750 710 // pack_cmd(&a2_can, l2_control.a);
benkatz 3:9ef9b4c66648 711 pack_cmd(&h1_can, l1_control.h);
benkatz 3:9ef9b4c66648 712 pack_cmd(&h2_can, l2_control.h);
indolcid 11:58a70b97c750 713 pack_cmd2(&k1_can, l1_control.k);
indolcid 11:58a70b97c750 714 pack_cmd2(&k2_can, l2_control.k);
benkatz 3:9ef9b4c66648 715 WriteAll();
benkatz 2:25837cbaee98 716
benkatz 2:25837cbaee98 717
benkatz 3:9ef9b4c66648 718 // SPI doesn't work if enabled while the CS pin is pulled low
benkatz 3:9ef9b4c66648 719 // Wait for CS to not be low, then enable SPI
benkatz 3:9ef9b4c66648 720 if(!spi_enabled){
benkatz 3:9ef9b4c66648 721 while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
benkatz 3:9ef9b4c66648 722 init_spi();
benkatz 3:9ef9b4c66648 723 spi_enabled = 1;
benkatz 3:9ef9b4c66648 724 }
benkatz 3:9ef9b4c66648 725
benkatz 3:9ef9b4c66648 726 while(1) {
benkatz 3:9ef9b4c66648 727 counter++;
benkatz 3:9ef9b4c66648 728 can2.read(rxMsg2);
benkatz 3:9ef9b4c66648 729 unpack_reply(rxMsg2, &l2_state);
benkatz 3:9ef9b4c66648 730 can1.read(rxMsg1); // read message into Rx message storage
benkatz 3:9ef9b4c66648 731 unpack_reply(rxMsg1, &l1_state);
benkatz 3:9ef9b4c66648 732 wait_us(10);
benkatz 2:25837cbaee98 733
indolcid 11:58a70b97c750 734 int32_t en_flag = spi_command.flags[0];
indolcid 11:58a70b97c750 735 pc.printf("%1X", (en_flag&0xF));
indolcid 11:58a70b97c750 736
Ikhee 10:54e95428798b 737 // counter++;
Ikhee 10:54e95428798b 738 // can1.read(rxMsg1);
Ikhee 10:54e95428798b 739 // unpack_reply(rxMsg1, &l1_state);
Ikhee 10:54e95428798b 740 // can2.read(rxMsg2); // read message into Rx message storage
Ikhee 10:54e95428798b 741 // unpack_reply(rxMsg2, &l2_state);
Ikhee 10:54e95428798b 742 // wait_us(10);
Ikhee 10:54e95428798b 743
benkatz 3:9ef9b4c66648 744 }
benkatz 2:25837cbaee98 745
benkatz 0:d6186b8990c5 746
benkatz 3:9ef9b4c66648 747
benkatz 0:d6186b8990c5 748
benkatz 0:d6186b8990c5 749 }
benkatz 0:d6186b8990c5 750
benkatz 0:d6186b8990c5 751
benkatz 0:d6186b8990c5 752
benkatz 0:d6186b8990c5 753
benkatz 0:d6186b8990c5 754