chen xin / CH_Communicatuin_Test2

1

main.cpp@2:25837cbaee98, 2017-11-29 (annotated)

Committer:
benkatz
Date:
Wed Nov 29 17:48:13 2017 +0000
Revision:
2:25837cbaee98
Parent:
1:79e0d4791936
Child:
3:9ef9b4c66648
class demo code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 0:d6186b8990c5 1
benkatz 1:79e0d4791936 2 #define CAN_ID 0x0
benkatz 0:d6186b8990c5 3
benkatz 0:d6186b8990c5 4 #include "mbed.h"
benkatz 0:d6186b8990c5 5 #include "math_ops.h"
benkatz 0:d6186b8990c5 6
benkatz 0:d6186b8990c5 7
benkatz 0:d6186b8990c5 8 Serial pc(PA_2, PA_3);
benkatz 1:79e0d4791936 9 CAN can1(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name
benkatz 1:79e0d4791936 10 CAN can2(PB_5, PB_13); // CAN Rx pin name, CAN Tx pin name
benkatz 1:79e0d4791936 11 CANMessage rxMsg1, rxMsg2;
benkatz 1:79e0d4791936 12 CANMessage abad1, abad2, hip1, hip2, knee1, knee2; //TX Messages
benkatz 0:d6186b8990c5 13 int ledState;
benkatz 0:d6186b8990c5 14 Ticker sendCAN;
benkatz 0:d6186b8990c5 15 int counter = 0;
benkatz 0:d6186b8990c5 16 volatile bool msgAvailable = false;
benkatz 0:d6186b8990c5 17 Ticker loop;
benkatz 2:25837cbaee98 18 AnalogIn knob(PC_0);
benkatz 2:25837cbaee98 19 DigitalOut toggle(PC_3);
benkatz 2:25837cbaee98 20 DigitalOut toggle2(PC_2);
benkatz 0:d6186b8990c5 21
benkatz 1:79e0d4791936 22 ///[[abad1, abad2]
benkatz 1:79e0d4791936 23 ///[hip1, hip2]
benkatz 1:79e0d4791936 24 ///[knee1, knee2]]
benkatz 2:25837cbaee98 25 float q1raw[3];
benkatz 2:25837cbaee98 26 float q2raw[3];
benkatz 2:25837cbaee98 27 float dq1raw[3];
benkatz 2:25837cbaee98 28 float dq2raw[3];
benkatz 2:25837cbaee98 29 float q1[3]; //Leg 1 joint angles
benkatz 2:25837cbaee98 30 float dq1[3]; //Leg 1 joint velocities
benkatz 2:25837cbaee98 31 float tau1[3]; //Leg 1 joint torques
benkatz 2:25837cbaee98 32 float q2[3]; //Leg 2 joint angles
benkatz 2:25837cbaee98 33 float dq2[3]; //Leg 2 joint velocities
benkatz 2:25837cbaee98 34 float tau2[3]; //Leg 2 joint torques
benkatz 2:25837cbaee98 35 float p1[3]; //Leg 1 end effector position
benkatz 2:25837cbaee98 36 float v1[3]; //Leg 1 end effector velocity
benkatz 2:25837cbaee98 37 float J1[3][3]; //Leg 1 Jacobian
benkatz 2:25837cbaee98 38 float f1[3]; //Leg 1 end effector forces
benkatz 2:25837cbaee98 39 float p2[3]; //Leg 2 end effector position
benkatz 2:25837cbaee98 40 float v2[3]; //Leg 2 end effector velocity
benkatz 2:25837cbaee98 41 float J2[3][3]; //Leg 2 Jacobian
benkatz 2:25837cbaee98 42 float f2[3]; //Leg 2 end effector forces
benkatz 2:25837cbaee98 43
benkatz 1:79e0d4791936 44 float q[3][2]; //Joint states for both legs
benkatz 1:79e0d4791936 45 float dq[3][2];
benkatz 1:79e0d4791936 46 float tau[3][3];
benkatz 2:25837cbaee98 47
benkatz 2:25837cbaee98 48 float I[3] = {0.005f, 0.0045f, 0.006f}; //Joint space inertias
benkatz 2:25837cbaee98 49 float M1[3][3]; //Leg 1 end effector inverse mass
benkatz 2:25837cbaee98 50 float M2[3][3]; //Leg 2 end effector inverse mass
benkatz 2:25837cbaee98 51 float KD1[3]; //Joint space damping
benkatz 2:25837cbaee98 52 float KD2[3];
benkatz 2:25837cbaee98 53 float contact1[3];
benkatz 2:25837cbaee98 54 float contact2[3];
benkatz 2:25837cbaee98 55
benkatz 2:25837cbaee98 56 const float offset[3] = {0.0f, 3.493f, -2.766f}; //Joint angle offsets at zero position
benkatz 2:25837cbaee98 57
benkatz 2:25837cbaee98 58 float kp = 800.0f;
benkatz 2:25837cbaee98 59 float kd = 100.0f;
benkatz 2:25837cbaee98 60 float kp_q = 100.0f;
benkatz 2:25837cbaee98 61 float kd_q = 0.8f;
benkatz 1:79e0d4791936 62 int enabled = 0;
benkatz 2:25837cbaee98 63 float scaling = 0;
benkatz 2:25837cbaee98 64
benkatz 2:25837cbaee98 65
benkatz 2:25837cbaee98 66 int control_mode = 0;
benkatz 0:d6186b8990c5 67
benkatz 0:d6186b8990c5 68 /// Value Limits ///
benkatz 0:d6186b8990c5 69 #define P_MIN -12.5f
benkatz 0:d6186b8990c5 70 #define P_MAX 12.5f
benkatz 0:d6186b8990c5 71 #define V_MIN -30.0f
benkatz 0:d6186b8990c5 72 #define V_MAX 30.0f
benkatz 0:d6186b8990c5 73 #define KP_MIN 0.0f
benkatz 0:d6186b8990c5 74 #define KP_MAX 500.0f
benkatz 0:d6186b8990c5 75 #define KD_MIN 0.0f
benkatz 0:d6186b8990c5 76 #define KD_MAX 5.0f
benkatz 0:d6186b8990c5 77 #define T_MIN -18.0f
benkatz 0:d6186b8990c5 78 #define T_MAX 18.0f
benkatz 0:d6186b8990c5 79 #define I_MAX 40.0f
benkatz 0:d6186b8990c5 80
benkatz 2:25837cbaee98 81 #define L1 0.0577f
benkatz 2:25837cbaee98 82 #define L2 0.2088f
benkatz 2:25837cbaee98 83 #define L3 0.175f
benkatz 2:25837cbaee98 84
benkatz 2:25837cbaee98 85 void kinematics(const float q[3], const float dq[3], float* p, float* v, float (* J)[3], float (* M)[3]){
benkatz 2:25837cbaee98 86 const float s1 = sinf(q[0]);
benkatz 2:25837cbaee98 87 const float s2 = sinf(q[1]);
benkatz 2:25837cbaee98 88 const float s3 = sinf(q[2]);
benkatz 2:25837cbaee98 89 const float c1 = cosf(q[0]);
benkatz 2:25837cbaee98 90 const float c2 = cosf(q[1]);
benkatz 2:25837cbaee98 91 const float c3 = cosf(q[2]);
benkatz 2:25837cbaee98 92
benkatz 2:25837cbaee98 93 const float c23 = c2*c3 - s2*s3;
benkatz 2:25837cbaee98 94 const float s23 = s2*c3 + c2*s3;
benkatz 2:25837cbaee98 95
benkatz 2:25837cbaee98 96 p[0] = L3*s23 + L2*s2;
benkatz 2:25837cbaee98 97 p[1] = L1*c1 + L3*s1*c23 + L2*c2*s1;
benkatz 2:25837cbaee98 98 p[2] = L1*s1 - L3*c1*c23 - L2*c1*c2;
benkatz 2:25837cbaee98 99
benkatz 2:25837cbaee98 100 J[0][0] = 0;
benkatz 2:25837cbaee98 101 J[0][1] = L3*c23 + L2*c2;
benkatz 2:25837cbaee98 102 J[0][2] = L3*c23;
benkatz 2:25837cbaee98 103 J[1][0] = L3*c1*c23 + L2*c1*c2 - L1*s1;
benkatz 2:25837cbaee98 104 J[1][1] = -L3*s1*s23 - L2*s1*s2;
benkatz 2:25837cbaee98 105 J[1][2] = -L3*s1*s23;
benkatz 2:25837cbaee98 106 J[2][0] = L3*s1*c23 + L2*c2*s1 + L1*c1;
benkatz 2:25837cbaee98 107 J[2][1] = L3*c1*s23 + L2*c1*s2;
benkatz 2:25837cbaee98 108 J[2][2] = L3*c1*s23;
benkatz 2:25837cbaee98 109
benkatz 2:25837cbaee98 110 M[0][0] = J[0][0]*J[0][0]/I[0] + J[0][1]*J[0][1]/I[1] + J[0][2]*J[0][2]/I[2];
benkatz 2:25837cbaee98 111 M[0][1] = 0;
benkatz 2:25837cbaee98 112 M[0][2] = 0;
benkatz 2:25837cbaee98 113 M[1][0] = 0;
benkatz 2:25837cbaee98 114 M[1][1] = J[1][0]*J[1][0]/I[0] + J[1][1]*J[1][1]/I[1] + J[1][2]*J[1][2]/I[2];
benkatz 2:25837cbaee98 115 M[1][2] = 0;
benkatz 2:25837cbaee98 116 M[2][0] = 0;
benkatz 2:25837cbaee98 117 M[2][1] = 0;
benkatz 2:25837cbaee98 118 M[2][2] = J[2][0]*J[2][0]/I[0] + J[2][1]*J[2][1]/I[1] + J[2][2]*J[2][2]/I[2];
benkatz 2:25837cbaee98 119
benkatz 2:25837cbaee98 120 v[0] = 0; v[1] = 0; v[2] = 0;
benkatz 2:25837cbaee98 121 for(int i = 0; i<3; i++){
benkatz 2:25837cbaee98 122 for(int j = 0; j<3; j++){
benkatz 2:25837cbaee98 123 v[i] += J[i][j]*dq[j];
benkatz 2:25837cbaee98 124 }
benkatz 2:25837cbaee98 125 }
benkatz 2:25837cbaee98 126 }
benkatz 2:25837cbaee98 127
benkatz 0:d6186b8990c5 128 /// CAN Command Packet Structure ///
benkatz 0:d6186b8990c5 129 /// 16 bit position command, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 130 /// 12 bit velocity command, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 131 /// 12 bit kp, between 0 and 500 N-m/rad
benkatz 0:d6186b8990c5 132 /// 12 bit kd, between 0 and 100 N-m*s/rad
benkatz 0:d6186b8990c5 133 /// 12 bit feed forward torque, between -18 and 18 N-m
benkatz 0:d6186b8990c5 134 /// CAN Packet is 8 8-bit words
benkatz 0:d6186b8990c5 135 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 136 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 137 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 138 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 139 /// 3: [velocity[3-0], kp[11-8]]
benkatz 0:d6186b8990c5 140 /// 4: [kp[7-0]]
benkatz 0:d6186b8990c5 141 /// 5: [kd[11-4]]
benkatz 0:d6186b8990c5 142 /// 6: [kd[3-0], torque[11-8]]
benkatz 0:d6186b8990c5 143 /// 7: [torque[7-0]]
benkatz 0:d6186b8990c5 144
benkatz 0:d6186b8990c5 145 void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){
benkatz 2:25837cbaee98 146
benkatz 0:d6186b8990c5 147 /// limit data to be within bounds ///
benkatz 0:d6186b8990c5 148 p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
benkatz 0:d6186b8990c5 149 v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
benkatz 0:d6186b8990c5 150 kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
benkatz 0:d6186b8990c5 151 kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
benkatz 0:d6186b8990c5 152 t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
benkatz 0:d6186b8990c5 153 /// convert floats to unsigned ints ///
benkatz 0:d6186b8990c5 154 int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 155 int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
benkatz 0:d6186b8990c5 156 int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
benkatz 0:d6186b8990c5 157 int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
benkatz 0:d6186b8990c5 158 int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
benkatz 0:d6186b8990c5 159 /// pack ints into the can buffer ///
benkatz 0:d6186b8990c5 160 msg->data[0] = p_int>>8;
benkatz 0:d6186b8990c5 161 msg->data[1] = p_int&0xFF;
benkatz 0:d6186b8990c5 162 msg->data[2] = v_int>>4;
benkatz 0:d6186b8990c5 163 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
benkatz 0:d6186b8990c5 164 msg->data[4] = kp_int&0xFF;
benkatz 0:d6186b8990c5 165 msg->data[5] = kd_int>>4;
benkatz 0:d6186b8990c5 166 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
benkatz 0:d6186b8990c5 167 msg->data[7] = t_int&0xff;
benkatz 0:d6186b8990c5 168 }
benkatz 0:d6186b8990c5 169
benkatz 0:d6186b8990c5 170 /// CAN Reply Packet Structure ///
benkatz 0:d6186b8990c5 171 /// 16 bit position, between -4*pi and 4*pi
benkatz 0:d6186b8990c5 172 /// 12 bit velocity, between -30 and + 30 rad/s
benkatz 0:d6186b8990c5 173 /// 12 bit current, between -40 and 40;
benkatz 0:d6186b8990c5 174 /// CAN Packet is 5 8-bit words
benkatz 0:d6186b8990c5 175 /// Formatted as follows. For each quantity, bit 0 is LSB
benkatz 0:d6186b8990c5 176 /// 0: [position[15-8]]
benkatz 0:d6186b8990c5 177 /// 1: [position[7-0]]
benkatz 0:d6186b8990c5 178 /// 2: [velocity[11-4]]
benkatz 0:d6186b8990c5 179 /// 3: [velocity[3-0], current[11-8]]
benkatz 0:d6186b8990c5 180 /// 4: [current[7-0]]
benkatz 0:d6186b8990c5 181
benkatz 1:79e0d4791936 182 void unpack_reply(CANMessage msg, int leg_num){
benkatz 0:d6186b8990c5 183 /// unpack ints from can buffer ///
benkatz 0:d6186b8990c5 184 int id = msg.data[0];
benkatz 0:d6186b8990c5 185 int p_int = (msg.data[1]<<8)|msg.data[2];
benkatz 0:d6186b8990c5 186 int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
benkatz 0:d6186b8990c5 187 int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
benkatz 0:d6186b8990c5 188 /// convert ints to floats ///
benkatz 0:d6186b8990c5 189 float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
benkatz 0:d6186b8990c5 190 float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
benkatz 1:79e0d4791936 191 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
benkatz 2:25837cbaee98 192 float qraw = p;
benkatz 2:25837cbaee98 193 float vraw = v;
benkatz 2:25837cbaee98 194 if(id==3){ //Extra belt 28:18 belt reduction on the knees;
benkatz 2:25837cbaee98 195 p = -p*0.643f;
benkatz 2:25837cbaee98 196 v = -v*0.643f;
benkatz 2:25837cbaee98 197 }
benkatz 2:25837cbaee98 198 else if(id==1){
benkatz 2:25837cbaee98 199 p = -p;
benkatz 2:25837cbaee98 200 v = -v;
benkatz 2:25837cbaee98 201 }
benkatz 2:25837cbaee98 202 p = p+offset[id-1];
benkatz 2:25837cbaee98 203 if(leg_num == 0){
benkatz 2:25837cbaee98 204 q1raw[id-1] = qraw;
benkatz 2:25837cbaee98 205 dq1raw[id-1] = vraw;
benkatz 2:25837cbaee98 206 q1[id-1] = p;
benkatz 2:25837cbaee98 207 dq1[id-1] = v;
benkatz 2:25837cbaee98 208 }
benkatz 2:25837cbaee98 209 else if(leg_num==1){
benkatz 2:25837cbaee98 210 q2raw[id-1] = qraw;
benkatz 2:25837cbaee98 211 dq2raw[id-1] = vraw;
benkatz 2:25837cbaee98 212 q2[id-1] = p;
benkatz 2:25837cbaee98 213 dq2[id-1] = v;
benkatz 2:25837cbaee98 214 }
benkatz 0:d6186b8990c5 215
benkatz 1:79e0d4791936 216 /*
benkatz 0:d6186b8990c5 217 if(id == 2){
benkatz 0:d6186b8990c5 218 theta1 = p;
benkatz 0:d6186b8990c5 219 dtheta1 = v;
benkatz 0:d6186b8990c5 220 }
benkatz 0:d6186b8990c5 221 else if(id ==3){
benkatz 0:d6186b8990c5 222 theta2 = p;
benkatz 0:d6186b8990c5 223 dtheta2 = v;
benkatz 0:d6186b8990c5 224 }
benkatz 1:79e0d4791936 225 */
benkatz 0:d6186b8990c5 226
benkatz 1:79e0d4791936 227 //printf("%d %.3f %.3f %.3f\n\r", id, p, v, i);
benkatz 0:d6186b8990c5 228 }
benkatz 0:d6186b8990c5 229
benkatz 1:79e0d4791936 230 void rxISR1() {
benkatz 1:79e0d4791936 231 can1.read(rxMsg1); // read message into Rx message storage
benkatz 1:79e0d4791936 232 unpack_reply(rxMsg1, 0);
benkatz 0:d6186b8990c5 233 }
benkatz 1:79e0d4791936 234 void rxISR2(){
benkatz 1:79e0d4791936 235 can2.read(rxMsg2);
benkatz 1:79e0d4791936 236 unpack_reply(rxMsg2, 1);
benkatz 1:79e0d4791936 237 }
benkatz 0:d6186b8990c5 238
benkatz 1:79e0d4791936 239 void WriteAll(){
benkatz 2:25837cbaee98 240 //toggle = 1;
benkatz 1:79e0d4791936 241 can1.write(abad1);
benkatz 1:79e0d4791936 242 wait(.0001);
benkatz 1:79e0d4791936 243 can2.write(abad2);
benkatz 1:79e0d4791936 244 wait(.0001);
benkatz 1:79e0d4791936 245 can1.write(hip1);
benkatz 1:79e0d4791936 246 wait(.0001);
benkatz 1:79e0d4791936 247 can2.write(hip2);
benkatz 1:79e0d4791936 248 wait(.0001);
benkatz 1:79e0d4791936 249 can1.write(knee1);
benkatz 1:79e0d4791936 250 wait(.0001);
benkatz 1:79e0d4791936 251 can2.write(knee2);
benkatz 1:79e0d4791936 252 wait(.0001);
benkatz 2:25837cbaee98 253 //toggle = 0;
benkatz 1:79e0d4791936 254 }
benkatz 0:d6186b8990c5 255
benkatz 0:d6186b8990c5 256 void sendCMD(){
benkatz 0:d6186b8990c5 257 /// bilateral teleoperation demo ///
benkatz 2:25837cbaee98 258 toggle2 = 1;
benkatz 1:79e0d4791936 259 counter ++;
benkatz 2:25837cbaee98 260 scaling = .99f*scaling + .01f*knob.read();
benkatz 2:25837cbaee98 261
benkatz 2:25837cbaee98 262 kinematics(q1, dq1, p1, v1, J1, M1);
benkatz 2:25837cbaee98 263 kinematics(q2, dq2, p2, v2, J2, M2);
benkatz 1:79e0d4791936 264
benkatz 1:79e0d4791936 265 if(enabled){
benkatz 2:25837cbaee98 266 switch(control_mode){
benkatz 2:25837cbaee98 267 case 0:
benkatz 2:25837cbaee98 268 {
benkatz 2:25837cbaee98 269 KD1[0] = 0; KD1[1] = 0; KD1[2] = 0;
benkatz 2:25837cbaee98 270 KD2[0] = 0; KD2[1] = 0; KD2[2] = 0;
benkatz 2:25837cbaee98 271 tau1[0] = 0; tau1[1] = 0; tau1[2] = 0;
benkatz 2:25837cbaee98 272 tau2[0] = 0; tau2[1] = 0; tau2[2] = 0;
benkatz 2:25837cbaee98 273 pack_cmd(&abad1, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 274 pack_cmd(&abad2, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 275 pack_cmd(&hip1, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 276 pack_cmd(&hip2, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 277 pack_cmd(&knee1, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 278 pack_cmd(&knee2, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 279 }
benkatz 2:25837cbaee98 280 break;
benkatz 2:25837cbaee98 281 case 1:
benkatz 2:25837cbaee98 282 {
benkatz 2:25837cbaee98 283 //Joint Coupling
benkatz 2:25837cbaee98 284 KD1[0] = 0; KD1[1] = 0; KD1[2] = 0;
benkatz 2:25837cbaee98 285 KD2[0] = 0; KD2[1] = 0; KD2[2] = 0;
benkatz 2:25837cbaee98 286 tau1[0] = -scaling*(kp_q*(q2[0] - q1[0]) + kd_q*(dq2[0] - dq1[0]));
benkatz 2:25837cbaee98 287 tau2[0] = -scaling*(kp_q*(q1[0] - q2[0]) + kd_q*(dq1[0] - dq2[0]));
benkatz 2:25837cbaee98 288 tau1[1] = scaling*(kp_q*(q2[1] - q1[1]) + kd_q*(dq2[1] - dq1[1]));
benkatz 2:25837cbaee98 289 tau2[1] = scaling*(kp_q*(q1[1] - q2[1]) + kd_q*(dq1[1] - dq2[1]));
benkatz 2:25837cbaee98 290 tau1[2] = -scaling*((kp_q/1.5f)*(q2[2] - q1[2]) + (kd_q/2.25f)*(dq2[2] - dq1[2]));
benkatz 2:25837cbaee98 291 tau2[2] = -scaling*((kp_q/1.5f)*(q1[2] - q2[2]) + (kd_q/2.25f)*(dq1[2] - dq2[2]));
benkatz 2:25837cbaee98 292
benkatz 2:25837cbaee98 293 pack_cmd(&abad1, 0, 0, 0, KD1[0]+.005f, tau1[0]);
benkatz 2:25837cbaee98 294 pack_cmd(&abad2, 0, 0, 0, KD2[0]+.005f, tau2[0]);
benkatz 2:25837cbaee98 295 pack_cmd(&hip1, 0, 0, 0, KD1[1]+.005f, tau1[1]);
benkatz 2:25837cbaee98 296 pack_cmd(&hip2, 0, 0, 0, KD2[1]+.005f, tau2[1]);
benkatz 2:25837cbaee98 297 pack_cmd(&knee1, 0, 0, 0, KD1[2]+.0033f, tau1[2]);
benkatz 2:25837cbaee98 298 pack_cmd(&knee2, 0, 0, 0, KD2[2]+.0033f, tau2[2]);
benkatz 2:25837cbaee98 299 }
benkatz 2:25837cbaee98 300 break;
benkatz 2:25837cbaee98 301
benkatz 2:25837cbaee98 302 case 2:
benkatz 2:25837cbaee98 303 {
benkatz 2:25837cbaee98 304 //Virtual Walls
benkatz 2:25837cbaee98 305 const float kmax = 25000.0f;
benkatz 2:25837cbaee98 306 const float wn_des = 100000.0f;
benkatz 2:25837cbaee98 307 const float xlim = 0.0f;
benkatz 2:25837cbaee98 308 const float ylim = 0.2f;
benkatz 2:25837cbaee98 309 const float zlim = -.2f;
benkatz 2:25837cbaee98 310
benkatz 2:25837cbaee98 311 contact1[0] = p1[0]<xlim;
benkatz 2:25837cbaee98 312 contact2[0] = p2[0]<xlim;
benkatz 2:25837cbaee98 313 contact1[1] = p1[1]>ylim;
benkatz 2:25837cbaee98 314 contact2[1] = p2[1]>ylim;
benkatz 2:25837cbaee98 315 contact1[2] = p1[2]<zlim;
benkatz 2:25837cbaee98 316 contact2[2] = p2[2]<zlim;
benkatz 2:25837cbaee98 317
benkatz 2:25837cbaee98 318 float kx1 = wn_des/M1[0][0];
benkatz 2:25837cbaee98 319 float kx2 = wn_des/M2[0][0];
benkatz 2:25837cbaee98 320 kx1 = fminf(kmax, kx1);
benkatz 2:25837cbaee98 321 kx2 = fminf(kmax, kx2);
benkatz 2:25837cbaee98 322 f1[0] = scaling*(kx1*(xlim - p1[0]) + 0.0f*kd*(0 - v1[0]))*contact1[0];
benkatz 2:25837cbaee98 323 f2[0] = scaling*(kx2*(xlim - p2[0]) + 0.0f*kd*(0 - v2[0]))*contact2[0];
benkatz 2:25837cbaee98 324
benkatz 2:25837cbaee98 325 float ky1 = wn_des/M1[1][1];
benkatz 2:25837cbaee98 326 float ky2 = wn_des/M2[1][1];
benkatz 2:25837cbaee98 327 ky1 = fminf(kmax, ky1);
benkatz 2:25837cbaee98 328 ky2 = fminf(kmax, ky2);
benkatz 2:25837cbaee98 329 f1[1] = scaling*(ky1*(ylim - p1[1]) + 0.0f*kd*(0 - v1[1]))*contact1[1];
benkatz 2:25837cbaee98 330 f2[1] = scaling*(ky2*(ylim - p2[1]) + 0.0f*kd*(0 - v2[1]))*contact2[1];
benkatz 2:25837cbaee98 331
benkatz 2:25837cbaee98 332 float kz1 = wn_des/M1[2][2];
benkatz 2:25837cbaee98 333 float kz2 = wn_des/M2[2][2];
benkatz 2:25837cbaee98 334 kz1 = fminf(kmax, kz1);
benkatz 2:25837cbaee98 335 kz2 = fminf(kmax, kz2);
benkatz 2:25837cbaee98 336 f1[2] = scaling*(kz1*(zlim - p1[2]) + 0.0f*kd*(0 - v1[2]))*contact1[2];
benkatz 2:25837cbaee98 337 f2[2] = scaling*(kz2*(zlim - p2[2]) + 0.0f*kd*(0 - v2[2]))*contact2[2];
benkatz 2:25837cbaee98 338 //
benkatz 2:25837cbaee98 339
benkatz 2:25837cbaee98 340 tau1[0] = -1*(f1[0]*J1[0][0] + f1[1]*J1[1][0] + f1[2]*J1[2][0]);
benkatz 2:25837cbaee98 341 tau2[0] = -1*(f2[0]*J2[0][0] + f2[1]*J2[1][0] + f2[2]*J2[2][0]);
benkatz 2:25837cbaee98 342 tau1[1] = f1[0]*J1[0][1] + f1[1]*J1[1][1] + f1[2]*J1[2][1];
benkatz 2:25837cbaee98 343 tau2[1] = f2[0]*J2[0][1] + f2[1]*J2[1][1] + f2[2]*J2[2][1];
benkatz 2:25837cbaee98 344 tau1[2] = -1*(f1[0]*J1[0][2] + f1[1]*J1[1][2] + f1[2]*J1[2][2]);
benkatz 2:25837cbaee98 345 tau2[2] = -1*(f2[0]*J2[0][2] + f2[1]*J2[1][2] + f2[2]*J2[2][2]);
benkatz 2:25837cbaee98 346
benkatz 2:25837cbaee98 347 KD1[0] = 0.4f*(kd*scaling)*(contact1[0]*J1[0][0]*J1[0][0] + contact1[1]*J1[1][0]*J1[1][0] + contact1[2]*J1[2][0]*J1[2][0]);
benkatz 2:25837cbaee98 348 KD2[0] = 0.4f*(kd*scaling)*(contact2[0]*J2[0][0]*J2[0][0] + contact2[1]*J2[1][0]*J2[1][0] + contact2[2]*J2[2][0]*J2[2][0]);
benkatz 2:25837cbaee98 349 KD1[1] = 0.4f*(kd*scaling)*(contact1[0]*J1[0][1]*J1[0][1] + contact1[1]*J1[1][1]*J1[1][1] + contact1[2]*J1[2][1]*J1[2][1]);
benkatz 2:25837cbaee98 350 KD2[1] = 0.4f*(kd*scaling)*(contact2[0]*J2[0][1]*J2[0][1] + contact2[1]*J2[1][1]*J2[1][1] + contact2[2]*J2[2][1]*J2[2][1]);
benkatz 2:25837cbaee98 351 KD1[2] = 0.4f*0.44f*(kd*scaling)*(contact1[0]*J1[0][2]*J1[0][2] + contact1[1]*J1[1][2]*J1[1][2] + contact1[2]*J1[2][2]*J1[2][2]);
benkatz 2:25837cbaee98 352 KD2[2] = 0.4f*0.44f*(kd*scaling)*(contact2[0]*J2[0][2]*J2[0][2] + contact2[1]*J2[1][2]*J2[1][2] + contact2[2]*J2[2][2]*J2[2][2]);
benkatz 2:25837cbaee98 353
benkatz 2:25837cbaee98 354 pack_cmd(&abad1, 0, 0, 0, KD1[0]+.005f, tau1[0]);
benkatz 2:25837cbaee98 355 pack_cmd(&abad2, 0, 0, 0, KD2[0]+.005f, tau2[0]);
benkatz 2:25837cbaee98 356 pack_cmd(&hip1, 0, 0, 0, KD1[1]+.005f, tau1[1]);
benkatz 2:25837cbaee98 357 pack_cmd(&hip2, 0, 0, 0, KD2[1]+.005f, tau2[1]);
benkatz 2:25837cbaee98 358 pack_cmd(&knee1, 0, 0, 0, KD1[2]+.0033f, tau1[2]);
benkatz 2:25837cbaee98 359 pack_cmd(&knee2, 0, 0, 0, KD2[2]+.0033f, tau2[2]);
benkatz 2:25837cbaee98 360 }
benkatz 2:25837cbaee98 361 break;
benkatz 2:25837cbaee98 362
benkatz 2:25837cbaee98 363 case 3:
benkatz 2:25837cbaee98 364 {
benkatz 2:25837cbaee98 365 pack_cmd(&abad1, q2raw[0], dq2raw[0], 2.0f*kp_q*scaling, 2.0f*kd_q*scaling, 0);
benkatz 2:25837cbaee98 366 pack_cmd(&abad2, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 367 pack_cmd(&hip1, q2raw[1], dq2raw[1], 2.0f*kp_q*scaling, 2.0f*kd_q*scaling, 0);
benkatz 2:25837cbaee98 368 pack_cmd(&hip2, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 369 pack_cmd(&knee1, q2raw[2], dq2raw[2], 2.0f*kp_q*scaling, 2.0f*kd_q*scaling, 0);
benkatz 2:25837cbaee98 370 pack_cmd(&knee2, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 371 }
benkatz 2:25837cbaee98 372 break;
benkatz 2:25837cbaee98 373 //
benkatz 2:25837cbaee98 374
benkatz 2:25837cbaee98 375 }
benkatz 2:25837cbaee98 376
benkatz 2:25837cbaee98 377
benkatz 2:25837cbaee98 378
benkatz 2:25837cbaee98 379
benkatz 2:25837cbaee98 380
benkatz 2:25837cbaee98 381
benkatz 2:25837cbaee98 382
benkatz 2:25837cbaee98 383
benkatz 1:79e0d4791936 384 if(counter>100){
benkatz 1:79e0d4791936 385 //tcmd = -1*tcmd;
benkatz 2:25837cbaee98 386 //printf("%.4f %.4f \n\r", q1[1], q2[1]);
benkatz 2:25837cbaee98 387 //printf("%f\n\r", scaling);
benkatz 1:79e0d4791936 388 counter = 0 ;
benkatz 1:79e0d4791936 389 }
benkatz 1:79e0d4791936 390 /*
benkatz 1:79e0d4791936 391 pack_cmd(&abad1, q[0][1], dq[0][1], kp, kd, 0);
benkatz 1:79e0d4791936 392 pack_cmd(&abad2, q[0][0], dq[0][0], kp, kd, 0);
benkatz 1:79e0d4791936 393 pack_cmd(&hip1, q[1][1], dq[1][1], kp, kd, 0);
benkatz 1:79e0d4791936 394 pack_cmd(&hip2, q[1][0], dq[1][0], kp, kd, 0);
benkatz 1:79e0d4791936 395 pack_cmd(&knee1, q[2][1], dq[2][1], kp/1.5f, kd/2.25f, 0);
benkatz 1:79e0d4791936 396 pack_cmd(&knee2, q[2][0], dq[2][0], kp/1.5f, kd/2.25f, 0);
benkatz 1:79e0d4791936 397 */
benkatz 1:79e0d4791936 398 }
benkatz 1:79e0d4791936 399 /*
benkatz 1:79e0d4791936 400 pack_cmd(&abad1, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 401 pack_cmd(&abad2, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 402 pack_cmd(&hip1, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 403 pack_cmd(&hip2, 0, 0, 10, .1, 0);
benkatz 1:79e0d4791936 404 pack_cmd(&knee1, 0, 0, 6.6, .04, 0);
benkatz 1:79e0d4791936 405 pack_cmd(&knee2, 0, 0, 6.6, .04, 0);
benkatz 1:79e0d4791936 406 */
benkatz 2:25837cbaee98 407 toggle2 = 0;
benkatz 1:79e0d4791936 408 WriteAll();
benkatz 0:d6186b8990c5 409 }
benkatz 0:d6186b8990c5 410
benkatz 1:79e0d4791936 411 void Zero(CANMessage * msg){
benkatz 1:79e0d4791936 412 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 413 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 414 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 415 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 416 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 417 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 418 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 419 msg->data[7] = 0xFE;
benkatz 1:79e0d4791936 420 WriteAll();
benkatz 1:79e0d4791936 421 }
benkatz 1:79e0d4791936 422
benkatz 1:79e0d4791936 423 void EnterMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 424 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 425 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 426 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 427 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 428 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 429 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 430 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 431 msg->data[7] = 0xFC;
benkatz 1:79e0d4791936 432 WriteAll();
benkatz 1:79e0d4791936 433 }
benkatz 1:79e0d4791936 434
benkatz 1:79e0d4791936 435 void ExitMotorMode(CANMessage * msg){
benkatz 1:79e0d4791936 436 msg->data[0] = 0xFF;
benkatz 1:79e0d4791936 437 msg->data[1] = 0xFF;
benkatz 1:79e0d4791936 438 msg->data[2] = 0xFF;
benkatz 1:79e0d4791936 439 msg->data[3] = 0xFF;
benkatz 1:79e0d4791936 440 msg->data[4] = 0xFF;
benkatz 1:79e0d4791936 441 msg->data[5] = 0xFF;
benkatz 1:79e0d4791936 442 msg->data[6] = 0xFF;
benkatz 1:79e0d4791936 443 msg->data[7] = 0xFD;
benkatz 1:79e0d4791936 444 WriteAll();
benkatz 1:79e0d4791936 445 }
benkatz 0:d6186b8990c5 446 void serial_isr(){
benkatz 1:79e0d4791936 447 /// handle keyboard commands from the serial terminal ///
benkatz 0:d6186b8990c5 448 while(pc.readable()){
benkatz 0:d6186b8990c5 449 char c = pc.getc();
benkatz 0:d6186b8990c5 450 switch(c){
benkatz 0:d6186b8990c5 451 case(27):
benkatz 1:79e0d4791936 452 loop.detach();
benkatz 0:d6186b8990c5 453 printf("\n\r exiting motor mode \n\r");
benkatz 1:79e0d4791936 454 ExitMotorMode(&abad1);
benkatz 1:79e0d4791936 455 ExitMotorMode(&abad2);
benkatz 1:79e0d4791936 456 ExitMotorMode(&hip1);
benkatz 1:79e0d4791936 457 ExitMotorMode(&hip2);
benkatz 1:79e0d4791936 458 ExitMotorMode(&knee1);
benkatz 1:79e0d4791936 459 ExitMotorMode(&knee2);
benkatz 1:79e0d4791936 460 enabled = 0;
benkatz 0:d6186b8990c5 461 break;
benkatz 0:d6186b8990c5 462 case('m'):
benkatz 0:d6186b8990c5 463 printf("\n\r entering motor mode \n\r");
benkatz 1:79e0d4791936 464 EnterMotorMode(&abad1);
benkatz 1:79e0d4791936 465 Zero(&abad1);
benkatz 1:79e0d4791936 466 EnterMotorMode(&abad2);
benkatz 1:79e0d4791936 467 Zero(&abad2);
benkatz 1:79e0d4791936 468 EnterMotorMode(&hip1);
benkatz 1:79e0d4791936 469 Zero(&hip1);
benkatz 1:79e0d4791936 470 EnterMotorMode(&hip2);
benkatz 1:79e0d4791936 471 Zero(&hip2);
benkatz 1:79e0d4791936 472 EnterMotorMode(&knee1);
benkatz 1:79e0d4791936 473 Zero(&knee1);
benkatz 1:79e0d4791936 474 EnterMotorMode(&knee2);
benkatz 1:79e0d4791936 475 Zero(&knee2);
benkatz 1:79e0d4791936 476 wait(.5);
benkatz 1:79e0d4791936 477 enabled = 1;
benkatz 1:79e0d4791936 478 loop.attach(&sendCMD, .001);
benkatz 0:d6186b8990c5 479 break;
benkatz 0:d6186b8990c5 480 case('z'):
benkatz 0:d6186b8990c5 481 printf("\n\r zeroing \n\r");
benkatz 1:79e0d4791936 482 Zero(&abad1);
benkatz 1:79e0d4791936 483 Zero(&abad2);
benkatz 1:79e0d4791936 484 Zero(&hip1);
benkatz 1:79e0d4791936 485 Zero(&hip2);
benkatz 1:79e0d4791936 486 Zero(&knee1);
benkatz 1:79e0d4791936 487 Zero(&knee2);
benkatz 0:d6186b8990c5 488 break;
benkatz 2:25837cbaee98 489 case('0'):
benkatz 2:25837cbaee98 490 control_mode = 0;
benkatz 2:25837cbaee98 491 break;
benkatz 2:25837cbaee98 492 case('1'):
benkatz 2:25837cbaee98 493 control_mode = 1;
benkatz 2:25837cbaee98 494 break;
benkatz 2:25837cbaee98 495 case('2'):
benkatz 2:25837cbaee98 496 control_mode = 2;
benkatz 2:25837cbaee98 497 break;
benkatz 2:25837cbaee98 498 case('3'):
benkatz 2:25837cbaee98 499 control_mode = 3;
benkatz 2:25837cbaee98 500 break;
benkatz 0:d6186b8990c5 501 }
benkatz 0:d6186b8990c5 502 }
benkatz 1:79e0d4791936 503 WriteAll();
benkatz 0:d6186b8990c5 504
benkatz 0:d6186b8990c5 505 }
benkatz 0:d6186b8990c5 506
benkatz 0:d6186b8990c5 507 int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
benkatz 0:d6186b8990c5 508 int main() {
benkatz 2:25837cbaee98 509 //wait(.5);
benkatz 2:25837cbaee98 510
benkatz 0:d6186b8990c5 511 pc.baud(921600);
benkatz 0:d6186b8990c5 512 pc.attach(&serial_isr);
benkatz 1:79e0d4791936 513 can1.frequency(1000000); // set bit rate to 1Mbps
benkatz 1:79e0d4791936 514 can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler
benkatz 1:79e0d4791936 515 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 1:79e0d4791936 516 can2.frequency(1000000); // set bit rate to 1Mbps
benkatz 1:79e0d4791936 517 can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler
benkatz 1:79e0d4791936 518 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
benkatz 1:79e0d4791936 519
benkatz 1:79e0d4791936 520 printf("\n\r Master\n\r");
benkatz 0:d6186b8990c5 521 //printf("%d\n\r", RX_ID << 18);
benkatz 1:79e0d4791936 522 abad1.len = 8; //transmit 8 bytes
benkatz 1:79e0d4791936 523 abad2.len = 8; //transmit 8 bytes
benkatz 1:79e0d4791936 524 hip1.len = 8;
benkatz 1:79e0d4791936 525 hip2.len = 8;
benkatz 1:79e0d4791936 526 knee1.len = 8;
benkatz 1:79e0d4791936 527 knee2.len = 8;
benkatz 1:79e0d4791936 528 rxMsg1.len = 6; //receive 5 bytes
benkatz 1:79e0d4791936 529 rxMsg2.len = 6; //receive 5 bytes
benkatz 1:79e0d4791936 530
benkatz 2:25837cbaee98 531
benkatz 1:79e0d4791936 532 abad1.id = 0x1;
benkatz 1:79e0d4791936 533 abad2.id = 0x1;
benkatz 1:79e0d4791936 534 hip1.id = 0x2;
benkatz 1:79e0d4791936 535 hip2.id = 0x2;
benkatz 1:79e0d4791936 536 knee1.id = 0x3;
benkatz 1:79e0d4791936 537 knee2.id = 0x3;
benkatz 1:79e0d4791936 538 pack_cmd(&abad1, 0, 0, 0, 0, 0); //Start out by sending all 0's
benkatz 1:79e0d4791936 539 pack_cmd(&abad2, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 540 pack_cmd(&hip1, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 541 pack_cmd(&hip2, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 542 pack_cmd(&knee1, 0, 0, 0, 0, 0);
benkatz 1:79e0d4791936 543 pack_cmd(&knee2, 0, 0, 0, 0, 0);
benkatz 2:25837cbaee98 544 //WriteAll();
benkatz 0:d6186b8990c5 545
benkatz 2:25837cbaee98 546 wait(.5);
benkatz 2:25837cbaee98 547 EnterMotorMode(&abad1);
benkatz 2:25837cbaee98 548 EnterMotorMode(&abad2);
benkatz 2:25837cbaee98 549 EnterMotorMode(&hip1);
benkatz 2:25837cbaee98 550 EnterMotorMode(&hip2);
benkatz 2:25837cbaee98 551 EnterMotorMode(&knee1);
benkatz 2:25837cbaee98 552 EnterMotorMode(&knee2);
benkatz 2:25837cbaee98 553 Zero(&knee2);
benkatz 2:25837cbaee98 554 Zero(&knee1);
benkatz 2:25837cbaee98 555 Zero(&hip1);
benkatz 2:25837cbaee98 556 Zero(&hip2);
benkatz 2:25837cbaee98 557 Zero(&abad2);
benkatz 2:25837cbaee98 558 Zero(&abad1);
benkatz 2:25837cbaee98 559
benkatz 2:25837cbaee98 560
benkatz 2:25837cbaee98 561
benkatz 2:25837cbaee98 562 wait(.5);
benkatz 2:25837cbaee98 563 enabled = 1;
benkatz 2:25837cbaee98 564 loop.attach(&sendCMD, .001);
benkatz 0:d6186b8990c5 565 while(1) {
benkatz 2:25837cbaee98 566
benkatz 0:d6186b8990c5 567
benkatz 0:d6186b8990c5 568 }
benkatz 0:d6186b8990c5 569
benkatz 0:d6186b8990c5 570 }
benkatz 0:d6186b8990c5 571
benkatz 0:d6186b8990c5 572
benkatz 0:d6186b8990c5 573
benkatz 0:d6186b8990c5 574
benkatz 0:d6186b8990c5 575