Ben Katz
bilateral feedback demo
Fork of
CanMasterTest
by 
Ben Katz
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 00002 #define CAN_ID 0x0 00003 00004 #include "mbed.h" 00005 #include "math_ops.h" 00006 00007 00008 Serial pc(PA_2, PA_3); 00009 CAN can1(PB_8, PB_9); // CAN Rx pin name, CAN Tx pin name 00010 CAN can2(PB_5, PB_13); // CAN Rx pin name, CAN Tx pin name 00011 CANMessage rxMsg1, rxMsg2; 00012 CANMessage abad1, abad2, hip1, hip2, knee1, knee2; //TX Messages 00013 int ledState; 00014 Ticker sendCAN; 00015 int counter = 0; 00016 volatile bool msgAvailable = false; 00017 Ticker loop; 00018 AnalogIn knob(PC_0); 00019 DigitalOut toggle(PC_3); 00020 DigitalOut toggle2(PC_2); 00021 00022 ///[[abad1, abad2] 00023 ///[hip1, hip2] 00024 ///[knee1, knee2]] 00025 float q1raw[3]; 00026 float q2raw[3]; 00027 float dq1raw[3]; 00028 float dq2raw[3]; 00029 float q1[3]; //Leg 1 joint angles 00030 float dq1[3]; //Leg 1 joint velocities 00031 float tau1[3]; //Leg 1 joint torques 00032 float q2[3]; //Leg 2 joint angles 00033 float dq2[3]; //Leg 2 joint velocities 00034 float tau2[3]; //Leg 2 joint torques 00035 float p1[3]; //Leg 1 end effector position 00036 float v1[3]; //Leg 1 end effector velocity 00037 float J1[3][3]; //Leg 1 Jacobian 00038 float f1[3]; //Leg 1 end effector forces 00039 float p2[3]; //Leg 2 end effector position 00040 float v2[3]; //Leg 2 end effector velocity 00041 float J2[3][3]; //Leg 2 Jacobian 00042 float f2[3]; //Leg 2 end effector forces 00043 00044 float q[3][2]; //Joint states for both legs 00045 float dq[3][2]; 00046 float tau[3][3]; 00047 00048 float I[3] = {0.005f, 0.0045f, 0.006f}; //Joint space inertias 00049 float M1[3][3]; //Leg 1 end effector inverse mass 00050 float M2[3][3]; //Leg 2 end effector inverse mass 00051 float KD1[3]; //Joint space damping 00052 float KD2[3]; 00053 float contact1[3]; 00054 float contact2[3]; 00055 00056 const float offset[3] = {0.0f, 3.493f, -2.766f}; //Joint angle offsets at zero position 00057 00058 float kp = 800.0f; 00059 float kd = 100.0f; 00060 float kp_q = 100.0f; 00061 float kd_q = 0.8f; 00062 int enabled = 0; 00063 float scaling = 0; 00064 00065 00066 int control_mode = 1; 00067 00068 /// Value Limits /// 00069 #define P_MIN -12.5f 00070 #define P_MAX 12.5f 00071 #define V_MIN -30.0f 00072 #define V_MAX 30.0f 00073 #define KP_MIN 0.0f 00074 #define KP_MAX 500.0f 00075 #define KD_MIN 0.0f 00076 #define KD_MAX 5.0f 00077 #define T_MIN -18.0f 00078 #define T_MAX 18.0f 00079 #define I_MAX 40.0f 00080 00081 #define L1 0.0577f 00082 #define L2 0.2088f 00083 #define L3 0.175f 00084 00085 void kinematics(const float q[3], const float dq[3], float* p, float* v, float (* J)[3], float (* M)[3]){ 00086 const float s1 = sinf(q[0]); 00087 const float s2 = sinf(q[1]); 00088 const float s3 = sinf(q[2]); 00089 const float c1 = cosf(q[0]); 00090 const float c2 = cosf(q[1]); 00091 const float c3 = cosf(q[2]); 00092 00093 const float c23 = c2*c3 - s2*s3; 00094 const float s23 = s2*c3 + c2*s3; 00095 00096 p[0] = L3*s23 + L2*s2; 00097 p[1] = L1*c1 + L3*s1*c23 + L2*c2*s1; 00098 p[2] = L1*s1 - L3*c1*c23 - L2*c1*c2; 00099 00100 J[0][0] = 0; 00101 J[0][1] = L3*c23 + L2*c2; 00102 J[0][2] = L3*c23; 00103 J[1][0] = L3*c1*c23 + L2*c1*c2 - L1*s1; 00104 J[1][1] = -L3*s1*s23 - L2*s1*s2; 00105 J[1][2] = -L3*s1*s23; 00106 J[2][0] = L3*s1*c23 + L2*c2*s1 + L1*c1; 00107 J[2][1] = L3*c1*s23 + L2*c1*s2; 00108 J[2][2] = L3*c1*s23; 00109 00110 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]; 00111 M[0][1] = 0; 00112 M[0][2] = 0; 00113 M[1][0] = 0; 00114 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]; 00115 M[1][2] = 0; 00116 M[2][0] = 0; 00117 M[2][1] = 0; 00118 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]; 00119 00120 v[0] = 0; v[1] = 0; v[2] = 0; 00121 for(int i = 0; i<3; i++){ 00122 for(int j = 0; j<3; j++){ 00123 v[i] += J[i][j]*dq[j]; 00124 } 00125 } 00126 } 00127 00128 /// CAN Command Packet Structure /// 00129 /// 16 bit position command, between -4*pi and 4*pi 00130 /// 12 bit velocity command, between -30 and + 30 rad/s 00131 /// 12 bit kp, between 0 and 500 N-m/rad 00132 /// 12 bit kd, between 0 and 100 N-m*s/rad 00133 /// 12 bit feed forward torque, between -18 and 18 N-m 00134 /// CAN Packet is 8 8-bit words 00135 /// Formatted as follows. For each quantity, bit 0 is LSB 00136 /// 0: [position[15-8]] 00137 /// 1: [position[7-0]] 00138 /// 2: [velocity[11-4]] 00139 /// 3: [velocity[3-0], kp[11-8]] 00140 /// 4: [kp[7-0]] 00141 /// 5: [kd[11-4]] 00142 /// 6: [kd[3-0], torque[11-8]] 00143 /// 7: [torque[7-0]] 00144 00145 void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){ 00146 00147 /// limit data to be within bounds /// 00148 p_des = fminf(fmaxf(P_MIN, p_des), P_MAX); 00149 v_des = fminf(fmaxf(V_MIN, v_des), V_MAX); 00150 kp = fminf(fmaxf(KP_MIN, kp), KP_MAX); 00151 kd = fminf(fmaxf(KD_MIN, kd), KD_MAX); 00152 t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX); 00153 /// convert floats to unsigned ints /// 00154 int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16); 00155 int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12); 00156 int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12); 00157 int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12); 00158 int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12); 00159 /// pack ints into the can buffer /// 00160 msg->data[0] = p_int>>8; 00161 msg->data[1] = p_int&0xFF; 00162 msg->data[2] = v_int>>4; 00163 msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8); 00164 msg->data[4] = kp_int&0xFF; 00165 msg->data[5] = kd_int>>4; 00166 msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8); 00167 msg->data[7] = t_int&0xff; 00168 } 00169 00170 /// CAN Reply Packet Structure /// 00171 /// 16 bit position, between -4*pi and 4*pi 00172 /// 12 bit velocity, between -30 and + 30 rad/s 00173 /// 12 bit current, between -40 and 40; 00174 /// CAN Packet is 5 8-bit words 00175 /// Formatted as follows. For each quantity, bit 0 is LSB 00176 /// 0: [position[15-8]] 00177 /// 1: [position[7-0]] 00178 /// 2: [velocity[11-4]] 00179 /// 3: [velocity[3-0], current[11-8]] 00180 /// 4: [current[7-0]] 00181 00182 void unpack_reply(CANMessage msg, int leg_num){ 00183 /// unpack ints from can buffer /// 00184 int id = msg.data[0]; 00185 int p_int = (msg.data[1]<<8)|msg.data[2]; 00186 int v_int = (msg.data[3]<<4)|(msg.data[4]>>4); 00187 int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5]; 00188 /// convert ints to floats /// 00189 float p = uint_to_float(p_int, P_MIN, P_MAX, 16); 00190 float v = uint_to_float(v_int, V_MIN, V_MAX, 12); 00191 float t = uint_to_float(i_int, -T_MAX, T_MAX, 12); 00192 float qraw = p; 00193 float vraw = v; 00194 if(id==3){ //Extra belt 28:18 belt reduction on the knees; 00195 p = -p*0.643f; 00196 v = -v*0.643f; 00197 } 00198 else if(id==1){ 00199 p = -p; 00200 v = -v; 00201 } 00202 p = p+offset[id-1]; 00203 if(leg_num == 0){ 00204 q1raw[id-1] = qraw; 00205 dq1raw[id-1] = vraw; 00206 q1[id-1] = p; 00207 dq1[id-1] = v; 00208 } 00209 else if(leg_num==1){ 00210 q2raw[id-1] = qraw; 00211 dq2raw[id-1] = vraw; 00212 q2[id-1] = p; 00213 dq2[id-1] = v; 00214 } 00215 00216 /* 00217 if(id == 2){ 00218 theta1 = p; 00219 dtheta1 = v; 00220 } 00221 else if(id ==3){ 00222 theta2 = p; 00223 dtheta2 = v; 00224 } 00225 */ 00226 00227 //printf("%d %.3f %.3f %.3f\n\r", id, p, v, i); 00228 } 00229 00230 void rxISR1() { 00231 can1.read(rxMsg1); // read message into Rx message storage 00232 unpack_reply(rxMsg1, 0); 00233 } 00234 void rxISR2(){ 00235 can2.read(rxMsg2); 00236 unpack_reply(rxMsg2, 1); 00237 } 00238 00239 void WriteAll(){ 00240 //toggle = 1; 00241 wait(.0001); 00242 can1.write(abad1); 00243 wait(.0001); 00244 can2.write(abad2); 00245 wait(.0001); 00246 can1.write(hip1); 00247 wait(.0001); 00248 can2.write(hip2); 00249 wait(.0001); 00250 can1.write(knee1); 00251 wait(.0001); 00252 can2.write(knee2); 00253 wait(.0001); 00254 //toggle = 0; 00255 } 00256 00257 void sendCMD(){ 00258 /// bilateral teleoperation demo /// 00259 toggle2 = 1; 00260 counter ++; 00261 scaling = .99f*scaling + .01f*knob.read(); 00262 00263 kinematics(q1, dq1, p1, v1, J1, M1); 00264 kinematics(q2, dq2, p2, v2, J2, M2); 00265 00266 if(enabled){ 00267 switch(control_mode){ 00268 case 0: 00269 { 00270 KD1[0] = 0; KD1[1] = 0; KD1[2] = 0; 00271 KD2[0] = 0; KD2[1] = 0; KD2[2] = 0; 00272 tau1[0] = 0; tau1[1] = 0; tau1[2] = 0; 00273 tau2[0] = 0; tau2[1] = 0; tau2[2] = 0; 00274 pack_cmd(&abad1, 0, 0, 0, 0, 0); 00275 pack_cmd(&abad2, 0, 0, 0, 0, 0); 00276 pack_cmd(&hip1, 0, 0, 0, 0, 0); 00277 pack_cmd(&hip2, 0, 0, 0, 0, 0); 00278 pack_cmd(&knee1, 0, 0, 0, 0, 0); 00279 pack_cmd(&knee2, 0, 0, 0, 0, 0); 00280 } 00281 break; 00282 case 1: 00283 { 00284 //Joint Coupling 00285 KD1[0] = 0; KD1[1] = 0; KD1[2] = 0; 00286 KD2[0] = 0; KD2[1] = 0; KD2[2] = 0; 00287 float deltaq1 = q2[0] - q1[0]; 00288 float deltaq2 = q2[1] - q1[1]; 00289 float deltaq3 = q2[2] - q1[2]; 00290 /* 00291 tau1[0] = -scaling*(kp_q*(deltaq1 + 1000.0f*deltaq1*abs(deltaq1)) + kd_q*(dq2[0] - dq1[0])); 00292 tau2[0] = -scaling*(kp_q*(-(deltaq1 + 1000.0f*deltaq1*abs(deltaq1))) + kd_q*(dq1[0] - dq2[0])); 00293 tau1[1] = scaling*(kp_q*(deltaq2+1000.0f*deltaq2*abs(deltaq2)) + kd_q*(dq2[1] - dq1[1])); 00294 tau2[1] = scaling*(kp_q*(-(deltaq2+1000.0f*deltaq2*abs(deltaq2))) + kd_q*(dq1[1] - dq2[1])); 00295 tau1[2] = -scaling*((kp_q/1.5f)*(deltaq3+1000.0f*deltaq3*abs(deltaq3)) + (kd_q/2.25f)*(dq2[2] - dq1[2])); 00296 tau2[2] = -scaling*((kp_q/1.5f)*(-(deltaq3+1000.0f*deltaq3*abs(deltaq3))) + (kd_q/2.25f)*(dq1[2] - dq2[2])); 00297 */ 00298 00299 tau1[0] = -scaling*(kp_q*(q2[0] - q1[0]) + kd_q*(dq2[0] - dq1[0])); 00300 tau2[0] = -scaling*(kp_q*(q1[0] - q2[0]) + kd_q*(dq1[0] - dq2[0])); 00301 tau1[1] = scaling*(kp_q*(q2[1] - q1[1]) + kd_q*(dq2[1] - dq1[1])); 00302 tau2[1] = scaling*(kp_q*(q1[1] - q2[1]) + kd_q*(dq1[1] - dq2[1])); 00303 tau1[2] = -scaling*((kp_q/1.5f)*(q2[2] - q1[2]) + (kd_q/2.25f)*(dq2[2] - dq1[2])); 00304 tau2[2] = -scaling*((kp_q/1.5f)*(q1[2] - q2[2]) + (kd_q/2.25f)*(dq1[2] - dq2[2])); 00305 00306 pack_cmd(&abad1, 0, 0, 0, KD1[0]+.005f, tau1[0]); 00307 pack_cmd(&abad2, 0, 0, 0, KD2[0]+.005f, tau2[0]); 00308 pack_cmd(&hip1, 0, 0, 0, KD1[1]+.005f, tau1[1]); 00309 pack_cmd(&hip2, 0, 0, 0, KD2[1]+.005f, tau2[1]); 00310 pack_cmd(&knee1, 0, 0, 0, KD1[2]+.0033f, tau1[2]); 00311 pack_cmd(&knee2, 0, 0, 0, KD2[2]+.0033f, tau2[2]); 00312 00313 //printf("%f %f\n\r", tau1[1], 10.0f*deltaq2*abs(deltaq2)); 00314 } 00315 break; 00316 00317 case 2: 00318 { 00319 //Virtual Walls 00320 const float kmax = 25000.0f; 00321 const float wn_des = 100000.0f; 00322 const float xlim = 0.0f; 00323 const float ylim = 0.2f; 00324 const float zlim = -.2f; 00325 00326 contact1[0] = p1[0]<xlim; 00327 contact2[0] = p2[0]<xlim; 00328 contact1[1] = p1[1]>ylim; 00329 contact2[1] = p2[1]>ylim; 00330 contact1[2] = p1[2]<zlim; 00331 contact2[2] = p2[2]<zlim; 00332 00333 float kx1 = wn_des/M1[0][0]; 00334 float kx2 = wn_des/M2[0][0]; 00335 kx1 = fminf(kmax, kx1); 00336 kx2 = fminf(kmax, kx2); 00337 f1[0] = scaling*(kx1*(xlim - p1[0]) + 0.03f*kd*(0 - v1[0]))*contact1[0]; 00338 f2[0] = scaling*(kx2*(xlim - p2[0]) + 0.03f*kd*(0 - v2[0]))*contact2[0]; 00339 00340 float ky1 = wn_des/M1[1][1]; 00341 float ky2 = wn_des/M2[1][1]; 00342 ky1 = fminf(kmax, ky1); 00343 ky2 = fminf(kmax, ky2); 00344 f1[1] = scaling*(ky1*(ylim - p1[1]) + 0.03f*kd*(0 - v1[1]))*contact1[1]; 00345 f2[1] = scaling*(ky2*(ylim - p2[1]) + 0.03f*kd*(0 - v2[1]))*contact2[1]; 00346 00347 float kz1 = wn_des/M1[2][2]; 00348 float kz2 = wn_des/M2[2][2]; 00349 kz1 = fminf(kmax, kz1); 00350 kz2 = fminf(kmax, kz2); 00351 f1[2] = scaling*(kz1*(zlim - p1[2]) + 0.03f*kd*(0 - v1[2]))*contact1[2]; 00352 f2[2] = scaling*(kz2*(zlim - p2[2]) + 0.03f*kd*(0 - v2[2]))*contact2[2]; 00353 // 00354 00355 tau1[0] = -1*(f1[0]*J1[0][0] + f1[1]*J1[1][0] + f1[2]*J1[2][0]); 00356 tau2[0] = -1*(f2[0]*J2[0][0] + f2[1]*J2[1][0] + f2[2]*J2[2][0]); 00357 tau1[1] = f1[0]*J1[0][1] + f1[1]*J1[1][1] + f1[2]*J1[2][1]; 00358 tau2[1] = f2[0]*J2[0][1] + f2[1]*J2[1][1] + f2[2]*J2[2][1]; 00359 tau1[2] = -1*(f1[0]*J1[0][2] + f1[1]*J1[1][2] + f1[2]*J1[2][2]); 00360 tau2[2] = -1*(f2[0]*J2[0][2] + f2[1]*J2[1][2] + f2[2]*J2[2][2]); 00361 00362 KD1[0] = 0.01f*(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]); 00363 KD2[0] = 0.01f*(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]); 00364 KD1[1] = 0.01f*(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]); 00365 KD2[1] = 0.01f*(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]); 00366 KD1[2] = 0.01f*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]); 00367 KD2[2] = 0.01f*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]); 00368 00369 pack_cmd(&abad1, 0, 0, 0, KD1[0]+.005f, tau1[0]); 00370 pack_cmd(&abad2, 0, 0, 0, KD2[0]+.005f, tau2[0]); 00371 pack_cmd(&hip1, 0, 0, 0, KD1[1]+.005f, tau1[1]); 00372 pack_cmd(&hip2, 0, 0, 0, KD2[1]+.005f, tau2[1]); 00373 pack_cmd(&knee1, 0, 0, 0, KD1[2]+.0033f, tau1[2]); 00374 pack_cmd(&knee2, 0, 0, 0, KD2[2]+.0033f, tau2[2]); 00375 } 00376 break; 00377 00378 case 3: 00379 { 00380 pack_cmd(&abad1, q2raw[0], dq2raw[0], 2.0f*kp_q*scaling, 2.0f*kd_q*scaling, 0); 00381 pack_cmd(&abad2, 0, 0, 0, 0, 0); 00382 pack_cmd(&hip1, q2raw[1], dq2raw[1], 2.0f*kp_q*scaling, 2.0f*kd_q*scaling, 0); 00383 pack_cmd(&hip2, 0, 0, 0, 0, 0); 00384 pack_cmd(&knee1, q2raw[2], dq2raw[2], 2.0f*kp_q*scaling, 2.0f*kd_q*scaling, 0); 00385 pack_cmd(&knee2, 0, 0, 0, 0, 0); 00386 } 00387 break; 00388 // 00389 00390 } 00391 00392 00393 00394 00395 00396 00397 00398 00399 if(counter>100){ 00400 //tcmd = -1*tcmd; 00401 printf("%.3f %.3f %.3f %.3f %.3f %.3f \n\r", q1[0], q1[1], q1[2], q2[0], q2[1], q2[2]); 00402 //printf("%f\n\r", scaling); 00403 counter = 0 ; 00404 } 00405 /* 00406 pack_cmd(&abad1, q[0][1], dq[0][1], kp, kd, 0); 00407 pack_cmd(&abad2, q[0][0], dq[0][0], kp, kd, 0); 00408 pack_cmd(&hip1, q[1][1], dq[1][1], kp, kd, 0); 00409 pack_cmd(&hip2, q[1][0], dq[1][0], kp, kd, 0); 00410 pack_cmd(&knee1, q[2][1], dq[2][1], kp/1.5f, kd/2.25f, 0); 00411 pack_cmd(&knee2, q[2][0], dq[2][0], kp/1.5f, kd/2.25f, 0); 00412 */ 00413 } 00414 /* 00415 pack_cmd(&abad1, 0, 0, 10, .1, 0); 00416 pack_cmd(&abad2, 0, 0, 10, .1, 0); 00417 pack_cmd(&hip1, 0, 0, 10, .1, 0); 00418 pack_cmd(&hip2, 0, 0, 10, .1, 0); 00419 pack_cmd(&knee1, 0, 0, 6.6, .04, 0); 00420 pack_cmd(&knee2, 0, 0, 6.6, .04, 0); 00421 */ 00422 toggle2 = 0; 00423 WriteAll(); 00424 } 00425 00426 void Zero(CANMessage * msg){ 00427 msg->data[0] = 0xFF; 00428 msg->data[1] = 0xFF; 00429 msg->data[2] = 0xFF; 00430 msg->data[3] = 0xFF; 00431 msg->data[4] = 0xFF; 00432 msg->data[5] = 0xFF; 00433 msg->data[6] = 0xFF; 00434 msg->data[7] = 0xFE; 00435 //WriteAll(); 00436 } 00437 00438 void EnterMotorMode(CANMessage * msg){ 00439 msg->data[0] = 0xFF; 00440 msg->data[1] = 0xFF; 00441 msg->data[2] = 0xFF; 00442 msg->data[3] = 0xFF; 00443 msg->data[4] = 0xFF; 00444 msg->data[5] = 0xFF; 00445 msg->data[6] = 0xFF; 00446 msg->data[7] = 0xFC; 00447 //WriteAll(); 00448 } 00449 00450 void ExitMotorMode(CANMessage * msg){ 00451 msg->data[0] = 0xFF; 00452 msg->data[1] = 0xFF; 00453 msg->data[2] = 0xFF; 00454 msg->data[3] = 0xFF; 00455 msg->data[4] = 0xFF; 00456 msg->data[5] = 0xFF; 00457 msg->data[6] = 0xFF; 00458 msg->data[7] = 0xFD; 00459 WriteAll(); 00460 } 00461 void serial_isr(){ 00462 /// handle keyboard commands from the serial terminal /// 00463 while(pc.readable()){ 00464 char c = pc.getc(); 00465 switch(c){ 00466 case(27): 00467 loop.detach(); 00468 printf("\n\r exiting motor mode \n\r"); 00469 ExitMotorMode(&abad1); 00470 ExitMotorMode(&abad2); 00471 ExitMotorMode(&hip1); 00472 ExitMotorMode(&hip2); 00473 ExitMotorMode(&knee1); 00474 ExitMotorMode(&knee2); 00475 enabled = 0; 00476 break; 00477 case('m'): 00478 printf("\n\r entering motor mode \n\r"); 00479 EnterMotorMode(&abad1); 00480 EnterMotorMode(&abad2); 00481 EnterMotorMode(&hip1); 00482 EnterMotorMode(&hip2); 00483 EnterMotorMode(&knee1); 00484 EnterMotorMode(&knee2); 00485 WriteAll(); 00486 00487 Zero(&abad1); 00488 Zero(&abad2); 00489 Zero(&hip1); 00490 Zero(&hip2); 00491 Zero(&knee1); 00492 Zero(&knee2); 00493 WriteAll(); 00494 wait(.5); 00495 enabled = 1; 00496 loop.attach(&sendCMD, .001); 00497 break; 00498 case('z'): 00499 printf("\n\r zeroing \n\r"); 00500 Zero(&abad1); 00501 can1.write(abad1); 00502 Zero(&abad2); 00503 can2.write(abad2); 00504 Zero(&hip1); 00505 can1.write(hip1); 00506 Zero(&hip2); 00507 can2.write(hip2); 00508 Zero(&knee1); 00509 can1.write(knee1); 00510 Zero(&knee2); 00511 can2.write(knee2); 00512 break; 00513 case('0'): 00514 control_mode = 0; 00515 break; 00516 case('1'): 00517 control_mode = 1; 00518 break; 00519 case('2'): 00520 control_mode = 2; 00521 break; 00522 case('3'): 00523 control_mode = 3; 00524 break; 00525 } 00526 } 00527 WriteAll(); 00528 00529 } 00530 00531 int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8}; 00532 int main() { 00533 //wait(.5); 00534 00535 pc.baud(921600); 00536 pc.attach(&serial_isr); 00537 can1.frequency(1000000); // set bit rate to 1Mbps 00538 can1.attach(&rxISR1); // attach 'CAN receive-complete' interrupt handler 00539 can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter 00540 can2.frequency(1000000); // set bit rate to 1Mbps 00541 can2.attach(&rxISR2); // attach 'CAN receive-complete' interrupt handler 00542 can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter 00543 00544 printf("\n\r Master\n\r"); 00545 //printf("%d\n\r", RX_ID << 18); 00546 abad1.len = 8; //transmit 8 bytes 00547 abad2.len = 8; //transmit 8 bytes 00548 hip1.len = 8; 00549 hip2.len = 8; 00550 knee1.len = 8; 00551 knee2.len = 8; 00552 rxMsg1.len = 6; //receive 5 bytes 00553 rxMsg2.len = 6; //receive 5 bytes 00554 00555 00556 abad1.id = 0x1; 00557 abad2.id = 0x1; 00558 hip1.id = 0x2; 00559 hip2.id = 0x2; 00560 knee1.id = 0x3; 00561 knee2.id = 0x3; 00562 pack_cmd(&abad1, 0, 0, 0, 0, 0); //Start out by sending all 0's 00563 pack_cmd(&abad2, 0, 0, 0, 0, 0); 00564 pack_cmd(&hip1, 0, 0, 0, 0, 0); 00565 pack_cmd(&hip2, 0, 0, 0, 0, 0); 00566 pack_cmd(&knee1, 0, 0, 0, 0, 0); 00567 pack_cmd(&knee2, 0, 0, 0, 0, 0); 00568 //WriteAll(); 00569 00570 wait(.5); 00571 EnterMotorMode(&abad1); 00572 EnterMotorMode(&abad2); 00573 EnterMotorMode(&hip1); 00574 EnterMotorMode(&hip2); 00575 EnterMotorMode(&knee1); 00576 EnterMotorMode(&knee2); 00577 WriteAll(); 00578 wait(.1); 00579 Zero(&abad1); 00580 can1.write(abad1); 00581 Zero(&abad2); 00582 can2.write(abad2); 00583 Zero(&hip1); 00584 can1.write(hip1); 00585 Zero(&hip2); 00586 can2.write(hip2); 00587 Zero(&knee1); 00588 can1.write(knee1); 00589 Zero(&knee2); 00590 can2.write(knee2); 00591 00592 00593 00594 00595 wait(.5); 00596 enabled = 1; 00597 loop.attach(&sendCMD, .001); 00598 while(1) { 00599 00600 00601 } 00602 00603 } 00604 00605 00606 00607 00608
Generated on Wed Jul 13 2022 16:10:33 by
1.7.2