main.cpp

00001 #include "mbed.h"
00002 
00003 #include "fw_config.h"
00004 #include "math_ops.h"
00005 #include "Inverter.h"
00006 #include "ServoInput.h"
00007 
00008 
00009 /*
00010 
00011 
00012 PA2 goes to servo input
00013 
00014 PA3-7, PA0-1 goes to  analog input  (on of those)
00015 
00016 use two of either PA8, 9,   10, 
00017 slaved with       PA7, PB0, PB1
00018 
00019 PA7 and PA8 for high and low sides
00020 
00021 PB6, 7 used for serial
00022 PA13, 14, reset used for programming
00023 
00024 some other random pin used for LED
00025 some other random pin used for clockpin, for debugging
00026 
00027 
00028 ======
00029 pins finally used by mike:
00030 PA2: RC PWM
00031 PA5: clockpin
00032 PA8: gate drive
00033 
00034 
00035 
00036 */
00037 
00038 
00039 
00040 DigitalOut clockpin(PA_5); 
00041 DigitalOut led1(PB_1);  
00042 DigitalIn gpio1(PA_6);  
00043 
00044 //DigitalIn gpio2(PA_2); 
00045 
00046 
00047 // controller modes
00048 #define REST_MODE 0
00049 #define VOLTAGE_MODE 1
00050 #define CURRENT_MODE 2
00051 
00052 
00053 volatile int count = 0;
00054 volatile int main_int_count = 0;
00055 volatile unsigned int main_int_clock = 0;
00056 
00057 int controller_state = 0;
00058 
00059 
00060 Serial pc(PB_6, PB_7);
00061 Inverter inverter;
00062 ServoTimer servoinp;
00063 
00064 
00065 float a1 = 0.0f;
00066 float a2 = 0.0f;
00067 
00068 
00069 float current = 0.0f;
00070 float current_raw = 0.0f;
00071 float servo_cmd = 0.0f;
00072 
00073 
00074 int servo_low = 890;
00075 int servo_high = 1800;
00076 int servo_range = servo_high - servo_low;
00077 
00078 
00079 
00080 
00081 // Main 20khz loop Interrupt ///
00082 
00083 extern "C" void TIM1_UP_TIM16_IRQHandler(void) {
00084   
00085   if (TIM1->SR & TIM_SR_UIF ) {
00086 
00087         main_int_count++;
00088         main_int_clock++;
00089 
00090         //
00091         //current_raw = float(ADC1->DR) - float(inverter.adc1_offset);
00092         current_raw = float(ADC1->DR) - 2000;
00093         //current_raw =  float(ADC1->DR));
00094         //current = current_raw*0.1f; //depends on shunts
00095         
00096         inverter.adc2_raw = ADC2->DR;
00097         inverter.adc1_raw = ADC1->DR;
00098         clockpin = 1;
00099         servoinp.update_servo_input();
00100         
00101         
00102         /// Check state machine state, and run the appropriate function ///
00103         switch(controller_state){
00104             case REST_MODE:   //nothing 
00105                 TIM1->CCR1 = 6400;  
00106                 count++;
00107                 if(count > 20000){
00108                     count = 0;                                                    
00109                     led1 = !led1;
00110                 }
00111                 servo_cmd = servoinp.get_servo_input();
00112                 
00113             break;
00114                 
00115             
00116             case VOLTAGE_MODE: 
00117                 
00118                 servo_cmd = servoinp.get_servo_input();
00119 
00120                 TIM1->CCR1 = 6400 - constrain((servo_cmd-880)*8.0f , 0, 6150);  //0 to 96% duty cycle
00121 
00122                 if(count > 10000){ count = 0; }
00123                 
00124             break;
00125             
00126             case CURRENT_MODE: 
00127                 
00128                 servo_cmd = servoinp.get_servo_input();
00129                 /*
00130                 float current_setpoint = (servo_cmd-880)/100;  //0 to 10 amps
00131                 if (current > current_setpoint) {
00132                     //set bridge state to LOW
00133                     TIM1->CCR1 = 6100;
00134                 }
00135                 else {
00136                     TIM1->CCR1 = 300;
00137                 }*/
00138                 
00139                 
00140                 // servo inputs go from 880 to 1700
00141                 
00142                 // the constant in the next line is set 1800/servo_range
00143                 //float current_setpoint_raw = (servo_cmd-servo_low)*2.25f;  //scales 800 range to 1800
00144                 float current_setpoint_raw = constrain((servo_cmd-servo_low)*2.0f , 0, 800);  //scales 800 range to 1800
00145                 if (current_raw > current_setpoint_raw) {
00146                     //set bridge state to LOW
00147                     TIM1->CCR1 = 6400;
00148                 }
00149                 else {
00150                     TIM1->CCR1 = 250;
00151                 }
00152                 
00153                 
00154                 if ((servo_cmd < servo_low) & (current_raw < 200) & (current_raw > -200)) {
00155                     TIM1->CCR1 = 6400;
00156                 }
00157                 
00158                 
00159                 if(count > 10000){ count = 0; }
00160                 
00161             break;
00162             
00163 
00164         }  
00165             
00166       }
00167             //b = TIM1->CNT;
00168   TIM1->SR = 0x0;  
00169   clockpin = 0;                                                             // reset the status register
00170 }
00171 
00172 int main() {
00173     wait_ms(200);
00174     pc.baud(256000);
00175     printf("\rStarting Hardware\n");
00176     gpio1.mode(PullUp);
00177 
00178     
00179     inverter.Init();                     // Setup PWM, ADC
00180     wait(0.1);
00181     controller_state = REST_MODE;
00182     wait(0.1);
00183     inverter.zero_current();
00184     wait(0.1);
00185     pc.printf("ADCs zeroed at ");
00186     pc.printf("%i, %i \n",inverter.adc1_offset,inverter.adc2_offset);                                                 
00187     wait(0.1);
00188     inverter.ADCsync();
00189 
00190     
00191 
00192     //controller_state = VOLTAGE_MODE;
00193     controller_state = CURRENT_MODE;
00194 
00195     
00196     while(1) {
00197 
00198         wait_us(2);
00199         
00200         while (1==1) {
00201             
00202         wait_us(30);
00203 
00204         pc.printf("%f,  ", servo_cmd); 
00205         
00206         pc.printf("%f,  ", current_raw); 
00207         
00208         pc.printf("%f,  ", float(inverter.adc1_offset));
00209         pc.printf("%f,  ", a1); 
00210         pc.printf("%f,  ", a2); 
00211         
00212         
00213         //printf("%i,  ", b3); 
00214         //printf("%i,  ", b4);
00215 
00216         //pc.printf("%i, ", TIM15->CNT);  
00217         //pc.printf("%x  ", GPIOA->AFR[0]); 
00218 
00219         pc.printf("%i,  ", ADC1->DR);  
00220         pc.printf("%i,  ", ADC2->DR);
00221         //printf("    %i, %i %i", a1, a2, a3);
00222         //if (gpio2) {pc.printf("low ");}
00223         pc.printf("\r");
00224 
00225 
00226         }
00227 
00228     }
00229 }
00230 
00231 
00232 
00233 
00234 
00235 
00236