![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Slurp
Diff: main.cpp
- Revision:
- 0:9edd6ec0f56a
diff -r 000000000000 -r 9edd6ec0f56a main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat May 20 21:42:20 2017 +0000 @@ -0,0 +1,241 @@ +#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"); + + } + + + + + } +}