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