main.cpp

00001 /// high-bandwidth 3-phase motor control, for robots
00002 /// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others
00003 /// Hardware documentation can be found at build-its.blogspot.com
00004 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
00005 
00006 #define REST_MODE 0
00007 #define CALIBRATION_MODE 1
00008 #define MOTOR_MODE 2
00009 #define SETUP_MODE 4
00010 #define ENCODER_MODE 5
00011 #define TEST_MODE 6    // this mode is used to test motor, which allow user control motor's torque directly on the GUI software
00012 
00013 #define VERSION_NUM "1.6"
00014 
00015 
00016 float __float_reg[64];                                                          // Floats stored in flash
00017 int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
00018 
00019 #include "mbed.h"
00020 #include "PositionSensor.h"
00021 #include "structs.h"
00022 #include "foc.h"
00023 #include "calibration.h"
00024 #include "hw_setup.h"
00025 #include "math_ops.h" 
00026 #include "current_controller_config.h"
00027 #include "hw_config.h"
00028 #include "motor_config.h"
00029 #include "stm32f4xx_flash.h"
00030 #include "FlashWriter.h"
00031 #include "user_config.h"
00032 #include "PreferenceWriter.h"
00033 #include "CAN_com.h"
00034 #include <string>
00035  
00036 PreferenceWriter prefs(6);
00037 
00038 GPIOStruct gpio;
00039 ControllerStruct controller;
00040 COMStruct com;
00041 ObserverStruct observer;
00042 
00043 // Serial pc(PA_2, PA_3);
00044 Serial pc(PC_6, PC_7); //SPI1_MISO SPI1_MOSI?
00045 
00046 DataPackage datapackage;
00047 
00048 CAN          can(PB_8, PB_9, 1000000);      // CAN Rx pin name, CAN Tx pin name
00049 CANMessage   rxMsg;
00050 CANMessage   txMsg;
00051 
00052 DigitalOut led_debug(PB_1);
00053 
00054 PositionSensorAM5147 spi(16384, 0.0, NPP);  // int CPR, float offset, int ppairs
00055 
00056 //volatile int count = 0;
00057 volatile int state = REST_MODE;
00058 volatile int state_change;
00059 
00060 volatile float test_tff,test_kp,test_kd,test_pos_des,test_vel_des;
00061 volatile float test_ia,test_ib,test_ic,test_id,test_iq;
00062 volatile float test_mech_angle,test_mech_speed;
00063 
00064 void onMsgReceived() {
00065     //msgAvailable = true;
00066     
00067     can.read(rxMsg);  
00068     if((rxMsg.id == CAN_ID)){
00069         controller.timeout = 0;
00070         if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC))){
00071             state = MOTOR_MODE;
00072             state_change = 1;
00073             }
00074         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){
00075             state = REST_MODE;
00076             state_change = 1;
00077             gpio.led->write(0);
00078             }
00079         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
00080             spi.ZeroPosition();
00081             }
00082         else if(state == MOTOR_MODE){
00083             unpack_cmd(rxMsg, &controller);
00084             }
00085         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
00086         can.write(txMsg);
00087         }
00088     
00089 }
00090 
00091 void enter_menu_state(void){
00092     pc.printf("\n\r\n\r\n\r");
00093     pc.printf(" Commands:\n\r");
00094     wait_us(10);
00095     pc.printf(" m - Motor Mode\n\r");
00096     wait_us(10);
00097     pc.printf(" c - Calibrate Encoder\n\r");
00098     wait_us(10);
00099     pc.printf(" s - Setup\n\r");
00100     wait_us(10);
00101     pc.printf(" e - Display Encoder\n\r");
00102     wait_us(10);
00103     pc.printf(" z - Set Zero Position\n\r");
00104     wait_us(10);
00105     pc.printf(" t - Motor Test Mode\n\r");
00106     wait_us(10);
00107     pc.printf(" esc - Exit to Menu\n\r");
00108     wait_us(10);
00109     state_change = 0;
00110     gpio.enable->write(0);
00111     gpio.led->write(0);
00112     }
00113 
00114 void enter_setup_state(void){
00115     pc.printf("\n\r\n\r Configuration Options \n\r\n\n");
00116     wait_us(10);
00117     pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
00118     wait_us(10);
00119     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
00120     wait_us(10);
00121     pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
00122     wait_us(10);
00123     pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
00124     wait_us(10);
00125     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
00126     wait_us(10);
00127     pc.printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
00128     wait_us(10);
00129     pc.printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
00130     wait_us(10);
00131     state_change = 0;
00132     }
00133 
00134 void enter_test_state(void){
00135     pc.printf("\n\r\n\r Control Options \n\r\n\n");
00136     wait_us(10);
00137     pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
00138     wait_us(10);
00139     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "t", "Torque", "-18", "18", controller.t_ff);
00140     wait_us(10);
00141     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "k", "Stiffness", "0", "500", controller.kp);
00142     wait_us(10);
00143     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "d", "Damping", "0", "5.0", controller.kd);
00144     wait_us(10);
00145     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "p", "Position", "-12.5", "12.5", controller.p_des);
00146     wait_us(10);
00147     pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Velocity", "-45", "45", controller.v_des);
00148     wait_us(10);
00149     state_change = 0;
00150     }
00151     
00152 void enter_torque_mode(void){
00153     controller.ovp_flag = 0;                                                    // ???
00154     gpio.enable->write(1);                                                      // Enable gate drive
00155     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
00156     wait(.001);
00157     controller.i_d_ref = 0;
00158     controller.i_q_ref = 0;                                                     // Current Setpoints
00159     gpio.led->write(1);                                                     // Turn on status LED
00160     state_change = 0;
00161     pc.printf("\n\r Entering Motor Mode \n\r");
00162     }
00163     
00164 void calibrate(void){
00165     gpio.enable->write(1);                                                      // Enable gate drive
00166     gpio.led->write(1);                                                    // Turn on status LED
00167     order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
00168     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
00169     gpio.led->write(0);                                                         // Turn off status LED
00170     wait(.2);
00171     gpio.enable->write(0);                                                      // Turn off gate drive
00172     pc.printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
00173      state_change = 0;
00174     }
00175     
00176 void print_encoder(void){
00177     pc.printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
00178     wait(.05);
00179     }
00180 
00181 /// Current Sampling Interrupt ///
00182 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
00183 int LLLL = 0;
00184 bool led_flag = 0;
00185 bool flag_test_first = true;
00186 
00187 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
00188   if (TIM1->SR & TIM_SR_UIF ) {
00189 
00190         ///Sample current always ///
00191         ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
00192         //volatile int delay;   
00193         //for (delay = 0; delay < 55; delay++);
00194         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
00195         controller.adc1_raw = ADC1->DR;
00196         controller.adc3_raw = ADC3->DR;
00197         spi.Sample(DT);                                                           // sample position sensor
00198         controller.theta_elec = spi.GetElecPosition();
00199         controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
00200         controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
00201         controller.dtheta_elec = spi.GetElecVelocity();
00202         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
00203         ///
00204         LLLL ++;
00205         if(LLLL==10000){
00206             LLLL=0;
00207             led_flag = 1-led_flag; 
00208             led_debug=(led_flag);
00209             // pc.printf("in interrupt\n\r");
00210             }
00211         
00212         /// Check state machine state, and run the appropriate function ///
00213         switch(state){
00214             case REST_MODE:                                                     // Do nothing
00215                 if(state_change){
00216                     enter_menu_state();
00217                     flag_test_first = true;
00218                     }
00219                 break;
00220             
00221             case CALIBRATION_MODE:                                              // Run encoder calibration procedure
00222                 if(state_change){
00223                     calibrate();
00224                     }
00225                 break;
00226              
00227             case MOTOR_MODE:                                                   // Run torque control
00228                 if(state_change){
00229                     enter_torque_mode();
00230 //                    count = 0;
00231                     }
00232                 
00233                 else{
00234                 /*
00235                 if(controller.v_bus>28.0f){         //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen
00236                     gpio.enable->write(0);
00237                     controller.ovp_flag = 1;
00238                     state = REST_MODE;
00239                     state_change = 1;
00240                     printf("OVP Triggered!\n\r");
00241                     }
00242                     */  
00243                 // controller.t_ff = 1;
00244                 torque_control(&controller);    
00245                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
00246                     controller.i_d_ref = 0;
00247                     controller.i_q_ref = 0;
00248                     controller.kp = 0;
00249                     controller.kd = 0;
00250                     controller.t_ff = 0;
00251                     } 
00252                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
00253                 controller.timeout += 1;
00254                 
00255                 /*
00256                 count++;
00257                 if(count == 4000){
00258                      printf("%.4f\n\r", controller.dtheta_mech);
00259                      count = 0;
00260                      }
00261                      */
00262                 }
00263                   
00264                 break;
00265             case SETUP_MODE:
00266                 if(state_change){
00267                     enter_setup_state();
00268                 }
00269                 break;
00270             
00271             case TEST_MODE:
00272                 if(flag_test_first){
00273                     // enter_test_state();  
00274                     // enter_torque_mode();  
00275                     controller.ovp_flag = 0;
00276                     gpio.enable->write(1);                                                      // Enable gate drive
00277                     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
00278                     wait(.001);
00279                     controller.i_d_ref = 0;
00280                     controller.i_q_ref = 0;                                                     // Current Setpoints
00281                     gpio.led->write(1);
00282                     flag_test_first = false;                                                     // Turn on status LED
00283                 }
00284                 else{
00285                      controller.t_ff = test_tff;
00286                      controller.kp = test_kp;
00287                      controller.p_des = test_pos_des;
00288                      controller.kd = test_kd;
00289                      controller.v_des = test_vel_des;
00290                      
00291                      torque_control(&controller);  
00292                      commutate(&controller, &observer, &gpio, controller.theta_elec); 
00293                      test_mech_angle = controller.theta_mech;
00294                      test_mech_speed = controller.dtheta_mech;
00295                      test_ia = controller.i_a;
00296                      test_ib = controller.i_b;
00297                      test_ic = controller.i_c;
00298 //                     test_ia = observer.i_q_est;
00299 //                     test_ib = controller.v_d;
00300 //                     test_ic = controller.v_q;
00301                      test_id = controller.i_d;
00302                      test_iq = controller.i_q;
00303 //                     test_iq = observer.i_q_est;
00304 //                     test_id = observer.i_d_est;
00305 //                     test_iq = controller->i_q_filt；
00306                 }
00307                 break;
00308             
00309             case ENCODER_MODE:
00310                 print_encoder();
00311                 break;
00312                 }                 
00313       }
00314   TIM1->SR = 0x0;                                                               // reset the status register
00315 }
00316 
00317 
00318 char cmd_val[8] = {0};
00319 char cmd_id = 0;
00320 char char_count = 0;
00321 
00322 /// Manage state machine with commands from serial terminal or configurator gui ///
00323 /// Called when data received over serial ///
00324 void serial_interrupt(void){
00325     while(pc.readable()){
00326         
00327         char c = pc.getc();
00328         
00329         if(c == 27){
00330             state = REST_MODE;
00331             state_change = 1;
00332             char_count = 0;
00333             cmd_id = 0;
00334             gpio.led->write(0);; 
00335             for(int i = 0; i<8; i++){cmd_val[i] = 0;}
00336         }
00337         
00338         if(state == REST_MODE){
00339             switch (c){
00340                 case 'c':
00341                     state = CALIBRATION_MODE;
00342                     state_change = 1;
00343                     break;
00344                 case 'm':
00345                     state = MOTOR_MODE;
00346                     state_change = 1;
00347                     break;
00348                 case 'e':
00349                     state = ENCODER_MODE;
00350                     state_change = 1;
00351                     break;
00352                 case 's':
00353                     state = SETUP_MODE;
00354                     state_change = 1;
00355                     break;
00356                 case 'z':
00357                     spi.SetMechOffset(0);
00358                     spi.Sample(DT);
00359                     wait_us(20);
00360                     M_OFFSET = spi.GetMechPosition();
00361                     if (!prefs.ready()) prefs.open();
00362                         prefs.flush();                                                  // Write new prefs to flash
00363                         prefs.close();    
00364                         prefs.load(); 
00365                     spi.SetMechOffset(M_OFFSET);
00366                     pc.printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
00367                     
00368                     break;
00369                 case 't':
00370                     // this state is used to test motor through GUI
00371                     state = TEST_MODE;
00372                     state_change = 1;
00373             }
00374         }
00375         
00376         else if(state == SETUP_MODE){
00377             if(c == 13){
00378                 switch (cmd_id){
00379                     case 'b':
00380                         I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
00381                         break;
00382                     case 'i':
00383                         CAN_ID = atoi(cmd_val);
00384                         break;
00385                     case 'm':
00386                         CAN_MASTER = atoi(cmd_val);
00387                         break;
00388                     case 'l':
00389                         TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
00390                         break;
00391                     case 't':
00392                         CAN_TIMEOUT = atoi(cmd_val);
00393                         break;
00394                     default:
00395                         pc.printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
00396                         break;
00397                 }
00398                     
00399                 if (!prefs.ready()) prefs.open();
00400                 prefs.flush();                                                  // Write new prefs to flash
00401                 prefs.close();    
00402                 prefs.load();                                              
00403                 state_change = 1;
00404                 char_count = 0;
00405                 cmd_id = 0;
00406                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
00407             }
00408             else{
00409                 if(char_count == 0){cmd_id = c;}
00410                 else{
00411                     cmd_val[char_count-1] = c;
00412                     
00413                 }
00414                 pc.putc(c);
00415                 char_count++;
00416                 }
00417             }
00418         
00419         else if (state == ENCODER_MODE){
00420             switch (c){
00421                 case 27:
00422                     state = REST_MODE;
00423                     state_change = 1;
00424                     break;
00425             }
00426         }
00427         
00428         else if(state == TEST_MODE){
00429             if(c == 13){
00430                 switch(cmd_id){
00431                     case 't':
00432                         // controller.t_ff = fmaxf(fminf(atof(cmd_val),  TORQUE_LIMIT), -TORQUE_LIMIT);
00433                         test_tff = fmaxf(fminf(atof(cmd_val),  TORQUE_LIMIT), -TORQUE_LIMIT);
00434                         break;
00435                     case 'k':
00436                         // controller.kp = fmaxf(fminf(atof(cmd_val),  KP_MAX), KP_MIN);
00437                         test_kp = fmaxf(fminf(atof(cmd_val),  KP_MAX), KP_MIN);
00438                         break;
00439                     case 'd':
00440                         // controller.kd = fmaxf(fminf(atof(cmd_val),  KD_MAX), KD_MIN);
00441                         test_kd = fmaxf(fminf(atof(cmd_val),  KD_MAX), KD_MIN);
00442                         break;
00443                     case 'p':
00444                         // controller.p_des = fmaxf(fminf(atof(cmd_val),  P_MAX), P_MIN);
00445                         test_pos_des = fmaxf(fminf(atof(cmd_val),  P_MAX), P_MIN);
00446                         break;
00447                     case 'v':
00448                         // controller.v_des = fmaxf(fminf(atof(cmd_val),  V_MAX), V_MIN);
00449                         test_vel_des = fmaxf(fminf(atof(cmd_val),  V_MAX), V_MIN);
00450                         break;
00451                     default:
00452                         // pc.printf("\n\r '%c' Not a valid command prefix\n\r", cmd_id);
00453                         break;
00454                 }
00455                 state_change = 1;
00456                 char_count = 0;
00457                 cmd_id = 0;
00458                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
00459             }
00460             
00461             else{
00462                 if(char_count == 0){
00463                     cmd_id = c;
00464                 }
00465                 else{
00466                     cmd_val[char_count-1] = c;
00467                 }
00468                 char_count++;
00469             }
00470         }    
00471     }
00472 }
00473        
00474 int main() {
00475     
00476     controller.v_bus = V_BUS;
00477     controller.mode = 0;
00478     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
00479 
00480     wait(.1);
00481     gpio.enable->write(1);
00482     TIM1->CCR3 = PWM_ARR*(1.0f);                        // Write duty cycles
00483     TIM1->CCR2 = PWM_ARR*(1.0f);
00484     TIM1->CCR1 = PWM_ARR*(1.0f);
00485     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
00486     gpio.enable->write(0);
00487     reset_foc(&controller);                                                     // Reset current controller
00488     TIM1->CR1 ^= TIM_CR1_UDIS;                                                  // ???
00489     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
00490     
00491     wait(.1);
00492     NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);                                             // commutation > communication
00493     
00494     NVIC_SetPriority(CAN1_RX0_IRQn, 3);
00495     can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00496                                                                     
00497     txMsg.id = CAN_MASTER;
00498     txMsg.len = 6;
00499     rxMsg.len = 8;
00500     can.attach(&onMsgReceived);                                     // attach 'CAN receive-complete' interrupt handler    ???
00501     
00502     prefs.load();                                                               // Read flash ???
00503     if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
00504     if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
00505     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
00506     spi.SetMechOffset(M_OFFSET);
00507     int lut[128] = {0};
00508     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
00509     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
00510     
00511     pc.baud(921600);                                                            // set serial baud rate
00512     // pc.baud(460800);
00513     
00514     wait(.01);
00515     pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r");
00516     wait(.01);
00517     pc.printf("\n\r Debug Info:\n\r");
00518     pc.printf(" Firmware Version: %s\n\r", VERSION_NUM);
00519     pc.printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
00520     pc.printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
00521     pc.printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
00522     pc.printf(" CAN ID:  %d\n\r", CAN_ID);
00523         
00524     pc.attach(&serial_interrupt);                                               // attach serial interrupt
00525     
00526     // =============================================================================================================
00527     // add watch dog here
00528     // Watchdog &watchdog = Watchdog::get_instance();
00529     // =============================================================================================================
00530     
00531     state_change = 1;
00532 
00533     datapackage.info.init_flag1 = 0xffffffff;
00534     datapackage.info.init_flag2 = 0xfffefdfc;
00535     datapackage.info.check_num = 0;
00536     while(1) {
00537         // pc.printf("hello_wooden");
00538         // pc.printf("I's still working\n\r");
00539         // pc.printf("adc1: %d, adc2: %d, adc3: %d",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw);
00540         if(state == TEST_MODE){
00541 //            datapackage.info.I_a = controller.i_a;
00542 //            datapackage.info.I_b = controller.i_b;
00543 //            datapackage.info.I_c = controller.i_c;
00544 //            datapackage.info.I_q = controller.i_q;
00545 //            datapackage.info.I_d = controller.i_d;
00546 //            datapackage.info.Angle_mech = controller.theta_mech;
00547 //            datapackage.info.Angle_elec = controller.theta_elec;
00548 //            datapackage.info.Control_tff = controller.t_ff;
00549 //            datapackage.info.Control_kp = controller.kp;
00550 //            datapackage.info.Control_p = controller.theta_mech;
00551 //            datapackage.info.Control_kd = controller.kd;
00552 //            datapackage.info.Control_v = controller.dtheta_mech;
00553 
00554             datapackage.info.I_a = test_ia;
00555             datapackage.info.I_b = test_ib;
00556             datapackage.info.I_c = test_ic;
00557             datapackage.info.I_q = test_iq;
00558             datapackage.info.I_d = test_id;
00559             datapackage.info.Angle_mech = test_mech_angle;
00560             datapackage.info.Angle_elec = test_mech_speed;  // actually this return the speed
00561             datapackage.info.Control_tff = test_tff;
00562             datapackage.info.Control_kp = test_kp;
00563             datapackage.info.Control_p = test_pos_des;
00564             datapackage.info.Control_kd = test_kd;
00565             datapackage.info.Control_v = test_vel_des;
00566             
00567             // pc.write(datapackage.decode, 60);
00568             // string s(datapackage.decode);
00569 //            pc.printf((const char *)datapackage.decode );wait_us(900);
00570             
00571             for(int i=0;i<60;i++){
00572                 pc.putc(datapackage.decode[i]);
00573                 // wait_us(30);
00574                 wait_us(300);
00575                 }
00576              //wait_us(5000);
00577         }
00578         else{
00579             // can.write(txMsg);
00580             wait(1.0);
00581         }
00582     }
00583 }