Shao Rui / SPIne_referenceTraj_DEMO

1

Dependencies:   mbed-dev_spine

main.cpp@7:e3cff4376669, 2020-07-20 (annotated)

Committer:
shaorui
Date:
Mon Jul 20 01:03:17 2020 +0000
Revision:
7:e3cff4376669
Parent:
6:3e6d09f56278 
1

Who changed what in which revision?

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