Shao Rui / SPIne_referenceTraj_DEMO

1

Dependencies:   mbed-dev_spine

main.cpp@6:3e6d09f56278, 2019-11-19 (annotated)

Committer:
Rushu
Date:
Tue Nov 19 09:00:12 2019 +0000
Revision:
6:3e6d09f56278
Parent:
5:4aa2a7fa7818
Child:
7:e3cff4376669
add 200ms reference trajectory divided.

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"
Rushu 5:4aa2a7fa7818 7 #include "referenceTraj.h"
Rushu 6:3e6d09f56278 8 #include "FastMath.h" //hjb added
Rushu 6:3e6d09f56278 9 //#include "rtos.h" //hjb added
Rushu 6:3e6d09f56278 10
Rushu 6:3e6d09f56278 11 //Thread thread; //hjb added
benkatz 3:9ef9b4c66648 12 // length of receive/transmit buffers
benkatz 3:9ef9b4c66648 13 #define RX_LEN 66
benkatz 3:9ef9b4c66648 14 #define TX_LEN 66
benkatz 0:d6186b8990c5 15
benkatz 3:9ef9b4c66648 16 // length of outgoing/incoming messages
benkatz 3:9ef9b4c66648 17 #define DATA_LEN 30
benkatz 3:9ef9b4c66648 18 #define CMD_LEN 66
benkatz 2:25837cbaee98 19
benkatz 3:9ef9b4c66648 20 // Master CAN ID ///
benkatz 3:9ef9b4c66648 21 #define CAN_ID 0x0
benkatz 2:25837cbaee98 22
benkatz 0:d6186b8990c5 23
benkatz 0:d6186b8990c5 24 /// Value Limits ///
benkatz 0:d6186b8990c5 25 #define P_MIN -12.5f
benkatz 0:d6186b8990c5 26 #define P_MAX 12.5f
benkatz 3:9ef9b4c66648 27 #define V_MIN -45.0f
benkatz 3:9ef9b4c66648 28 #define V_MAX 45.0f
benkatz 0:d6186b8990c5 29 #define KP_MIN 0.0f
benkatz 0:d6186b8990c5 30 #define KP_MAX 500.0f
benkatz 0:d6186b8990c5 31 #define KD_MIN 0.0f
benkatz 0:d6186b8990c5 32 #define KD_MAX 5.0f
Rushu 5:4aa2a7fa7818 33 //#define T_MIN -18.0f
Rushu 5:4aa2a7fa7818 34 //#define T_MAX 18.0f
Rushu 5:4aa2a7fa7818 35
Rushu 6:3e6d09f56278 36 #define T_MIN -3.0f //hjb changed test
Rushu 6:3e6d09f56278 37 #define T_MAX 3.0f
Rushu 5:4aa2a7fa7818 38 #define SPI_TIMEOUT 1000 // hjb added for spi time out
benkatz 0:d6186b8990c5 39
benkatz 3:9ef9b4c66648 40 /// Joint Soft Stops ///
Rushu 5:4aa2a7fa7818 41 #define A_LIM_P 5.0f//1.5f
Rushu 5:4aa2a7fa7818 42 #define A_LIM_N -5.0f//-1.5f
benkatz 3:9ef9b4c66648 43 #define H_LIM_P 5.0f
benkatz 3:9ef9b4c66648 44 #define H_LIM_N -5.0f
benkatz 3:9ef9b4c66648 45 #define K_LIM_P 0.2f
benkatz 3:9ef9b4c66648 46 #define K_LIM_N 7.7f
Rushu 5:4aa2a7fa7818 47 #define KP_SOFTSTOP 0.0f//100.0f
benkatz 3:9ef9b4c66648 48 #define KD_SOFTSTOP 0.4f;
benkatz 3:9ef9b4c66648 49
benkatz 3:9ef9b4c66648 50 #define ENABLE_CMD 0xFFFF
benkatz 3:9ef9b4c66648 51 #define DISABLE_CMD 0x1F1F
benkatz 3:9ef9b4c66648 52
benkatz 3:9ef9b4c66648 53 spi_data_t spi_data; // data from spine to up
benkatz 3:9ef9b4c66648 54 spi_command_t spi_command; // data from up to spine
Rushu 5:4aa2a7fa7818 55 spi_command_t *mspi_command=&spi_command; // data from up to spine
benkatz 3:9ef9b4c66648 56
benkatz 3:9ef9b4c66648 57 // spi buffers
benkatz 3:9ef9b4c66648 58 uint16_t rx_buff[RX_LEN];
benkatz 3:9ef9b4c66648 59 uint16_t tx_buff[TX_LEN];
benkatz 3:9ef9b4c66648 60
benkatz 3:9ef9b4c66648 61 DigitalOut led(PC_5);
benkatz 3:9ef9b4c66648 62
benkatz 3:9ef9b4c66648 63
benkatz 3:9ef9b4c66648 64 Serial pc(PA_2, PA_3);
benkatz 3:9ef9b4c66648 65 CAN can1(PB_12, PB_13); // CAN Rx pin name, CAN Tx pin name
benkatz 3:9ef9b4c66648 66 CAN can2(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 3:9ef9b4c66648 67
benkatz 3:9ef9b4c66648 68 CANMessage rxMsg1, rxMsg2;
benkatz 3:9ef9b4c66648 69 CANMessage txMsg1, txMsg2;
benkatz 3:9ef9b4c66648 70 CANMessage a1_can, a2_can, h1_can, h2_can, k1_can, k2_can; //TX Messages
benkatz 3:9ef9b4c66648 71 int ledState;
benkatz 3:9ef9b4c66648 72 Ticker sendCAN;
Rushu 6:3e6d09f56278 73 volatile int counter = 0;
benkatz 3:9ef9b4c66648 74 volatile bool msgAvailable = false;
benkatz 3:9ef9b4c66648 75 Ticker loop;
benkatz 3:9ef9b4c66648 76
benkatz 3:9ef9b4c66648 77 int spi_enabled = 0;
benkatz 3:9ef9b4c66648 78 InterruptIn cs(PA_4);
Rushu 5:4aa2a7fa7818 79 //**//DigitalIn estop(PA_14);
Rushu 5:4aa2a7fa7818 80 DigitalIn estop(PB_15);
benkatz 3:9ef9b4c66648 81 //SPISlave spi(PA_7, PA_6, PA_5, PA_4);
benkatz 3:9ef9b4c66648 82
benkatz 3:9ef9b4c66648 83
benkatz 3:9ef9b4c66648 84 leg_state l1_state, l2_state;;
benkatz 3:9ef9b4c66648 85 leg_control l1_control, l2_control;
benkatz 3:9ef9b4c66648 86
benkatz 3:9ef9b4c66648 87 uint16_t x = 0;
benkatz 3:9ef9b4c66648 88 uint16_t x2 = 0;
benkatz 3:9ef9b4c66648 89 uint16_t count = 0;
benkatz 3:9ef9b4c66648 90 uint16_t counter2 = 0;
benkatz 3:9ef9b4c66648 91
benkatz 3:9ef9b4c66648 92 int control_mode = 1;
benkatz 3:9ef9b4c66648 93 int is_standing = 0;
benkatz 3:9ef9b4c66648 94 int enabled = 0;
benkatz 3:9ef9b4c66648 95
benkatz 3:9ef9b4c66648 96 // generates fake spi data from spi command
benkatz 3:9ef9b4c66648 97 void test_control();
benkatz 3:9ef9b4c66648 98 void control();
benkatz 3:9ef9b4c66648 99
Rushu 5:4aa2a7fa7818 100 //==========hjb=======//=================================================================================//
Rushu 5:4aa2a7fa7818 101 ///////////////////////////////////////////////////////
Rushu 5:4aa2a7fa7818 102 #define MULTI200 1
Rushu 5:4aa2a7fa7818 103 #define MAX_TRACK 10240*5
Rushu 5:4aa2a7fa7818 104 #define MIN_SAMPLE 0.001
Rushu 5:4aa2a7fa7818 105 float TimeSample, TimeSample2;
Rushu 5:4aa2a7fa7818 106 #define TS (MULTI200 * MIN_SAMPLE) // Sample Time
Rushu 5:4aa2a7fa7818 107 #define SP 0.2 // Interval
Rushu 5:4aa2a7fa7818 108
Rushu 5:4aa2a7fa7818 109 float Fz_mea;//2.7
Rushu 5:4aa2a7fa7818 110 float trajRef[5][9], trajRef2[5][9]; // ref. Traj. 0..2 -> q_d, 3..5-> v_d, 6..8->a_d.
Rushu 5:4aa2a7fa7818 111 REF ref[5];
Rushu 5:4aa2a7fa7818 112 ///////////////////////////////////////////////////////
Rushu 5:4aa2a7fa7818 113 //++++++++ FOURARM +++++++++++++++++++++++//
Rushu 5:4aa2a7fa7818 114 float g_ArmSpeed = 250;//2.50f; //控制关节速度
Rushu 5:4aa2a7fa7818 115 float g_ArmJointAngleRe[5];
Rushu 5:4aa2a7fa7818 116 float g_ArmJointAngleDe[5];
Rushu 5:4aa2a7fa7818 117 float g_ArmJointAngleLineDe[5];
Rushu 5:4aa2a7fa7818 118 float g_ArmJointAngleDe_Past[5] = {0,0,0,0,0};
Rushu 5:4aa2a7fa7818 119 float g_ArmJointAngleDe_PastP[5] = {0,0,0,0,0};
Rushu 5:4aa2a7fa7818 120 float g_ArmTrajRef[15];
Rushu 5:4aa2a7fa7818 121 float g_ArmTrajLineRef[15];
Rushu 5:4aa2a7fa7818 122 int g_ArmInit=TRUE;
Rushu 5:4aa2a7fa7818 123 static int Control_Flag = 0;
Rushu 5:4aa2a7fa7818 124 static int timeout = 0;
Rushu 5:4aa2a7fa7818 125
Rushu 5:4aa2a7fa7818 126 //++++++++ FOURARM +++++++++++++//
Rushu 5:4aa2a7fa7818 127 ///////////////////////////////////////////////////////
Rushu 5:4aa2a7fa7818 128 //++++++++ FOURARM +++++++++++++++++++++++//
Rushu 5:4aa2a7fa7818 129 #define ARMSP 0.200 // Interval
Rushu 5:4aa2a7fa7818 130 #define ARMJOINTSPEED 2.5f //关节速度定义 2.5f
Rushu 5:4aa2a7fa7818 131 #define ARMJOINTACC 0.100f //关节加速度时间
Rushu 5:4aa2a7fa7818 132
Rushu 5:4aa2a7fa7818 133 //++++++++ FOURARM +++++++++++++++++++++++//
Rushu 5:4aa2a7fa7818 134 ARMREF g_ArmRef;
Rushu 5:4aa2a7fa7818 135 //++++++++ FOURARM +++++++++++++//
Rushu 6:3e6d09f56278 136 //================HJB=============added==========
Rushu 6:3e6d09f56278 137 using namespace FastMath;
Rushu 6:3e6d09f56278 138 volatile float Init_pos = 0;
Rushu 6:3e6d09f56278 139 volatile float Pmag = 1;
Rushu 6:3e6d09f56278 140 volatile float Pmag_last = 1;
Rushu 6:3e6d09f56278 141 volatile float Tperiod = 25;
Rushu 6:3e6d09f56278 142 volatile float p_des_HJB=0;
Rushu 6:3e6d09f56278 143 volatile float v_des_HJB=0;
Rushu 6:3e6d09f56278 144 volatile int count_HJB = 0;
Rushu 6:3e6d09f56278 145 //===================HJB end===================
Rushu 6:3e6d09f56278 146
Rushu 5:4aa2a7fa7818 147
Rushu 5:4aa2a7fa7818 148 //==========hjb=======//=================================================================================//
Rushu 5:4aa2a7fa7818 149
Rushu 5:4aa2a7fa7818 150
Rushu 5:4aa2a7fa7818 151
benkatz 3:9ef9b4c66648 152
benkatz 0:d6186b8990c5 153 /// CAN Command Packet Structure ///
benkatz 0:d6186b8990c5 154 /// 16 bit position command, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 155 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 156 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 0:d6186b8990c5 157 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 0:d6186b8990c5 158 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 0:d6186b8990c5 159 /// CAN Packet is 8 8-bit words
benkatz 0:d6186b8990c5 160 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 161 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 162 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 163 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 164 /// 3: [velocity[3-0], kp[11-8]]
benkatz 0:d6186b8990c5 165 /// 4: [kp[7-0]]
benkatz 0:d6186b8990c5 166 /// 5: [kd[11-4]]
benkatz 0:d6186b8990c5 167 /// 6: [kd[3-0], torque[11-8]]
benkatz 0:d6186b8990c5 168 /// 7: [torque[7-0]]
benkatz 0:d6186b8990c5 169
benkatz 3:9ef9b4c66648 170 void pack_cmd(CANMessage * msg, joint_control joint){
Rushu 6:3e6d09f56278 171
benkatz 0:d6186b8990c5 172 /// limit data to be within bounds ///
Rushu 5:4aa2a7fa7818 173 //float p_des = fminf(fmaxf(P_MIN, uint_to_float(joint.p_des, P_MIN, P_MAX, 16)), P_MAX);
Rushu 5:4aa2a7fa7818 174 //float v_des = fminf(fmaxf(V_MIN, uint_to_float(joint.v_des, V_MIN, V_MAX, 12)), V_MAX);
Rushu 6:3e6d09f56278 175 //float p_des = fminf(fmaxf(P_MIN, p_des_HJB), P_MAX);
Rushu 6:3e6d09f56278 176 //float v_des = fminf(fmaxf(V_MIN, v_des_HJB), V_MAX);
benkatz 3:9ef9b4c66648 177 float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);
benkatz 3:9ef9b4c66648 178 float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
Rushu 5:4aa2a7fa7818 179 float kp = fminf(fmaxf(KP_MIN, uint_to_float(joint.kp, KP_MIN, KP_MAX, 12)), KP_MAX);
Rushu 5:4aa2a7fa7818 180 float kd = fminf(fmaxf(KD_MIN, uint_to_float(joint.kd, KD_MIN, KD_MAX, 12)), KD_MAX);
Rushu 5:4aa2a7fa7818 181 float t_ff = fminf(fmaxf(T_MIN, uint_to_float(joint.t_ff, T_MIN, T_MAX, 12)), T_MAX);
benkatz 0:d6186b8990c5 182 /// convert floats to unsigned ints ///
benkatz 3:9ef9b4c66648 183 uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
benkatz 3:9ef9b4c66648 184 uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
benkatz 3:9ef9b4c66648 185 uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
benkatz 3:9ef9b4c66648 186 uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
benkatz 3:9ef9b4c66648 187 uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
benkatz 0:d6186b8990c5 188 /// pack ints into the can buffer ///
benkatz 0:d6186b8990c5 189 msg->data[0] = p_int>>8;
benkatz 0:d6186b8990c5 190 msg->data[1] = p_int&0xFF;
benkatz 0:d6186b8990c5 191 msg->data[2] = v_int>>4;
benkatz 0:d6186b8990c5 192 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
benkatz 0:d6186b8990c5 193 msg->data[4] = kp_int&0xFF;
benkatz 0:d6186b8990c5 194 msg->data[5] = kd_int>>4;
benkatz 0:d6186b8990c5 195 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
benkatz 0:d6186b8990c5 196 msg->data[7] = t_int&0xff;
Rushu 5:4aa2a7fa7818 197 ///printf("p:%d v:%d kp:%d kd:%d t:%d ",(uint16_t)p_int, (uint16_t)v_int, kp_int, kd_int, t_int);
Rushu 5:4aa2a7fa7818 198
benkatz 0:d6186b8990c5 199 }
benkatz 0:d6186b8990c5 200
benkatz 0:d6186b8990c5 201 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 202 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 203 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 204 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 205 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 206 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 207 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 208 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 209 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 210 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 211 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 212
benkatz 3:9ef9b4c66648 213 void unpack_reply(CANMessage msg, leg_state * leg){
benkatz 0:d6186b8990c5 214 /// unpack ints from can buffer ///
benkatz 3:9ef9b4c66648 215 uint16_t id = msg.data[0];
benkatz 3:9ef9b4c66648 216 uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
benkatz 3:9ef9b4c66648 217 uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
benkatz 3:9ef9b4c66648 218 uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
benkatz 3:9ef9b4c66648 219 /// convert uints to floats ///
benkatz 0:d6186b8990c5 220 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 221 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 1:79e0d4791936 222 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
benkatz 3:9ef9b4c66648 223
benkatz 3:9ef9b4c66648 224 if(id==1){
benkatz 3:9ef9b4c66648 225 leg->a.p = p;
benkatz 3:9ef9b4c66648 226 leg->a.v = v;
benkatz 3:9ef9b4c66648 227 leg->a.t = t;
Rushu 5:4aa2a7fa7818 228 //printf("p=%.3f v=%.3f i=%.3f", p, v, t);
benkatz 2:25837cbaee98 229 }
benkatz 3:9ef9b4c66648 230 else if(id==2){
benkatz 3:9ef9b4c66648 231 leg->h.p = p;
benkatz 3:9ef9b4c66648 232 leg->h.v = v;
benkatz 3:9ef9b4c66648 233 leg->h.t = t;
benkatz 2:25837cbaee98 234 }
benkatz 3:9ef9b4c66648 235 else if(id==3){
benkatz 3:9ef9b4c66648 236 leg->k.p = p;
benkatz 3:9ef9b4c66648 237 leg->k.v = v;
benkatz 3:9ef9b4c66648 238 leg->k.t = t;
benkatz 0:d6186b8990c5 239 }
Rushu 5:4aa2a7fa7818 240
benkatz 0:d6186b8990c5 241 }
benkatz 3:9ef9b4c66648 242
benkatz 1:79e0d4791936 243 void rxISR1() {
benkatz 1:79e0d4791936 244 can1.read(rxMsg1); // read message into Rx message storage
benkatz 3:9ef9b4c66648 245 unpack_reply(rxMsg1, &l1_state);
benkatz 0:d6186b8990c5 246 }
benkatz 1:79e0d4791936 247 void rxISR2(){
benkatz 1:79e0d4791936 248 can2.read(rxMsg2);
benkatz 3:9ef9b4c66648 249 unpack_reply(rxMsg2, &l2_state);
benkatz 1:79e0d4791936 250 }
benkatz 3:9ef9b4c66648 251 void PackAll(){
benkatz 3:9ef9b4c66648 252 pack_cmd(&a1_can, l1_control.a);
benkatz 3:9ef9b4c66648 253 pack_cmd(&a2_can, l2_control.a);
benkatz 3:9ef9b4c66648 254 pack_cmd(&h1_can, l1_control.h);
benkatz 3:9ef9b4c66648 255 pack_cmd(&h2_can, l2_control.h);
benkatz 3:9ef9b4c66648 256 pack_cmd(&k1_can, l1_control.k);
benkatz 3:9ef9b4c66648 257 pack_cmd(&k2_can, l2_control.k);
benkatz 3:9ef9b4c66648 258
benkatz 3:9ef9b4c66648 259 }
benkatz 1:79e0d4791936 260 void WriteAll(){
benkatz 2:25837cbaee98 261 //toggle = 1;
benkatz 3:9ef9b4c66648 262 can1.write(a1_can);
Rushu 5:4aa2a7fa7818 263 wait(.0001);
benkatz 3:9ef9b4c66648 264 can2.write(a2_can);
Rushu 6:3e6d09f56278 265 wait(.0002);
benkatz 3:9ef9b4c66648 266 can1.write(h1_can);
Rushu 5:4aa2a7fa7818 267 wait(.0001);
benkatz 3:9ef9b4c66648 268 can2.write(h2_can);
Rushu 6:3e6d09f56278 269 wait(.0002);
Rushu 6:3e6d09f56278 270
benkatz 3:9ef9b4c66648 271 can1.write(k1_can);
Rushu 5:4aa2a7fa7818 272 wait(.0001);
benkatz 3:9ef9b4c66648 273 can2.write(k2_can);
Rushu 6:3e6d09f56278 274 //wait(.0001);
Rushu 6:3e6d09f56278 275
benkatz 2:25837cbaee98 276 //toggle = 0;
benkatz 1:79e0d4791936 277 }
benkatz 0:d6186b8990c5 278
benkatz 0:d6186b8990c5 279 void sendCMD(){
Rushu 5:4aa2a7fa7818 280 Control_Flag = 1;
Rushu 5:4aa2a7fa7818 281 //=========================hjb ==============================================================//
Rushu 5:4aa2a7fa7818 282 //#define PI 3.1415926f
Rushu 5:4aa2a7fa7818 283 //static int init = TRUE;
Rushu 5:4aa2a7fa7818 284 //static double Tacc[3]={0.060, 0.060, 0.060}; // acceleration time 40ms, 60, 60 40Deg/S
Rushu 5:4aa2a7fa7818 285 //static float MAX_STEP[3]={12.0, 12.0, 12.0}; // max step limitation 12 degree
Rushu 5:4aa2a7fa7818 286 //control();
benkatz 1:79e0d4791936 287
Rushu 5:4aa2a7fa7818 288
benkatz 3:9ef9b4c66648 289 WriteAll();
benkatz 2:25837cbaee98 290
Rushu 5:4aa2a7fa7818 291
benkatz 3:9ef9b4c66648 292 }
benkatz 2:25837cbaee98 293
benkatz 3:9ef9b4c66648 294
benkatz 3:9ef9b4c66648 295
benkatz 0:d6186b8990c5 296
benkatz 1:79e0d4791936 297 void Zero(CANMessage * msg){
benkatz 1:79e0d4791936 298 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 299 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 300 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 301 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 302 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 303 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 304 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 305 msg->data[7] = 0xFE;
benkatz 1:79e0d4791936 306 WriteAll();
benkatz 1:79e0d4791936 307 }
benkatz 1:79e0d4791936 308
benkatz 1:79e0d4791936 309 void EnterMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 310 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 311 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 312 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 313 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 314 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 315 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 316 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 317 msg->data[7] = 0xFC;
benkatz 3:9ef9b4c66648 318 //WriteAll();
benkatz 1:79e0d4791936 319 }
benkatz 1:79e0d4791936 320
benkatz 1:79e0d4791936 321 void ExitMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 322 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 323 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 324 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 325 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 326 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 327 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 328 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 329 msg->data[7] = 0xFD;
benkatz 3:9ef9b4c66648 330 //WriteAll();
benkatz 1:79e0d4791936 331 }
benkatz 0:d6186b8990c5 332 void serial_isr(){
benkatz 1:79e0d4791936 333 /// handle keyboard commands from the serial terminal ///
benkatz 0:d6186b8990c5 334 while(pc.readable()){
benkatz 0:d6186b8990c5 335 char c = pc.getc();
Rushu 5:4aa2a7fa7818 336 led = !led;
benkatz 0:d6186b8990c5 337 switch(c){
benkatz 0:d6186b8990c5 338 case(27):
benkatz 3:9ef9b4c66648 339 //loop.detach();
benkatz 0:d6186b8990c5 340 printf("\n\r exiting motor mode \n\r");
benkatz 3:9ef9b4c66648 341 ExitMotorMode(&a1_can);
benkatz 3:9ef9b4c66648 342 ExitMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 343 ExitMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 344 ExitMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 345 ExitMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 346 ExitMotorMode(&k2_can);
benkatz 1:79e0d4791936 347 enabled = 0;
benkatz 0:d6186b8990c5 348 break;
benkatz 0:d6186b8990c5 349 case('m'):
benkatz 0:d6186b8990c5 350 printf("\n\r entering motor mode \n\r");
benkatz 3:9ef9b4c66648 351 EnterMotorMode(&a1_can);
benkatz 3:9ef9b4c66648 352 EnterMotorMode(&a2_can);
benkatz 3:9ef9b4c66648 353 EnterMotorMode(&h1_can);
benkatz 3:9ef9b4c66648 354 EnterMotorMode(&h2_can);
benkatz 3:9ef9b4c66648 355 EnterMotorMode(&k1_can);
benkatz 3:9ef9b4c66648 356 EnterMotorMode(&k2_can);
Rushu 6:3e6d09f56278 357 //WriteAll(); //hjb added
Rushu 6:3e6d09f56278 358 //wait(.5); //hjb delete
benkatz 1:79e0d4791936 359 enabled = 1;
Rushu 6:3e6d09f56278 360
benkatz 3:9ef9b4c66648 361 break;
benkatz 3:9ef9b4c66648 362 case('s'):
benkatz 3:9ef9b4c66648 363 printf("\n\r standing \n\r");
benkatz 3:9ef9b4c66648 364 counter2 = 0;
benkatz 3:9ef9b4c66648 365 is_standing = 1;
benkatz 3:9ef9b4c66648 366 //stand();
benkatz 0:d6186b8990c5 367 break;
benkatz 0:d6186b8990c5 368 case('z'):
benkatz 0:d6186b8990c5 369 printf("\n\r zeroing \n\r");
benkatz 3:9ef9b4c66648 370 Zero(&a1_can);
benkatz 3:9ef9b4c66648 371 Zero(&a2_can);
benkatz 3:9ef9b4c66648 372 Zero(&h1_can);
benkatz 3:9ef9b4c66648 373 Zero(&h2_can);
benkatz 3:9ef9b4c66648 374 Zero(&k1_can);
benkatz 3:9ef9b4c66648 375 Zero(&k2_can);
benkatz 2:25837cbaee98 376 break;
benkatz 0:d6186b8990c5 377 }
benkatz 0:d6186b8990c5 378 }
benkatz 1:79e0d4791936 379 WriteAll();
benkatz 0:d6186b8990c5 380
Rushu 6:3e6d09f56278 381
benkatz 0:d6186b8990c5 382 }
benkatz 0:d6186b8990c5 383
Rushu 5:4aa2a7fa7818 384 uint16_t xor_checksum(uint16_t* data, size_t len)
benkatz 3:9ef9b4c66648 385 {
Rushu 5:4aa2a7fa7818 386 uint16_t t = 0;
benkatz 3:9ef9b4c66648 387 for(int i = 0; i < len; i++)
benkatz 3:9ef9b4c66648 388 t = t ^ data[i];
benkatz 3:9ef9b4c66648 389 return t;
benkatz 3:9ef9b4c66648 390 }
benkatz 3:9ef9b4c66648 391
benkatz 3:9ef9b4c66648 392 void spi_isr(void)
benkatz 3:9ef9b4c66648 393 {
Rushu 6:3e6d09f56278 394 //led = !led; // HJB added
Rushu 5:4aa2a7fa7818 395 timeout = 0;
benkatz 3:9ef9b4c66648 396 GPIOC->ODR |= (1 << 8);
benkatz 3:9ef9b4c66648 397 GPIOC->ODR &= ~(1 << 8);
benkatz 3:9ef9b4c66648 398 int bytecount = 0;
Rushu 5:4aa2a7fa7818 399 // tx_buff[0]=0x1111; //hjb added
benkatz 3:9ef9b4c66648 400 SPI1->DR = tx_buff[0];
benkatz 3:9ef9b4c66648 401 while(cs == 0) {
benkatz 3:9ef9b4c66648 402 if(SPI1->SR&0x1) {
benkatz 3:9ef9b4c66648 403 rx_buff[bytecount] = SPI1->DR;
benkatz 3:9ef9b4c66648 404 bytecount++;
benkatz 3:9ef9b4c66648 405 if(bytecount<TX_LEN) {
benkatz 3:9ef9b4c66648 406 SPI1->DR = tx_buff[bytecount];
benkatz 3:9ef9b4c66648 407 }
benkatz 3:9ef9b4c66648 408 }
benkatz 3:9ef9b4c66648 409
benkatz 3:9ef9b4c66648 410 }
benkatz 2:25837cbaee98 411
benkatz 3:9ef9b4c66648 412 // after reading, save into spi_command
benkatz 3:9ef9b4c66648 413 // should probably check checksum first!
Rushu 5:4aa2a7fa7818 414 //uint16_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32); //===hjb del ====
Rushu 5:4aa2a7fa7818 415 uint16_t calc_checksum = xor_checksum(rx_buff,32);
Rushu 5:4aa2a7fa7818 416 // for(int i = 0; i < CMD_LEN; i++)
Rushu 5:4aa2a7fa7818 417 // {
Rushu 5:4aa2a7fa7818 418 // ((uint16_t*)(&spi_command))[i] = rx_buff[i];
Rushu 5:4aa2a7fa7818 419 //}
Rushu 5:4aa2a7fa7818 420 spi_command.q_des_abad[0] = rx_buff[0];
Rushu 5:4aa2a7fa7818 421 spi_command.q_des_hip[0] = rx_buff[2];
Rushu 5:4aa2a7fa7818 422 spi_command.q_des_knee[0] = rx_buff[4];
Rushu 5:4aa2a7fa7818 423
Rushu 5:4aa2a7fa7818 424 spi_command.qd_des_abad[0] = rx_buff[6];
Rushu 5:4aa2a7fa7818 425 spi_command.qd_des_hip[0] = rx_buff[8];
Rushu 5:4aa2a7fa7818 426 spi_command.qd_des_knee[0] = rx_buff[10];
Rushu 5:4aa2a7fa7818 427
Rushu 5:4aa2a7fa7818 428 spi_command.kp_abad[0] = rx_buff[12];
Rushu 5:4aa2a7fa7818 429 spi_command.kp_hip[0] = rx_buff[14];
Rushu 5:4aa2a7fa7818 430 spi_command.kp_knee[0] = rx_buff[16];
Rushu 5:4aa2a7fa7818 431
Rushu 5:4aa2a7fa7818 432 spi_command.kd_abad[0] = rx_buff[18];
Rushu 5:4aa2a7fa7818 433 spi_command.kd_hip[0] = rx_buff[20];
Rushu 5:4aa2a7fa7818 434 spi_command.kd_knee[0] = rx_buff[22];
Rushu 5:4aa2a7fa7818 435
Rushu 5:4aa2a7fa7818 436 spi_command.tau_abad_ff[0] = rx_buff[24];
Rushu 5:4aa2a7fa7818 437 spi_command.tau_hip_ff[0] = rx_buff[26];
Rushu 5:4aa2a7fa7818 438 spi_command.tau_knee_ff[0] = rx_buff[28];
Rushu 5:4aa2a7fa7818 439
Rushu 5:4aa2a7fa7818 440 spi_command.flags[0] = rx_buff[30];
Rushu 5:4aa2a7fa7818 441
Rushu 5:4aa2a7fa7818 442 spi_command.q_des_abad[1] = rx_buff[1];
Rushu 5:4aa2a7fa7818 443 spi_command.q_des_hip[1] = rx_buff[3];
Rushu 5:4aa2a7fa7818 444 spi_command.q_des_knee[1] = rx_buff[5];
Rushu 5:4aa2a7fa7818 445
Rushu 5:4aa2a7fa7818 446 spi_command.qd_des_abad[1] = rx_buff[7];
Rushu 5:4aa2a7fa7818 447 spi_command.qd_des_hip[1] = rx_buff[9];
Rushu 5:4aa2a7fa7818 448 spi_command.qd_des_knee[1] = rx_buff[11];
Rushu 5:4aa2a7fa7818 449
Rushu 5:4aa2a7fa7818 450 spi_command.kp_abad[1] = rx_buff[13];
Rushu 5:4aa2a7fa7818 451 spi_command.kp_hip[1] = rx_buff[15];
Rushu 5:4aa2a7fa7818 452 spi_command.kp_knee[1] = rx_buff[17];
Rushu 5:4aa2a7fa7818 453
Rushu 5:4aa2a7fa7818 454 spi_command.kd_abad[1] = rx_buff[19];
Rushu 5:4aa2a7fa7818 455 spi_command.kd_hip[1] = rx_buff[21];
Rushu 5:4aa2a7fa7818 456 spi_command.kd_knee[1] = rx_buff[23];
Rushu 5:4aa2a7fa7818 457
Rushu 5:4aa2a7fa7818 458 spi_command.tau_abad_ff[1] = rx_buff[25];
Rushu 5:4aa2a7fa7818 459 spi_command.tau_hip_ff[1] = rx_buff[27];
Rushu 5:4aa2a7fa7818 460 spi_command.tau_knee_ff[1] = rx_buff[29];
Rushu 5:4aa2a7fa7818 461
Rushu 5:4aa2a7fa7818 462 spi_command.flags[1] = rx_buff[31];
Rushu 5:4aa2a7fa7818 463
Rushu 5:4aa2a7fa7818 464 spi_command.checksum = rx_buff[32];
benkatz 3:9ef9b4c66648 465 // run control, which fills in tx_buff for the next iteration
benkatz 3:9ef9b4c66648 466 if(calc_checksum != spi_command.checksum){
benkatz 3:9ef9b4c66648 467 spi_data.flags[1] = 0xdead;}
benkatz 3:9ef9b4c66648 468
benkatz 3:9ef9b4c66648 469 //test_control();
Rushu 5:4aa2a7fa7818 470 //spi_data.q_abad[0] = 13.0f;
Rushu 5:4aa2a7fa7818 471 //control();
Rushu 5:4aa2a7fa7818 472 //PackAll(); //hjb cancel
Rushu 5:4aa2a7fa7818 473 //WriteAll(); //hjb cancel
Rushu 5:4aa2a7fa7818 474 //sendCMD(); //hjb added
benkatz 2:25837cbaee98 475
Rushu 5:4aa2a7fa7818 476 //for (int i=0; i<TX_LEN; i++) {
Rushu 5:4aa2a7fa7818 477 // printf("%d ", rx_buff[i]);
Rushu 5:4aa2a7fa7818 478 // printf("R= %.3f",*(&spi_command)[i]);
Rushu 5:4aa2a7fa7818 479 // }
benkatz 3:9ef9b4c66648 480 //printf("\n\r");
Rushu 5:4aa2a7fa7818 481
benkatz 3:9ef9b4c66648 482 }
benkatz 3:9ef9b4c66648 483
Rushu 5:4aa2a7fa7818 484 int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){ //**// exceed limit edge, using the impedance control
benkatz 3:9ef9b4c66648 485 if((state.p)>=limit_p){
benkatz 3:9ef9b4c66648 486 //control->p_des = limit_p;
benkatz 3:9ef9b4c66648 487 control->v_des = 0.0f;
benkatz 3:9ef9b4c66648 488 control->kp = 0;
benkatz 3:9ef9b4c66648 489 control->kd = KD_SOFTSTOP;
benkatz 3:9ef9b4c66648 490 control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
Rushu 5:4aa2a7fa7818 491 //control->t_ff = KP_SOFTSTOP*(limit_p - state.p); //hjb changed
benkatz 3:9ef9b4c66648 492 return 1;
benkatz 3:9ef9b4c66648 493 }
benkatz 3:9ef9b4c66648 494 else if((state.p)<=limit_n){
benkatz 3:9ef9b4c66648 495 //control->p_des = limit_n;
benkatz 3:9ef9b4c66648 496 control->v_des = 0.0f;
benkatz 3:9ef9b4c66648 497 control->kp = 0;
benkatz 3:9ef9b4c66648 498 control->kd = KD_SOFTSTOP;
benkatz 3:9ef9b4c66648 499 control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
Rushu 5:4aa2a7fa7818 500 //control->t_ff = KP_SOFTSTOP*(limit_n - state.p); //hjb changed
benkatz 3:9ef9b4c66648 501 return 1;
benkatz 3:9ef9b4c66648 502 }
benkatz 3:9ef9b4c66648 503 return 0;
benkatz 3:9ef9b4c66648 504
benkatz 3:9ef9b4c66648 505 }
benkatz 3:9ef9b4c66648 506
benkatz 3:9ef9b4c66648 507
benkatz 3:9ef9b4c66648 508 void control()
benkatz 3:9ef9b4c66648 509 {
benkatz 3:9ef9b4c66648 510 spi_data.q_abad[0] = l1_state.a.p;
benkatz 3:9ef9b4c66648 511 spi_data.q_hip[0] = l1_state.h.p;
benkatz 3:9ef9b4c66648 512 spi_data.q_knee[0] = l1_state.k.p;
benkatz 3:9ef9b4c66648 513 spi_data.qd_abad[0] = l1_state.a.v;
benkatz 3:9ef9b4c66648 514 spi_data.qd_hip[0] = l1_state.h.v;
benkatz 3:9ef9b4c66648 515 spi_data.qd_knee[0] = l1_state.k.v;
benkatz 3:9ef9b4c66648 516
benkatz 3:9ef9b4c66648 517 spi_data.q_abad[1] = l2_state.a.p;
benkatz 3:9ef9b4c66648 518 spi_data.q_hip[1] = l2_state.h.p;
benkatz 3:9ef9b4c66648 519 spi_data.q_knee[1] = l2_state.k.p;
benkatz 3:9ef9b4c66648 520 spi_data.qd_abad[1] = l2_state.a.v;
benkatz 3:9ef9b4c66648 521 spi_data.qd_hip[1] = l2_state.h.v;
benkatz 3:9ef9b4c66648 522 spi_data.qd_knee[1] = l2_state.k.v;
benkatz 3:9ef9b4c66648 523
Rushu 5:4aa2a7fa7818 524 tx_buff[0] = float_to_uint(spi_data.q_abad[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_abad[0]);
Rushu 5:4aa2a7fa7818 525 tx_buff[1] = float_to_uint(spi_data.q_abad[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_abad[1]);
Rushu 5:4aa2a7fa7818 526 tx_buff[2] = float_to_uint(spi_data.q_hip[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_hip[0]);
Rushu 5:4aa2a7fa7818 527 tx_buff[3] = float_to_uint(spi_data.q_hip[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_hip[1]);
Rushu 5:4aa2a7fa7818 528 tx_buff[4] = float_to_uint(spi_data.q_knee[0],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_knee[0]);
Rushu 5:4aa2a7fa7818 529 tx_buff[5] = float_to_uint(spi_data.q_knee[1],P_MIN, P_MAX, 16);//(uint16_t)(spi_data.q_knee[1]);
benkatz 3:9ef9b4c66648 530
Rushu 5:4aa2a7fa7818 531 tx_buff[6] = float_to_uint(spi_data.qd_abad[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_abad[0]);
Rushu 5:4aa2a7fa7818 532 tx_buff[7] = float_to_uint(spi_data.qd_abad[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_abad[1]);
Rushu 5:4aa2a7fa7818 533 tx_buff[8] = float_to_uint(spi_data.qd_hip[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_hip[0]);
Rushu 5:4aa2a7fa7818 534 tx_buff[9] = float_to_uint(spi_data.qd_hip[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_hip[1]);
Rushu 5:4aa2a7fa7818 535 tx_buff[10] = float_to_uint(spi_data.qd_knee[0], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_knee[0]);
Rushu 5:4aa2a7fa7818 536 tx_buff[11] = float_to_uint(spi_data.qd_knee[1], V_MIN, V_MAX, 12);//(uint16_t)(spi_data.qd_knee[1]);
benkatz 3:9ef9b4c66648 537
Rushu 5:4aa2a7fa7818 538 tx_buff[12] = (uint16_t)(spi_data.flags[0]);
Rushu 5:4aa2a7fa7818 539 tx_buff[13] = (uint16_t)(spi_data.flags[1]);
Rushu 5:4aa2a7fa7818 540 spi_data.checksum = xor_checksum(tx_buff,14);
Rushu 5:4aa2a7fa7818 541
Rushu 5:4aa2a7fa7818 542 tx_buff[14] = (spi_data.checksum)&0xFFFF;
Rushu 5:4aa2a7fa7818 543
Rushu 5:4aa2a7fa7818 544 //=================================================hjb added==========================//
Rushu 5:4aa2a7fa7818 545 static double g_ArmTacc[5] = {0.060, 0.060, 0.060, 0.060, 0.060}; // acceleration time 40ms, 60, 60 40Deg/S
Rushu 6:3e6d09f56278 546 static float g_ArmMAX_STEP[5] = {20.0, 20.0, 20.0, 20.0, 20.0};
Rushu 5:4aa2a7fa7818 547 //static float Accr1 = 0.0,Accr2 = 0.0,Accr3 = 0.0,Accr4 = 0.0,Accr5 = 0.0;
Rushu 5:4aa2a7fa7818 548 int k =0;
Rushu 5:4aa2a7fa7818 549 for (k=0; k<5; k++) g_ArmTacc[k] = ARMJOINTACC; //0.030f;
Rushu 5:4aa2a7fa7818 550 for (k=0; k<5; k++) g_ArmMAX_STEP[k] = g_ArmSpeed*2.0f*g_ArmTacc[k];
Rushu 5:4aa2a7fa7818 551 timeout++;
Rushu 6:3e6d09f56278 552 if((timeout > SPI_TIMEOUT) && (SPI_TIMEOUT > 0)){
Rushu 6:3e6d09f56278 553 enabled = 0;
Rushu 5:4aa2a7fa7818 554 memset(&l1_control, 0, sizeof(l1_control));
Rushu 5:4aa2a7fa7818 555 memset(&l2_control, 0, sizeof(l2_control));
Rushu 5:4aa2a7fa7818 556 ExitMotorMode(&a1_can);
Rushu 5:4aa2a7fa7818 557 //can1.write(a1_can);
Rushu 6:3e6d09f56278 558 //wait(0.0001);
Rushu 6:3e6d09f56278 559 ExitMotorMode(&a2_can);
Rushu 6:3e6d09f56278 560 //can2.write(a2_can);
Rushu 6:3e6d09f56278 561 //wait(0.0001);
Rushu 6:3e6d09f56278 562 ExitMotorMode(&k1_can);
Rushu 6:3e6d09f56278 563 //can1.write(k1_can);
Rushu 6:3e6d09f56278 564 //wait(0.0001);
Rushu 6:3e6d09f56278 565 ExitMotorMode(&k2_can);
Rushu 6:3e6d09f56278 566 //can2.write(k2_can);
Rushu 6:3e6d09f56278 567 //wait(0.0001);
Rushu 6:3e6d09f56278 568 ExitMotorMode(&h1_can);
Rushu 6:3e6d09f56278 569 //can1.write(h1_can);
Rushu 6:3e6d09f56278 570 //wait(0.0001);
Rushu 6:3e6d09f56278 571 ExitMotorMode(&h2_can);
Rushu 6:3e6d09f56278 572 //can2.write(k2_can);
Rushu 6:3e6d09f56278 573 //printf("Time out\n\r");
Rushu 6:3e6d09f56278 574 spi_data.flags[0] = 0xdead;
Rushu 6:3e6d09f56278 575 spi_data.flags[1] = 0xdead;
Rushu 6:3e6d09f56278 576 led = 1; // HJB added
Rushu 6:3e6d09f56278 577 return;
Rushu 6:3e6d09f56278 578 }
Rushu 6:3e6d09f56278 579 else
Rushu 6:3e6d09f56278 580 {
Rushu 6:3e6d09f56278 581 if(((spi_command.flags[0]&0x1)==1) && (enabled==0)){
Rushu 6:3e6d09f56278 582 //===============================================HJB added====================================================//
Rushu 6:3e6d09f56278 583 Init_pos = spi_data.q_abad[0]; //==== initial the first position
Rushu 6:3e6d09f56278 584 count_HJB = 0; //for trajectory hjb
Rushu 6:3e6d09f56278 585 enabled = 1;
Rushu 6:3e6d09f56278 586 EnterMotorMode(&a1_can);
Rushu 6:3e6d09f56278 587 can1.write(a1_can);
Rushu 6:3e6d09f56278 588 wait(0.0001);
Rushu 6:3e6d09f56278 589 EnterMotorMode(&a2_can);
Rushu 6:3e6d09f56278 590 can2.write(a2_can);
Rushu 6:3e6d09f56278 591 wait(0.0001);
Rushu 6:3e6d09f56278 592 EnterMotorMode(&k1_can);
Rushu 6:3e6d09f56278 593 can1.write(k1_can);
Rushu 6:3e6d09f56278 594 wait(0.0001);
Rushu 6:3e6d09f56278 595 EnterMotorMode(&k2_can);
Rushu 6:3e6d09f56278 596 can2.write(k2_can);
Rushu 6:3e6d09f56278 597 wait(0.0001);
Rushu 6:3e6d09f56278 598 EnterMotorMode(&h1_can);
Rushu 6:3e6d09f56278 599 can1.write(h1_can);
Rushu 6:3e6d09f56278 600 wait(0.0001);
Rushu 6:3e6d09f56278 601 EnterMotorMode(&h2_can);
Rushu 6:3e6d09f56278 602 can2.write(h2_can);
Rushu 6:3e6d09f56278 603 wait(0.0001);
Rushu 6:3e6d09f56278 604 printf("e\n\r");
Rushu 6:3e6d09f56278 605 return;
Rushu 6:3e6d09f56278 606 }
Rushu 6:3e6d09f56278 607 else if((((spi_command.flags[0]&0x1))==0) && (enabled==1)){ //**//exit motor mode
Rushu 6:3e6d09f56278 608 enabled = 0;
Rushu 6:3e6d09f56278 609 ExitMotorMode(&a1_can);
Rushu 6:3e6d09f56278 610 //can1.write(a1_can);
Rushu 5:4aa2a7fa7818 611 ExitMotorMode(&a2_can);
Rushu 5:4aa2a7fa7818 612 //can2.write(a2_can);
Rushu 5:4aa2a7fa7818 613 ExitMotorMode(&h1_can);
Rushu 5:4aa2a7fa7818 614 //can1.write(h1_can);
Rushu 5:4aa2a7fa7818 615 ExitMotorMode(&h2_can);
Rushu 5:4aa2a7fa7818 616 //can2.write(h2_can);
Rushu 5:4aa2a7fa7818 617 ExitMotorMode(&k1_can);
Rushu 5:4aa2a7fa7818 618 //can1.write(k1_can);
Rushu 5:4aa2a7fa7818 619 ExitMotorMode(&k2_can);
Rushu 5:4aa2a7fa7818 620 //can2.write(k2_can);
Rushu 6:3e6d09f56278 621 printf("x\n\r");
Rushu 5:4aa2a7fa7818 622 return;
Rushu 6:3e6d09f56278 623 }
Rushu 6:3e6d09f56278 624 }
Rushu 6:3e6d09f56278 625 if(enabled){
Rushu 6:3e6d09f56278 626 if(estop==0){
Rushu 6:3e6d09f56278 627 //printf("estopped!!!!\n\r");
Rushu 6:3e6d09f56278 628 //enabled = 0; //hjb added
Rushu 6:3e6d09f56278 629 memset(&l1_control, 0, sizeof(l1_control));
Rushu 6:3e6d09f56278 630 memset(&l2_control, 0, sizeof(l2_control));
Rushu 6:3e6d09f56278 631 ExitMotorMode(&a1_can);
Rushu 6:3e6d09f56278 632 //can1.write(a1_can);
Rushu 6:3e6d09f56278 633 ExitMotorMode(&a2_can);
Rushu 6:3e6d09f56278 634 //can2.write(a2_can);
Rushu 6:3e6d09f56278 635 ExitMotorMode(&h1_can);
Rushu 6:3e6d09f56278 636 //can1.write(h1_can);
Rushu 6:3e6d09f56278 637 ExitMotorMode(&h2_can);
Rushu 6:3e6d09f56278 638 //can2.write(h2_can);
Rushu 6:3e6d09f56278 639 ExitMotorMode(&k1_can);
Rushu 6:3e6d09f56278 640 //can1.write(k1_can);
Rushu 6:3e6d09f56278 641 ExitMotorMode(&k2_can);
Rushu 6:3e6d09f56278 642 //can2.write(k2_can);
Rushu 6:3e6d09f56278 643
Rushu 6:3e6d09f56278 644 spi_data.flags[0] = 0xdead;
Rushu 6:3e6d09f56278 645 spi_data.flags[1] = 0xdead;
Rushu 6:3e6d09f56278 646 led = 1; // HJB added
Rushu 6:3e6d09f56278 647
Rushu 6:3e6d09f56278 648 return;
Rushu 6:3e6d09f56278 649 }
Rushu 6:3e6d09f56278 650
Rushu 6:3e6d09f56278 651
Rushu 5:4aa2a7fa7818 652
Rushu 6:3e6d09f56278 653 else{
Rushu 6:3e6d09f56278 654 led = 0;
Rushu 6:3e6d09f56278 655 memset(&l1_control, 0, sizeof(l1_control));
Rushu 6:3e6d09f56278 656 memset(&l2_control, 0, sizeof(l2_control));
Rushu 6:3e6d09f56278 657
Rushu 6:3e6d09f56278 658 g_ArmJointAngleDe[0] =uint_to_float(spi_command.q_des_abad[0], P_MIN, P_MAX, 16); //input angle from platform
Rushu 6:3e6d09f56278 659 g_ArmJointAngleDe[1] =uint_to_float(spi_command.q_des_hip[0], P_MIN, P_MAX, 16);//spi_command.q_des_hip[0];
Rushu 6:3e6d09f56278 660 g_ArmJointAngleDe[2] =uint_to_float(spi_command.q_des_knee[0], P_MIN, P_MAX, 16);//spi_command.q_des_knee[0];
Rushu 6:3e6d09f56278 661 g_ArmJointAngleDe[3] =uint_to_float(spi_command.q_des_abad[1], P_MIN, P_MAX, 16); ;
Rushu 6:3e6d09f56278 662 g_ArmJointAngleDe[4] =uint_to_float(spi_command.q_des_hip[1], P_MIN, P_MAX, 16);
Rushu 6:3e6d09f56278 663
Rushu 6:3e6d09f56278 664 g_ArmJointAngleRe[0] = l1_state.a.p;
Rushu 6:3e6d09f56278 665 g_ArmJointAngleRe[1] = l1_state.h.p;
Rushu 6:3e6d09f56278 666 g_ArmJointAngleRe[2] = l1_state.k.p;
Rushu 6:3e6d09f56278 667 g_ArmJointAngleRe[3] = l2_state.a.p;
Rushu 6:3e6d09f56278 668 g_ArmJointAngleRe[4] = l2_state.h.p;
Rushu 6:3e6d09f56278 669 arm_reference_trajectory(g_ArmInit, TS, ARMSP, g_ArmTacc, &g_ArmRef, g_ArmMAX_STEP, g_ArmJointAngleRe, g_ArmJointAngleDe, g_ArmTrajRef);
Rushu 6:3e6d09f56278 670
Rushu 6:3e6d09f56278 671 g_ArmInit = FALSE;
Rushu 6:3e6d09f56278 672
Rushu 6:3e6d09f56278 673 l1_control.a.p_des = g_ArmTrajRef[0];
Rushu 6:3e6d09f56278 674 l1_control.h.p_des = g_ArmTrajRef[1];
Rushu 6:3e6d09f56278 675 l1_control.k.p_des = g_ArmTrajRef[2];
Rushu 6:3e6d09f56278 676 l2_control.a.p_des = g_ArmTrajRef[3];
Rushu 6:3e6d09f56278 677 l2_control.h.p_des = g_ArmTrajRef[4];
Rushu 6:3e6d09f56278 678
Rushu 6:3e6d09f56278 679 l1_control.a.v_des = g_ArmTrajRef[5];
Rushu 6:3e6d09f56278 680 l1_control.h.v_des = g_ArmTrajRef[6];
Rushu 6:3e6d09f56278 681 l1_control.k.v_des = g_ArmTrajRef[7];
Rushu 6:3e6d09f56278 682 l2_control.a.v_des = g_ArmTrajRef[8];
Rushu 6:3e6d09f56278 683 l2_control.h.v_des = g_ArmTrajRef[9];
benkatz 3:9ef9b4c66648 684
Rushu 6:3e6d09f56278 685 //========================================HJB added for trajectory input=========================================//
Rushu 6:3e6d09f56278 686 Pmag = uint_to_float(spi_command.q_des_abad[0], P_MIN, P_MAX, 16);
Rushu 6:3e6d09f56278 687 Tperiod = uint_to_float(spi_command.qd_des_abad[0], V_MIN, V_MAX, 12);//;
Rushu 6:3e6d09f56278 688 if (Pmag != Pmag_last){count_HJB = 0;}
Rushu 6:3e6d09f56278 689 Pmag_last = Pmag;
Rushu 6:3e6d09f56278 690 Init_pos = 0;
Rushu 6:3e6d09f56278 691 p_des_HJB = Init_pos + Pmag*FastSin(2*PI*count_HJB/(Tperiod*1000));//Pmag*FastSin(2*PI*count/(Tperiod*40000));
Rushu 6:3e6d09f56278 692 v_des_HJB = 2*PI*Pmag*FastCos(2*PI*count_HJB/(Tperiod*1000))/Tperiod;
Rushu 6:3e6d09f56278 693 l1_control.a.p_des = p_des_HJB;//uint_to_float(p_des_HJB, -15.f, 15.f, 16);
Rushu 6:3e6d09f56278 694 l1_control.a.v_des = v_des_HJB;
Rushu 6:3e6d09f56278 695 if(count_HJB>=(Tperiod*1000)) {
Rushu 6:3e6d09f56278 696 count_HJB = 0;
Rushu 6:3e6d09f56278 697 }
Rushu 6:3e6d09f56278 698 count_HJB++;
Rushu 6:3e6d09f56278 699 //========================================HJB end=========================================//
Rushu 6:3e6d09f56278 700 //l1_control.a.p_des = spi_command.q_des_abad[0];
Rushu 6:3e6d09f56278 701 //l1_control.a.v_des = spi_command.qd_des_abad[0];
Rushu 6:3e6d09f56278 702 l1_control.a.kp = spi_command.kp_abad[0];
Rushu 6:3e6d09f56278 703 l1_control.a.kd = spi_command.kd_abad[0];
Rushu 6:3e6d09f56278 704 l1_control.a.t_ff = spi_command.tau_abad_ff[0];
Rushu 6:3e6d09f56278 705
Rushu 6:3e6d09f56278 706 //l1_control.h.p_des = spi_command.q_des_hip[0];
Rushu 6:3e6d09f56278 707 //l1_control.h.v_des = spi_command.qd_des_hip[0];
Rushu 6:3e6d09f56278 708 l1_control.h.kp = spi_command.kp_hip[0];
Rushu 6:3e6d09f56278 709 l1_control.h.kd = spi_command.kd_hip[0];
Rushu 6:3e6d09f56278 710 l1_control.h.t_ff = spi_command.tau_hip_ff[0];
Rushu 6:3e6d09f56278 711
Rushu 6:3e6d09f56278 712 //l1_control.k.p_des = spi_command.q_des_knee[0];
Rushu 6:3e6d09f56278 713 //l1_control.k.v_des = spi_command.qd_des_knee[0];
Rushu 6:3e6d09f56278 714 l1_control.k.kp = spi_command.kp_knee[0];
Rushu 6:3e6d09f56278 715 l1_control.k.kd = spi_command.kd_knee[0];
Rushu 6:3e6d09f56278 716 l1_control.k.t_ff = spi_command.tau_knee_ff[0];
Rushu 6:3e6d09f56278 717
Rushu 6:3e6d09f56278 718 //l2_control.a.p_des = spi_command.q_des_abad[1];
Rushu 6:3e6d09f56278 719 //l2_control.a.v_des = spi_command.qd_des_abad[1];
Rushu 6:3e6d09f56278 720 l2_control.a.kp = spi_command.kp_abad[1];
Rushu 6:3e6d09f56278 721 l2_control.a.kd = spi_command.kd_abad[1];
Rushu 6:3e6d09f56278 722 l2_control.a.t_ff = spi_command.tau_abad_ff[1];
Rushu 6:3e6d09f56278 723
Rushu 6:3e6d09f56278 724 //l2_control.h.p_des = spi_command.q_des_hip[1];
Rushu 6:3e6d09f56278 725 //l2_control.h.v_des = spi_command.qd_des_hip[1];
Rushu 6:3e6d09f56278 726 l2_control.h.kp = spi_command.kp_hip[1];
Rushu 6:3e6d09f56278 727 l2_control.h.kd = spi_command.kd_hip[1];
Rushu 6:3e6d09f56278 728 l2_control.h.t_ff = spi_command.tau_hip_ff[1];
Rushu 6:3e6d09f56278 729
Rushu 6:3e6d09f56278 730 l2_control.k.p_des = spi_command.q_des_knee[1];
Rushu 6:3e6d09f56278 731 l2_control.k.v_des = spi_command.qd_des_knee[1];
Rushu 6:3e6d09f56278 732 l2_control.k.kp = spi_command.kp_knee[1];
Rushu 6:3e6d09f56278 733 l2_control.k.kd = spi_command.kd_knee[1];
Rushu 6:3e6d09f56278 734 l2_control.k.t_ff = spi_command.tau_knee_ff[1];
Rushu 6:3e6d09f56278 735
Rushu 6:3e6d09f56278 736 ///printf("ap=%d akp=%d at=%d \n\r",(uint16_t)l1_control.a.p_des,(uint16_t)l1_control.a.kp,(uint16_t)l1_control.a.t_ff);
Rushu 6:3e6d09f56278 737
Rushu 6:3e6d09f56278 738 spi_data.flags[0] = 0;
Rushu 6:3e6d09f56278 739 spi_data.flags[1] = 0;
Rushu 6:3e6d09f56278 740 //spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N); //hjb cancelled
Rushu 6:3e6d09f56278 741 //spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled
Rushu 6:3e6d09f56278 742 ////spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2;
Rushu 6:3e6d09f56278 743 //spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N); //hjb cancelled
Rushu 6:3e6d09f56278 744 //spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1; //hjb cancelled
Rushu 6:3e6d09f56278 745 ////spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2;
Rushu 6:3e6d09f56278 746
Rushu 6:3e6d09f56278 747 //spi_data.flags[0] = 0xbeef;
Rushu 6:3e6d09f56278 748 //spi_data.flags[1] = 0xbeef;
Rushu 6:3e6d09f56278 749 PackAll();
Rushu 6:3e6d09f56278 750 //WriteAll();
Rushu 6:3e6d09f56278 751 }
Rushu 5:4aa2a7fa7818 752
Rushu 6:3e6d09f56278 753
benkatz 3:9ef9b4c66648 754 }
benkatz 3:9ef9b4c66648 755
Rushu 5:4aa2a7fa7818 756 //spi_data.checksum = xor_checksum((uint16_t*)&spi_data,14);
Rushu 5:4aa2a7fa7818 757 // for(int i = 0; i < DATA_LEN; i++){
Rushu 5:4aa2a7fa7818 758 // tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
Rushu 5:4aa2a7fa7818 759
benkatz 3:9ef9b4c66648 760 }
benkatz 3:9ef9b4c66648 761
benkatz 3:9ef9b4c66648 762
benkatz 3:9ef9b4c66648 763 void test_control()
benkatz 3:9ef9b4c66648 764 {
Rushu 5:4aa2a7fa7818 765
benkatz 3:9ef9b4c66648 766 for(int i = 0; i < 2; i++)
benkatz 3:9ef9b4c66648 767 {
benkatz 3:9ef9b4c66648 768 spi_data.q_abad[i] = spi_command.q_des_abad[i] + 1.f;
benkatz 3:9ef9b4c66648 769 spi_data.q_knee[i] = spi_command.q_des_knee[i] + 1.f;
benkatz 3:9ef9b4c66648 770 spi_data.q_hip[i] = spi_command.q_des_hip[i] + 1.f;
benkatz 3:9ef9b4c66648 771
benkatz 3:9ef9b4c66648 772 spi_data.qd_abad[i] = spi_command.qd_des_abad[i] + 1.f;
benkatz 3:9ef9b4c66648 773 spi_data.qd_knee[i] = spi_command.qd_des_knee[i] + 1.f;
benkatz 3:9ef9b4c66648 774 spi_data.qd_hip[i] = spi_command.qd_des_hip[i] + 1.f;
Rushu 5:4aa2a7fa7818 775 //printf("%d %d %f %.3f %.3f %.3f\n\r",(uint16_t)(spi_command.q_des_abad[i]),(uint16_t)spi_command.q_des_knee[i],spi_command.q_des_hip[i],spi_data.qd_abad[i],spi_data.qd_knee[i],spi_data.qd_hip[i]);
benkatz 3:9ef9b4c66648 776 }
benkatz 3:9ef9b4c66648 777
Rushu 5:4aa2a7fa7818 778 //spi_data.flags[0] = 0xdead;
benkatz 3:9ef9b4c66648 779 //spi_data.flags[1] = 0xbeef;
benkatz 3:9ef9b4c66648 780
Rushu 5:4aa2a7fa7818 781 spi_data.flags[0] = 2;
Rushu 5:4aa2a7fa7818 782 spi_data.flags[1] = 2;
Rushu 5:4aa2a7fa7818 783
benkatz 3:9ef9b4c66648 784 // only do first 56 bytes of message.
Rushu 5:4aa2a7fa7818 785 //spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
Rushu 5:4aa2a7fa7818 786
Rushu 5:4aa2a7fa7818 787 tx_buff[0] = (uint16_t)(spi_data.q_abad[0]);
Rushu 5:4aa2a7fa7818 788 tx_buff[1] = (uint16_t)(spi_data.q_abad[1]);
Rushu 5:4aa2a7fa7818 789 tx_buff[2] = (uint16_t)(spi_data.q_hip[0]);
Rushu 5:4aa2a7fa7818 790 tx_buff[3] = (uint16_t)(spi_data.q_hip[1]);
Rushu 5:4aa2a7fa7818 791 tx_buff[4] = (uint16_t)(spi_data.q_knee[0]);
Rushu 5:4aa2a7fa7818 792 tx_buff[5] = (uint16_t)(spi_data.q_knee[1]);
Rushu 5:4aa2a7fa7818 793
Rushu 5:4aa2a7fa7818 794 tx_buff[6] = (uint16_t)(spi_data.qd_abad[0]);
Rushu 5:4aa2a7fa7818 795 tx_buff[7] = (uint16_t)(spi_data.qd_abad[1]);
Rushu 5:4aa2a7fa7818 796 tx_buff[8] = (uint16_t)(spi_data.qd_hip[0]);
Rushu 5:4aa2a7fa7818 797 tx_buff[9] = (uint16_t)(spi_data.qd_hip[1]);
Rushu 5:4aa2a7fa7818 798 tx_buff[10] = (uint16_t)(spi_data.qd_knee[0]);
Rushu 5:4aa2a7fa7818 799 tx_buff[11] = (uint16_t)(spi_data.qd_knee[1]);
Rushu 5:4aa2a7fa7818 800
Rushu 5:4aa2a7fa7818 801 tx_buff[12] = (uint16_t)(spi_data.flags[0]);
Rushu 5:4aa2a7fa7818 802 tx_buff[13] = (uint16_t)(spi_data.flags[1]);
Rushu 5:4aa2a7fa7818 803 spi_data.checksum = xor_checksum(tx_buff,14);
Rushu 5:4aa2a7fa7818 804
Rushu 5:4aa2a7fa7818 805 tx_buff[14] = (spi_data.checksum)&0xFFFF;
Rushu 5:4aa2a7fa7818 806
benkatz 3:9ef9b4c66648 807
benkatz 3:9ef9b4c66648 808 for(int i = 0; i < DATA_LEN; i++)
Rushu 5:4aa2a7fa7818 809 { // tx_buff[i] = ((uint16_t*)(&spi_data))[i];
Rushu 5:4aa2a7fa7818 810 printf("%d ", tx_buff[i]);
Rushu 5:4aa2a7fa7818 811 }
Rushu 5:4aa2a7fa7818 812 int testchecksum = xor_checksum((uint16_t*)&spi_data,14);
Rushu 5:4aa2a7fa7818 813
Rushu 5:4aa2a7fa7818 814 printf("%d %d\n\r",testchecksum,spi_data.checksum);
Rushu 5:4aa2a7fa7818 815
benkatz 3:9ef9b4c66648 816 }
benkatz 3:9ef9b4c66648 817
benkatz 3:9ef9b4c66648 818 void init_spi(void){
benkatz 3:9ef9b4c66648 819 SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
benkatz 3:9ef9b4c66648 820 spi->format(16, 0);
benkatz 3:9ef9b4c66648 821 spi->frequency(12000000);
benkatz 3:9ef9b4c66648 822 spi->reply(0x0);
benkatz 3:9ef9b4c66648 823 cs.fall(&spi_isr);
benkatz 3:9ef9b4c66648 824 printf("done\n\r");
benkatz 3:9ef9b4c66648 825 }
benkatz 3:9ef9b4c66648 826
benkatz 3:9ef9b4c66648 827
benkatz 3:9ef9b4c66648 828 int main() {
benkatz 3:9ef9b4c66648 829 //wait(.5);
benkatz 3:9ef9b4c66648 830 //led = 1;
Rushu 5:4aa2a7fa7818 831 //**// pc.baud(921600);
Rushu 6:3e6d09f56278 832 pc.baud(460800); //115200
benkatz 3:9ef9b4c66648 833 pc.attach(&serial_isr);
benkatz 3:9ef9b4c66648 834 estop.mode(PullUp);
benkatz 3:9ef9b4c66648 835 //spi.format(16, 0);
benkatz 3:9ef9b4c66648 836 //spi.frequency(1000000);
benkatz 3:9ef9b4c66648 837 //spi.reply(0x0);
benkatz 3:9ef9b4c66648 838 //cs.fall(&spi_isr);
benkatz 3:9ef9b4c66648 839
benkatz 3:9ef9b4c66648 840 can1.frequency(1000000); // set bit rate to 1Mbps
Rushu 5:4aa2a7fa7818 841 can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler
benkatz 3:9ef9b4c66648 842 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 3:9ef9b4c66648 843 can2.frequency(1000000); // set bit rate to 1Mbps
Rushu 5:4aa2a7fa7818 844 can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler
benkatz 3:9ef9b4c66648 845 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 3:9ef9b4c66648 846
benkatz 3:9ef9b4c66648 847 memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
benkatz 3:9ef9b4c66648 848 memset(&spi_data, 0, sizeof(spi_data_t));
benkatz 3:9ef9b4c66648 849 memset(&spi_command,0,sizeof(spi_command_t));
benkatz 3:9ef9b4c66648 850
benkatz 3:9ef9b4c66648 851
benkatz 3:9ef9b4c66648 852 NVIC_SetPriority(TIM5_IRQn, 1);
Rushu 6:3e6d09f56278 853 NVIC_SetPriority(CAN1_RX0_IRQn, 4);
Rushu 5:4aa2a7fa7818 854 NVIC_SetPriority(CAN2_RX0_IRQn, 3);
Rushu 5:4aa2a7fa7818 855 /*
Rushu 5:4aa2a7fa7818 856 //=============================hjb===============================================================//
Rushu 5:4aa2a7fa7818 857 //ISR Setup
Rushu 5:4aa2a7fa7818 858 #define CAN_ARR 0x56 /// timer autoreload value 0x8CA
Rushu 5:4aa2a7fa7818 859 NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
Rushu 5:4aa2a7fa7818 860
Rushu 5:4aa2a7fa7818 861 TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt
Rushu 5:4aa2a7fa7818 862 TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up
Rushu 5:4aa2a7fa7818 863 TIM1->CR1 |= TIM_CR1_UDIS;
Rushu 5:4aa2a7fa7818 864 TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on,
Rushu 5:4aa2a7fa7818 865 TIM1->RCR |= 0x001; // update event once per up/down count of tim1
Rushu 5:4aa2a7fa7818 866 TIM1->EGR |= TIM_EGR_UG;
Rushu 5:4aa2a7fa7818 867
Rushu 5:4aa2a7fa7818 868 //PWM Setup
Rushu 5:4aa2a7fa7818 869
Rushu 5:4aa2a7fa7818 870 TIM1->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
Rushu 5:4aa2a7fa7818 871 TIM1->ARR = CAN_ARR; // set auto reload, 40 khz
Rushu 5:4aa2a7fa7818 872 TIM1->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on.
Rushu 5:4aa2a7fa7818 873 TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1
Rushu 5:4aa2a7fa7818 874
Rushu 5:4aa2a7fa7818 875 //++++++++ FOURARM ++++++++++++++++++++++//
Rushu 5:4aa2a7fa7818 876 //----------------------------------------------------------
Rushu 5:4aa2a7fa7818 877
Rushu 5:4aa2a7fa7818 878 int i = 0;
Rushu 5:4aa2a7fa7818 879 for (i=0; i<5; i++)
Rushu 5:4aa2a7fa7818 880 {
Rushu 5:4aa2a7fa7818 881 for (k=0; k<3; k++) trajRef[i][k] = 0;
Rushu 5:4aa2a7fa7818 882 for (k=0; k<3; k++) trajRef2[i][k] = 0;
Rushu 5:4aa2a7fa7818 883 }
Rushu 5:4aa2a7fa7818 884 //----------------------------------------------------------
Rushu 5:4aa2a7fa7818 885 //----------------------------------------------------------
Rushu 5:4aa2a7fa7818 886 for (i=0; i<5; i++)
Rushu 5:4aa2a7fa7818 887 {
Rushu 5:4aa2a7fa7818 888 for (k=0; k<3; k++)
Rushu 5:4aa2a7fa7818 889 {
Rushu 5:4aa2a7fa7818 890 ref[i].T1[k] = 0; // ?????????? time t //
Rushu 5:4aa2a7fa7818 891 ref[i].t1[k] = 0; // ?????????? time t //
Rushu 5:4aa2a7fa7818 892 ref[i].t[k] = 0; // current time t //
Rushu 5:4aa2a7fa7818 893 ref[i].M[k] = 0; // Desire Position C //
Rushu 5:4aa2a7fa7818 894 ref[i].N[k] = 0; // Desire Position C //
Rushu 5:4aa2a7fa7818 895 ref[i].L[k] = 0; // Last Position B //
Rushu 5:4aa2a7fa7818 896 ref[i].det_M[k] = 0; // C - B //
Rushu 5:4aa2a7fa7818 897 ref[i].det_L[k] = 0; // A - B //
Rushu 5:4aa2a7fa7818 898 }
Rushu 5:4aa2a7fa7818 899 }
Rushu 5:4aa2a7fa7818 900
Rushu 5:4aa2a7fa7818 901 //----------------------------------------------------------
Rushu 5:4aa2a7fa7818 902 */
Rushu 5:4aa2a7fa7818 903 int i, k;
Rushu 5:4aa2a7fa7818 904
Rushu 5:4aa2a7fa7818 905 //++++++++++++++++++ FOURARM ++++++++++++++++++++//
Rushu 5:4aa2a7fa7818 906 for (i=0; i<5; i++)
Rushu 5:4aa2a7fa7818 907 {
Rushu 5:4aa2a7fa7818 908 g_ArmTrajRef[i] = 0.0f;
Rushu 5:4aa2a7fa7818 909 g_ArmJointAngleRe[i] = 0.0f;
Rushu 5:4aa2a7fa7818 910 g_ArmJointAngleDe[i] = 0.0f;
Rushu 5:4aa2a7fa7818 911 }
Rushu 5:4aa2a7fa7818 912
Rushu 5:4aa2a7fa7818 913 for (k=0; k<5; k++)
Rushu 5:4aa2a7fa7818 914 {
Rushu 5:4aa2a7fa7818 915 g_ArmRef.T1[k] = 0.0f; // ?????????? time t //
Rushu 5:4aa2a7fa7818 916 g_ArmRef.t1[k] = 0.0f; // ?????????? time t //
Rushu 5:4aa2a7fa7818 917 g_ArmRef.t[k] = 0.0f; // current time t //
Rushu 5:4aa2a7fa7818 918 g_ArmRef.M[k] = 0.0f; // Desire Position C //
Rushu 5:4aa2a7fa7818 919 g_ArmRef.N[k] = 0.0f; // Desire Position C //
Rushu 5:4aa2a7fa7818 920 g_ArmRef.L[k] = 0.0f; // Last Position B //
Rushu 5:4aa2a7fa7818 921 g_ArmRef.det_M[k] = 0.0f; // C - B //
Rushu 5:4aa2a7fa7818 922 g_ArmRef.det_L[k] = 0.0f; // A - B //
Rushu 5:4aa2a7fa7818 923 }
Rushu 5:4aa2a7fa7818 924
Rushu 5:4aa2a7fa7818 925
Rushu 5:4aa2a7fa7818 926 //++++++++++++++++++ FOURARM ++++++++++++++++++++//
Rushu 5:4aa2a7fa7818 927 //=============================hjb===============================================================//
Rushu 5:4aa2a7fa7818 928
benkatz 3:9ef9b4c66648 929
benkatz 3:9ef9b4c66648 930 printf("\n\r SPIne\n\r");
benkatz 3:9ef9b4c66648 931 //printf("%d\n\r", RX_ID << 18);
benkatz 3:9ef9b4c66648 932
benkatz 3:9ef9b4c66648 933 a1_can.len = 8; //transmit 8 bytes
benkatz 3:9ef9b4c66648 934 a2_can.len = 8; //transmit 8 bytes
benkatz 3:9ef9b4c66648 935 h1_can.len = 8;
benkatz 3:9ef9b4c66648 936 h2_can.len = 8;
benkatz 3:9ef9b4c66648 937 k1_can.len = 8;
benkatz 3:9ef9b4c66648 938 k2_can.len = 8;
benkatz 3:9ef9b4c66648 939 rxMsg1.len = 6; //receive 6 bytes
benkatz 3:9ef9b4c66648 940 rxMsg2.len = 6; //receive 6 bytes
benkatz 3:9ef9b4c66648 941
benkatz 3:9ef9b4c66648 942 a1_can.id = 0x1;
benkatz 3:9ef9b4c66648 943 a2_can.id = 0x1;
benkatz 3:9ef9b4c66648 944 h1_can.id = 0x2;
benkatz 3:9ef9b4c66648 945 h2_can.id = 0x2;
benkatz 3:9ef9b4c66648 946 k1_can.id = 0x3;
benkatz 3:9ef9b4c66648 947 k2_can.id = 0x3;
benkatz 3:9ef9b4c66648 948
benkatz 3:9ef9b4c66648 949 pack_cmd(&a1_can, l1_control.a);
benkatz 3:9ef9b4c66648 950 pack_cmd(&a2_can, l2_control.a);
benkatz 3:9ef9b4c66648 951 pack_cmd(&h1_can, l1_control.h);
benkatz 3:9ef9b4c66648 952 pack_cmd(&h2_can, l2_control.h);
benkatz 3:9ef9b4c66648 953 pack_cmd(&k1_can, l1_control.k);
benkatz 3:9ef9b4c66648 954 pack_cmd(&k2_can, l2_control.k);
benkatz 3:9ef9b4c66648 955 WriteAll();
benkatz 2:25837cbaee98 956
benkatz 2:25837cbaee98 957
benkatz 3:9ef9b4c66648 958 // SPI doesn't work if enabled while the CS pin is pulled low
benkatz 3:9ef9b4c66648 959 // Wait for CS to not be low, then enable SPI
benkatz 3:9ef9b4c66648 960 if(!spi_enabled){
benkatz 3:9ef9b4c66648 961 while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
benkatz 3:9ef9b4c66648 962 init_spi();
benkatz 3:9ef9b4c66648 963 spi_enabled = 1;
benkatz 3:9ef9b4c66648 964 }
Rushu 6:3e6d09f56278 965 loop.attach(&sendCMD, .001); //========hjb added========//
Rushu 6:3e6d09f56278 966 /*if(spi_enabled){
Rushu 5:4aa2a7fa7818 967 loop.attach(&sendCMD, .001); //============hjb added===========//
Rushu 5:4aa2a7fa7818 968 }
Rushu 6:3e6d09f56278 969 */
benkatz 3:9ef9b4c66648 970 while(1) {
Rushu 5:4aa2a7fa7818 971 //counter++;
Rushu 5:4aa2a7fa7818 972 //can2.read(rxMsg2);
Rushu 5:4aa2a7fa7818 973 //unpack_reply(rxMsg2, &l2_state);
Rushu 5:4aa2a7fa7818 974 //can1.read(rxMsg1); // read message into Rx message storage
Rushu 5:4aa2a7fa7818 975 //unpack_reply(rxMsg1, &l1_state);
Rushu 5:4aa2a7fa7818 976
Rushu 5:4aa2a7fa7818 977 if(Control_Flag){
Rushu 5:4aa2a7fa7818 978 control();
Rushu 6:3e6d09f56278 979 counter ++;
Rushu 6:3e6d09f56278 980
Rushu 5:4aa2a7fa7818 981
Rushu 6:3e6d09f56278 982 if(counter>100){
Rushu 6:3e6d09f56278 983
Rushu 6:3e6d09f56278 984 //printf("%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n\r", l1_control.a.p_des*Deg_rad, l1_state.a.p*Deg_rad, l1_control.h.p_des*Deg_rad, l1_state.h.p*Deg_rad, l1_control.a.v_des*Deg_rad, l1_state.a.v*Deg_rad, l1_control.h.v_des*Deg_rad, l1_state.h.v*Deg_rad);
Rushu 6:3e6d09f56278 985 printf("%.3f %.3f %.3f %.3f %d\n\r", p_des_HJB*Deg_rad, l1_state.a.p*Deg_rad, v_des_HJB*Deg_rad, l1_state.a.v*Deg_rad, count_HJB);
Rushu 6:3e6d09f56278 986
Rushu 5:4aa2a7fa7818 987 //printf("%.3f %.3f\n\r", l1_control.h.p_des, g_ArmJointAngleDe[1]);
Rushu 5:4aa2a7fa7818 988 counter = 0 ;
Rushu 5:4aa2a7fa7818 989 }
Rushu 5:4aa2a7fa7818 990 Control_Flag =0;
Rushu 5:4aa2a7fa7818 991 }
Rushu 5:4aa2a7fa7818 992 //wait_us(10);
benkatz 2:25837cbaee98 993
Rushu 5:4aa2a7fa7818 994 }
Rushu 5:4aa2a7fa7818 995
benkatz 0:d6186b8990c5 996
benkatz 0:d6186b8990c5 997 }
benkatz 0:d6186b8990c5 998
benkatz 0:d6186b8990c5 999
benkatz 0:d6186b8990c5 1000
benkatz 0:d6186b8990c5 1001
benkatz 0:d6186b8990c5 1002