main.cpp

00001 
00002 #define CAN_ID 0x1
00003 
00004 #include "mbed.h"
00005 #include "math_ops.h"
00006 
00007 
00008 Serial       pc(PA_2, PA_3);
00009 CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
00010 CANMessage   rxMsg;
00011 CANMessage   txMsg1;
00012 CANMessage    txMsg2;
00013 int                     ledState;
00014 Timer                   timer;
00015 Ticker                  sendCAN;
00016 int                     counter = 0;
00017 volatile bool           msgAvailable = false;
00018 Ticker loop;
00019 
00020  float theta1, theta2, dtheta1, dtheta2;
00021 
00022 /// Value Limits ///
00023 #define P_MIN -12.5f
00024  #define P_MAX 12.5f
00025  #define V_MIN -45.0f
00026  #define V_MAX 45.0f
00027  #define KP_MIN 0.0f
00028  #define KP_MAX 500.0f
00029  #define KD_MIN 0.0f
00030  #define KD_MAX 5.0f
00031  #define T_MIN -18.0f
00032  #define T_MAX 18.0f
00033  
00034 /// CAN Command Packet Structure ///
00035 /// 16 bit position command, between -4*pi and 4*pi
00036 /// 12 bit velocity command, between -30 and + 30 rad/s
00037 /// 12 bit kp, between 0 and 500 N-m/rad
00038 /// 12 bit kd, between 0 and 100 N-m*s/rad
00039 /// 12 bit feed forward torque, between -18 and 18 N-m
00040 /// CAN Packet is 8 8-bit words
00041 /// Formatted as follows.  For each quantity, bit 0 is LSB
00042 /// 0: [position[15-8]]
00043 /// 1: [position[7-0]] 
00044 /// 2: [velocity[11-4]]
00045 /// 3: [velocity[3-0], kp[11-8]]
00046 /// 4: [kp[7-0]]
00047 /// 5: [kd[11-4]]
00048 /// 6: [kd[3-0], torque[11-8]]
00049 /// 7: [torque[7-0]]
00050 
00051 void pack_cmd(CANMessage * msg, float p_des, float v_des, float kp, float kd, float t_ff){
00052      /// limit data to be within bounds ///
00053      p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);                    
00054      v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
00055      kp = fminf(fmaxf(KP_MIN, kp), KP_MAX);
00056      kd = fminf(fmaxf(KD_MIN, kd), KD_MAX);
00057      t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);
00058      /// convert floats to unsigned ints ///
00059      int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
00060      int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
00061      int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
00062      int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
00063      int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
00064      /// pack ints into the can buffer ///
00065      msg->data[0] = p_int>>8;                                       
00066      msg->data[1] = p_int&0xFF;
00067      msg->data[2] = v_int>>4;
00068      msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
00069      msg->data[4] = kp_int&0xFF;
00070      msg->data[5] = kd_int>>4;
00071      msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
00072      msg->data[7] = t_int&0xff;
00073      }
00074      
00075 /// CAN Reply Packet Structure ///
00076 /// 16 bit position, between -4*pi and 4*pi
00077 /// 12 bit velocity, between -30 and + 30 rad/s
00078 /// 12 bit current, between -40 and 40;
00079 /// CAN Packet is 5 8-bit words
00080 /// Formatted as follows.  For each quantity, bit 0 is LSB
00081 /// 0: [position[15-8]]
00082 /// 1: [position[7-0]] 
00083 /// 2: [velocity[11-4]]
00084 /// 3: [velocity[3-0], current[11-8]]
00085 /// 4: [current[7-0]]
00086 
00087 void unpack_reply(CANMessage msg){
00088     /// unpack ints from can buffer ///
00089     int id = msg.data[0];
00090     int p_int = (msg.data[1]<<8)|msg.data[2];
00091     int v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
00092     int i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
00093     /// convert ints to floats ///
00094     float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
00095     float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
00096     float i = uint_to_float(i_int, -I_MAX, I_MAX, 12);
00097     
00098     if(id == 2){
00099         theta1 = p;
00100         dtheta1 = v;
00101         }
00102     else if(id ==3){
00103         theta2 = p;
00104         dtheta2 = v;
00105         }
00106     
00107     //printf("%d  %.3f   %.3f   %.3f\n\r", id, p, v, i);
00108     } 
00109     
00110  void onMsgReceived() {
00111     can.read(rxMsg);                    // read message into Rx message storage
00112     unpack_reply(rxMsg);
00113 }
00114 
00115 
00116 void sendCMD(){
00117     /// bilateral teleoperation demo ///
00118     pack_cmd(&txMsg1, 0, 0, 0, 0, -0.1f); 
00119     //pack_cmd(&txMsg2, theta1, dtheta1, 10, .1, 0);
00120     //pack_cmd(&txMsg2, 0, 0, 0, 0, 1.0);
00121     //pack_cmd(&txMsg1, 0, 0, 0, 0, 1.0);
00122     //can.write(txMsg2);
00123     wait(.0003);        // Give motor 1 time to respond.
00124     can.write(txMsg1);
00125     }
00126     
00127 void serial_isr(){
00128      /// hangle keyboard commands from the serial terminal ///
00129      while(pc.readable()){
00130         char c = pc.getc();
00131         switch(c){
00132             case(27):
00133                 printf("\n\r exiting motor mode \n\r");
00134                 txMsg1.data[0] = 0xFF;
00135                 txMsg1.data[1] = 0xFF;
00136                 txMsg1.data[2] = 0xFF;
00137                 txMsg1.data[3] = 0xFF;
00138                 txMsg1.data[4] = 0xFF;
00139                 txMsg1.data[5] = 0xFF;
00140                 txMsg1.data[6] = 0xFF;
00141                 txMsg1.data[7] = 0xFD;
00142                 
00143                 txMsg2.data[0] = 0xFF;
00144                 txMsg2.data[1] = 0xFF;
00145                 txMsg2.data[2] = 0xFF;
00146                 txMsg2.data[3] = 0xFF;
00147                 txMsg2.data[4] = 0xFF;
00148                 txMsg2.data[5] = 0xFF;
00149                 txMsg2.data[6] = 0xFF;
00150                 txMsg2.data[7] = 0xFD;
00151                 break;
00152             case('m'):
00153                 printf("\n\r entering motor mode \n\r");
00154                 txMsg1.data[0] = 0xFF;
00155                 txMsg1.data[1] = 0xFF;
00156                 txMsg1.data[2] = 0xFF;
00157                 txMsg1.data[3] = 0xFF;
00158                 txMsg1.data[4] = 0xFF;
00159                 txMsg1.data[5] = 0xFF;
00160                 txMsg1.data[6] = 0xFF;
00161                 txMsg1.data[7] = 0xFC;
00162                 
00163                 txMsg2.data[0] = 0xFF;
00164                 txMsg2.data[1] = 0xFF;
00165                 txMsg2.data[2] = 0xFF;
00166                 txMsg2.data[3] = 0xFF;
00167                 txMsg2.data[4] = 0xFF;
00168                 txMsg2.data[5] = 0xFF;
00169                 txMsg2.data[6] = 0xFF;
00170                 txMsg2.data[7] = 0xFC;
00171                 break;
00172             case('z'):
00173                 printf("\n\r zeroing \n\r");
00174                 txMsg1.data[0] = 0xFF;
00175                 txMsg1.data[1] = 0xFF;
00176                 txMsg1.data[2] = 0xFF;
00177                 txMsg1.data[3] = 0xFF;
00178                 txMsg1.data[4] = 0xFF;
00179                 txMsg1.data[5] = 0xFF;
00180                 txMsg1.data[6] = 0xFF;
00181                 txMsg1.data[7] = 0xFE;
00182                 
00183                 txMsg2.data[0] = 0xFF;
00184                 txMsg2.data[1] = 0xFF;
00185                 txMsg2.data[2] = 0xFF;
00186                 txMsg2.data[3] = 0xFF;
00187                 txMsg2.data[4] = 0xFF;
00188                 txMsg2.data[5] = 0xFF;
00189                 txMsg2.data[6] = 0xFF;
00190                 txMsg2.data[7] = 0xFE;
00191                 break;
00192             }
00193         }
00194         can.write(txMsg1);
00195         can.write(txMsg2);
00196         
00197     }
00198     
00199 int can_packet[8] = {1, 2, 3, 4, 5, 6, 7, 8};
00200 int main() {
00201     pc.baud(921600);
00202     pc.attach(&serial_isr);
00203     can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
00204     can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
00205     printf("Master\n\r");
00206     //printf("%d\n\r", RX_ID << 18);
00207     int count = 0;
00208     txMsg1.len = 8;                         //transmit 8 bytes
00209     txMsg2.len = 8;                         //transmit 8 bytes
00210     rxMsg.len = 6;                          //receive 5 bytes
00211     loop.attach(&sendCMD, .001);
00212     txMsg1.id = 0x2;                        //1st motor ID
00213     txMsg2.id = 0x3;                        //2nd motor ID
00214     pack_cmd(&txMsg1, 0, 0, 0, 0, 0);       //Start out by sending all 0's
00215     pack_cmd(&txMsg2, 0, 0, 0, 0, 0);
00216     timer.start();
00217     
00218     while(1) {
00219 
00220         }
00221         
00222     }
00223     
00224 
00225 
00226 
00227