main.cpp

00001 #include "mbed.h"
00002 #include "PositionSensor.h"
00003 #include "structs.h"
00004 
00005 #include "Inverter.h"
00006 //#define PI 3.14159f
00007 
00008 #include "foc.h"
00009 
00010 
00011 
00012 
00013 DigitalOut clockpin(PC_12);
00014 //AnalogIn   pot_in(PC_3);
00015 
00016 
00017 // controller modes
00018 #define REST_MODE 0
00019 #define OLSINE 1
00020 #define ONEUPTWODOWN 2
00021 #define VMODESINE 3
00022 //#define BEN_CALIBRATION_MODE 1
00023 #define TORQUE_MODE 4
00024 //#define PD_MODE 5
00025 //#define SETUP_MODE 6
00026 //#define ENCODER_MODE 7
00027  
00028 
00029 GPIOStruct gpio;
00030 ControllerStruct controller;
00031 
00032 
00033 //float fake_theta = 0.0;
00034 float theta1 = 0.0f;
00035 //float theta_offset = 4.708f;
00036 //float theta_offset = 3.38f;
00037 //float theta_offset = 4.97f;
00038 float theta_offset = 4.8f;
00039 
00040 int bing1 = 0;
00041 int bing2 = 0;
00042 int bing3 = 0;
00043 
00044 int var1 = 0;
00045 int var2 = 0;
00046 
00047 float a = 0.0f;
00048 float b = 0.0f;
00049 float c = 0.0f;
00050 
00051 volatile int count = 0;
00052 
00053 int controller_state = ONEUPTWODOWN;//VMODESINE;
00054 
00055 Serial pc(PA_2, PA_3);
00056 
00057 PositionSensorEncoder encoder(ENC_TICKS_PER_REV, 0, POLE_PAIRS); 
00058 
00059 
00060 // Main 20khz loop Interrupt ///
00061 /// This runs at 20 kHz, regardless of of the mode the controller is in, because it is triggered by hw timers ///
00062 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
00063   //clockpin = 1;
00064   
00065   if (TIM1->SR & TIM_SR_UIF ) {
00066         
00067         
00068         ///Sample current always ///
00069         ADC1->CR2  |= 0x40000000;                                               //Begin sample and conversion
00070         //volatile int delay;   
00071         //for (delay = 0; delay < 55; delay++);
00072         controller.adc2_raw = ADC2->DR;
00073         controller.adc1_raw = ADC1->DR;
00074 
00075 
00076         
00077         /// Check state machine state, and run the appropriate function ///
00078         //printf("%d\n\r", state);
00079         switch(controller_state){
00080             case REST_MODE:   //nothing 
00081                 if(count > 1000){
00082                     count = 0;                                                    
00083                     printf("Rest Mode");
00084                     printf("\n\r");
00085                 }
00086                 break;
00087             
00088             case OLSINE:        // open loop sines
00089 
00090                 //theta1+= 0.1f*pot_in;
00091                 theta1+= 0.001f;
00092                 if (theta1 > 2*PI) { theta1-=(2*PI); }
00093     
00094                 //trigger clock pin sychronous with electrical revolutions
00095                 if (theta1 > PI) { clockpin = 1;  }
00096                 else {clockpin = 0;}
00097 
00098                 a = cosf(theta1)/2 + 0.5f;
00099                 b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
00100                 c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
00101 
00102                 TIM1->CCR1 = 0x1194*(a);
00103                 TIM1->CCR2 = 0x1194*(b);
00104                 TIM1->CCR3 = 0x1194*(c);
00105                 break;
00106                 
00107             case ONEUPTWODOWN:        // one up, two down
00108         
00109                 a = 0.8f;
00110                 b = 0.2f;
00111                 c = 0.2f;
00112                 
00113                 count++;                       
00114                 if(count > 500){
00115                     count = 0;
00116                     //printf("%f  ",encoder.GetElecPosition());
00117                 }
00118                 
00119                 TIM1->CCR1 = 0x1194*(a);
00120                 TIM1->CCR2 = 0x1194*(b);
00121                 TIM1->CCR3 = 0x1194*(c);
00122                 
00123                 break;
00124                 
00125             case VMODESINE:        // position mode sines
00126                 theta1 = encoder.GetElecPosition() + theta_offset;
00127 
00128                 if (theta1 > 2*PI) {
00129                     theta1-=(2*PI);
00130                 }
00131     
00132                 //trigger clock pin sychronous with electrical revolutions
00133                 if (encoder.GetElecPosition() > PI) { clockpin = 1;  }
00134                 else {clockpin = 0;}
00135 
00136                 a = cosf(theta1)/2 + 0.5f;
00137                 b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
00138                 c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;
00139 
00140                 TIM1->CCR1 = 0x1194*(a);
00141                 TIM1->CCR2 = 0x1194*(b);
00142                 TIM1->CCR3 = 0x1194*(c);
00143                 
00144                 
00145                 //remove this code later
00146                 //print Q and D values
00147                 count++;
00148                           // Run current loop
00149                 //spi.Sample();                                                   // Sample position sensor
00150                 if(count > 1000){
00151                     count = 0;
00152                     //printf("%f  ",controller.i_q);
00153                     //printf("%f  ",controller.i_d);
00154                     //printf("\n\r");
00155                     }
00156                 
00157                 break;
00158              
00159             case TORQUE_MODE: 
00160              
00161                 // Run torque control
00162                 count++;
00163                 controller.theta_elec = encoder.GetElecPosition() + theta_offset;                  
00164                 commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
00165                 //spi.Sample();                                                   // Sample position sensor
00166                 if(count > 1000){
00167                     count = 0;
00168                     //readCAN();
00169                     //controller.i_q_ref = ((float)(canCmd-1000))/100;
00170                     controller.i_q_ref = 2;
00171                     //pc.printf("%f\n\r ", controller.theta_elec);
00172             
00173                     //printf("%i  ",controller.adc1_raw);
00174                     //printf("%i \n\r ",controller.adc2_raw);
00175         }
00176                 }
00177                 
00178     
00179         ///*
00180         
00181         controller.theta_elec = encoder.GetElecPosition() + theta_offset;                  
00182         commutate(&controller, &gpio, controller.theta_elec); 
00183         
00184         
00185         
00186         //*/
00187             
00188       }
00189   TIM1->SR = 0x0;  
00190   //clockpin = 0;                                                             // reset the status register
00191 }
00192 
00193 int main() {
00194     //float meas;
00195     pc.baud(115200);
00196     printf("\nStarting Hardware\n");
00197     Init_All_HW(&gpio);
00198     
00199     zero_current(&controller.adc1_offset, &controller.adc2_offset);                                                        // Setup PWM, ADC, GPIO
00200     wait(0.1);
00201     
00202     
00203     while(1) {
00204         //printf("AnalogIn example\n");
00205         //printf("%f  ",a);
00206         //printf("\n");
00207         wait(0.1);
00208         
00209         if (0==0) {
00210         //printf("%i  ", TIM3->CNT);
00211         
00212         printf("%f  ",encoder.GetElecPosition());
00213         //printf("%f  ",encoder.GetMechPosition());
00214         
00215         printf("%f  ",theta_offset);
00216         
00217         printf("%i  ",controller.adc1_raw);
00218         printf("%i  ",controller.adc2_raw);
00219         
00220         printf("%i  ",controller.adc1_offset);
00221         printf("%i  ",controller.adc2_offset);
00222         
00223         printf("%f  ",controller.i_q);
00224         printf("%f  ",controller.i_d);
00225         
00226         printf("%f  ",controller.i_a);
00227         printf("%f  ",controller.i_b);
00228         
00229         //printf("%f  ",controller.theta_elec);
00230         
00231         
00232         
00233         printf("\n\r");
00234         
00235         }
00236         
00237         
00238         
00239         
00240     }
00241 }