File content as of revision 0:9edd6ec0f56a:

#include "mbed.h"
#include "PositionSensor.h"
#include "structs.h"

#include "Inverter.h"
//#define PI 3.14159f

#include "foc.h"




DigitalOut clockpin(PC_12);
//AnalogIn   pot_in(PC_3);


// controller modes
#define REST_MODE 0
#define OLSINE 1
#define ONEUPTWODOWN 2
#define VMODESINE 3
//#define BEN_CALIBRATION_MODE 1
#define TORQUE_MODE 4
//#define PD_MODE 5
//#define SETUP_MODE 6
//#define ENCODER_MODE 7
 

GPIOStruct gpio;
ControllerStruct controller;


//float fake_theta = 0.0;
float theta1 = 0.0f;
//float theta_offset = 4.708f;
//float theta_offset = 3.38f;
//float theta_offset = 4.97f;
float theta_offset = 4.8f;

int bing1 = 0;
int bing2 = 0;
int bing3 = 0;

int var1 = 0;
int var2 = 0;

float a = 0.0f;
float b = 0.0f;
float c = 0.0f;

volatile int count = 0;

int controller_state = ONEUPTWODOWN;//VMODESINE;

Serial pc(PA_2, PA_3);

PositionSensorEncoder encoder(ENC_TICKS_PER_REV, 0, POLE_PAIRS); 


// Main 20khz loop Interrupt ///
/// This runs at 20 kHz, regardless of of the mode the controller is in, because it is triggered by hw timers ///
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
  //clockpin = 1;
  
  if (TIM1->SR & TIM_SR_UIF ) {
        
        
        ///Sample current always ///
        ADC1->CR2  |= 0x40000000;                                               //Begin sample and conversion
        //volatile int delay;   
        //for (delay = 0; delay < 55; delay++);
        controller.adc2_raw = ADC2->DR;
        controller.adc1_raw = ADC1->DR;


        
        /// Check state machine state, and run the appropriate function ///
        //printf("%d\n\r", state);
        switch(controller_state){
            case REST_MODE:   //nothing 
                if(count > 1000){
                    count = 0;                                                    
                    printf("Rest Mode");
                    printf("\n\r");
                }
                break;
            
            case OLSINE:        // open loop sines

                //theta1+= 0.1f*pot_in;
                theta1+= 0.001f;
                if (theta1 > 2*PI) { theta1-=(2*PI); }
    
                //trigger clock pin sychronous with electrical revolutions
                if (theta1 > PI) { clockpin = 1;  }
                else {clockpin = 0;}

                a = cosf(theta1)/2 + 0.5f;
                b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
                c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;

                TIM1->CCR1 = 0x1194*(a);
                TIM1->CCR2 = 0x1194*(b);
                TIM1->CCR3 = 0x1194*(c);
                break;
                
            case ONEUPTWODOWN:        // one up, two down
        
                a = 0.8f;
                b = 0.2f;
                c = 0.2f;
                
                count++;                       
                if(count > 500){
                    count = 0;
                    //printf("%f  ",encoder.GetElecPosition());
                }
                
                TIM1->CCR1 = 0x1194*(a);
                TIM1->CCR2 = 0x1194*(b);
                TIM1->CCR3 = 0x1194*(c);
                
                break;
                
            case VMODESINE:        // position mode sines
                theta1 = encoder.GetElecPosition() + theta_offset;

                if (theta1 > 2*PI) {
                    theta1-=(2*PI);
                }
    
                //trigger clock pin sychronous with electrical revolutions
                if (encoder.GetElecPosition() > PI) { clockpin = 1;  }
                else {clockpin = 0;}

                a = cosf(theta1)/2 + 0.5f;
                b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f;
                c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f;

                TIM1->CCR1 = 0x1194*(a);
                TIM1->CCR2 = 0x1194*(b);
                TIM1->CCR3 = 0x1194*(c);
                
                
                //remove this code later
                //print Q and D values
                count++;
                          // Run current loop
                //spi.Sample();                                                   // Sample position sensor
                if(count > 1000){
                    count = 0;
                    //printf("%f  ",controller.i_q);
                    //printf("%f  ",controller.i_d);
                    //printf("\n\r");
                    }
                
                break;
             
            case TORQUE_MODE: 
             
                // Run torque control
                count++;
                controller.theta_elec = encoder.GetElecPosition() + theta_offset;                  
                commutate(&controller, &gpio, controller.theta_elec);           // Run current loop
                //spi.Sample();                                                   // Sample position sensor
                if(count > 1000){
                    count = 0;
                    //readCAN();
                    //controller.i_q_ref = ((float)(canCmd-1000))/100;
                    controller.i_q_ref = 2;
                    //pc.printf("%f\n\r ", controller.theta_elec);
            
                    //printf("%i  ",controller.adc1_raw);
                    //printf("%i \n\r ",controller.adc2_raw);
        }
                }
                
    
        ///*
        
        controller.theta_elec = encoder.GetElecPosition() + theta_offset;                  
        commutate(&controller, &gpio, controller.theta_elec); 
        
        
        
        //*/
            
      }
  TIM1->SR = 0x0;  
  //clockpin = 0;                                                             // reset the status register
}

int main() {
    //float meas;
    pc.baud(115200);
    printf("\nStarting Hardware\n");
    Init_All_HW(&gpio);
    
    zero_current(&controller.adc1_offset, &controller.adc2_offset);                                                        // Setup PWM, ADC, GPIO
    wait(0.1);
    
    
    while(1) {
        //printf("AnalogIn example\n");
        //printf("%f  ",a);
        //printf("\n");
        wait(0.1);
        
        if (0==0) {
        //printf("%i  ", TIM3->CNT);
        
        printf("%f  ",encoder.GetElecPosition());
        //printf("%f  ",encoder.GetMechPosition());
        
        printf("%f  ",theta_offset);
        
        printf("%i  ",controller.adc1_raw);
        printf("%i  ",controller.adc2_raw);
        
        printf("%i  ",controller.adc1_offset);
        printf("%i  ",controller.adc2_offset);
        
        printf("%f  ",controller.i_q);
        printf("%f  ",controller.i_d);
        
        printf("%f  ",controller.i_a);
        printf("%f  ",controller.i_b);
        
        //printf("%f  ",controller.theta_elec);
        
        
        
        printf("\n\r");
        
        }
        
        
        
        
    }
}