main.cpp

00001 #define CAN_ID 0x0
00002 #include "mbed.h"
00003 #include "math_ops.h"
00004 #include "MotorModule.h"
00005 
00006 
00007 
00008 Serial       pc(PA_2, PA_3);                // Serial port to the computer
00009 CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
00010 
00011 Ticker loop_ctrl;                                // Control loop interrupt handler
00012 Ticker loop_msg;
00013 
00014 #define MArty               1
00015 #define DT_ctrl             .001f             // Control loop period
00016 #define DT_msg              .01f             // msg loop period    
00017 #define N_MOTORS            1               // Number of motors on the can bus
00018 
00019 MotorStruct motors[N_MOTORS];               // Create a list of the motors attached
00020 
00021 /* Communication functions.  Do not touch */
00022 void onMsgReceived();
00023 void init_motors(int ids[N_MOTORS]);
00024 
00025 int loop_counter = 0;
00026 int newprint = 1;
00027 int nskip = 5;  
00028 int skip_counter = 0;
00029 
00030 float t = 0.0;
00031 float A=  0.5;
00032 float f = 1.0;
00033 double mycur = 0.35;
00034 bool flagCtrl = true;
00035 
00036 void printnew(){
00037     newprint = 1;
00038 }
00039 
00040 void control(){ 
00041     t = DT_ctrl*loop_counter;  //time in seconds
00042     if (flagCtrl) {
00043         // if control is ON
00044         if (t<1) {
00045             motors[0].control.p_des = 0;
00046             motors[0].control.v_des = 0;
00047             motors[0].control.kp = 0;
00048             motors[0].control.kd = 0;
00049         }
00050         if (1<=t<20){
00051             motors[0].control.i_ff = mycur;
00052             motors[0].control.p_des = 0;
00053             motors[0].control.kd = 0;
00054             motors[0].control.kp = 0;
00055         }
00056         for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
00057             pack_cmd(&motors[i]);
00058             can.write(motors[i].txMsg);
00059         }
00060     } else {
00061         // if control is OFF
00062         motors[0].control.i_ff = 0.0f;  //all set to zero (no control)
00063         motors[0].control.kp = 0.0f;
00064         motors[0].control.kd = 0.0f;
00065         
00066         loop_counter = 0.0f;            //re-zero loop timing 
00067         
00068         for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
00069             pack_cmd(&motors[i]);
00070             can.write(motors[i].txMsg);
00071         }
00072     }
00073     loop_counter++;     // increase loop counter
00074 }
00075 
00076 int main() 
00077 {
00078     /* Setup & Initialization */
00079     pc.baud(921600);                            // Set baud rate for communication over USB serial
00080     can.attach(&onMsgReceived);                 // attach 'CAN receive-complete' interrupt handler
00081     can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
00082 
00083     
00084     int ids[N_MOTORS] = {1};                 // List of motor CAN ID's
00085     init_motors(ids);                           // Initialize the list of motors
00086     
00087     enable_motor(&motors[0], &can);             // Enable motors
00088     //enable_motor(&motors[1], &can);
00089     
00090     zero_motor(&motors[0], &can);               //Zero motors
00091     //zero_motor(&motors[1], &can);
00092     wait(1);                                    // Wait 1 second
00093     
00094     //disable_motor(&motors[0], &can);            // Disable first motor
00095     //disable_motor(&motors[1], &can); 
00096     
00097     /* Interrupt Enables */
00098     loop_ctrl.attach(&control, DT_ctrl);                 // Start running the contorl interrupt at 1/DT Hz
00099     loop_msg.attach(&printnew, DT_msg);    
00100     
00101     while(1) {
00102         if (pc.readable()) {
00103             char c = pc.getc();
00104             if(c == 's') {
00105                 flagCtrl = false;
00106                 printf("s");
00107             }
00108             if(c == 'g') {
00109                 flagCtrl = true;
00110                 printf("g");
00111             }
00112             if(c == 'e') {
00113                 mycur = mycur +0.1;
00114             }
00115             if(c == 'r') {
00116                 mycur = mycur - 0.1;
00117             }
00118 
00119         }
00120         if(newprint == 1){
00121             printf("%f \t%f \t%f \r\n", t, motors[0].state.position, motors[0].state.current);
00122             //pc.putc(motors[0].state.position);
00123             //pc.write(&motors[0].state.position);
00124             //dat1.f = motors[0].state.position;
00125             //pc.printf("%c%c%c%c", dat1.s[0], dat1.s[1], dat1.s[2], dat1.s[3]);
00126             newprint = 0;
00127         }
00128     }
00129 }
00130     
00131 /* low-level communication functoins below.  Do not touch */
00132 
00133  void onMsgReceived() 
00134 /* This interrupt gets called when a CAN message with ID CAN_ID shows up */
00135 {
00136     CANMessage   rxMsg;
00137     rxMsg.len = 6;
00138     can.read(rxMsg);                    // read message into Rx message storage
00139     int id = rxMsg.data[0];
00140     for (int i = 0; i< N_MOTORS; i++)
00141     {
00142         if(motors[i].control.id == id)
00143         {
00144             memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
00145             unpack_reply(&motors[i]);
00146         }
00147     }
00148 }
00149 
00150 void init_motors(int ids[N_MOTORS])
00151 /* Initialize buffer lengths and IDs of the motors in the list */
00152 {
00153     for(int i = 0; i<N_MOTORS; i++)
00154     {
00155         motors[i].txMsg.len = 8;
00156         motors[i].rxMsg.len = 6;
00157         motors[i].control.id = ids[i];
00158         motors[i].txMsg.id = ids[i];
00159         motors[i].control.p_des = 0;
00160         motors[i].control.v_des = 0;
00161         motors[i].control.kp = 0;
00162         motors[i].control.kd = 0;
00163         motors[i].control.i_ff = 0;
00164     }
00165 }