Austin Brown
/
ESCmk2_Inductance
Inductance Testing Code
Fork of CurrentModeSine by
main.cpp@0:9edd6ec0f56a, 2017-05-20 (annotated)
- Committer:
- austinbrown124
- Date:
- Sat May 20 21:42:20 2017 +0000
- Revision:
- 0:9edd6ec0f56a
- Child:
- 1:64b881306f6f
First Commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
austinbrown124 | 0:9edd6ec0f56a | 1 | #include "mbed.h" |
austinbrown124 | 0:9edd6ec0f56a | 2 | #include "PositionSensor.h" |
austinbrown124 | 0:9edd6ec0f56a | 3 | #include "structs.h" |
austinbrown124 | 0:9edd6ec0f56a | 4 | |
austinbrown124 | 0:9edd6ec0f56a | 5 | #include "Inverter.h" |
austinbrown124 | 0:9edd6ec0f56a | 6 | //#define PI 3.14159f |
austinbrown124 | 0:9edd6ec0f56a | 7 | |
austinbrown124 | 0:9edd6ec0f56a | 8 | #include "foc.h" |
austinbrown124 | 0:9edd6ec0f56a | 9 | |
austinbrown124 | 0:9edd6ec0f56a | 10 | |
austinbrown124 | 0:9edd6ec0f56a | 11 | |
austinbrown124 | 0:9edd6ec0f56a | 12 | |
austinbrown124 | 0:9edd6ec0f56a | 13 | DigitalOut clockpin(PC_12); |
austinbrown124 | 0:9edd6ec0f56a | 14 | //AnalogIn pot_in(PC_3); |
austinbrown124 | 0:9edd6ec0f56a | 15 | |
austinbrown124 | 0:9edd6ec0f56a | 16 | |
austinbrown124 | 0:9edd6ec0f56a | 17 | // controller modes |
austinbrown124 | 0:9edd6ec0f56a | 18 | #define REST_MODE 0 |
austinbrown124 | 0:9edd6ec0f56a | 19 | #define OLSINE 1 |
austinbrown124 | 0:9edd6ec0f56a | 20 | #define ONEUPTWODOWN 2 |
austinbrown124 | 0:9edd6ec0f56a | 21 | #define VMODESINE 3 |
austinbrown124 | 0:9edd6ec0f56a | 22 | //#define BEN_CALIBRATION_MODE 1 |
austinbrown124 | 0:9edd6ec0f56a | 23 | #define TORQUE_MODE 4 |
austinbrown124 | 0:9edd6ec0f56a | 24 | //#define PD_MODE 5 |
austinbrown124 | 0:9edd6ec0f56a | 25 | //#define SETUP_MODE 6 |
austinbrown124 | 0:9edd6ec0f56a | 26 | //#define ENCODER_MODE 7 |
austinbrown124 | 0:9edd6ec0f56a | 27 | |
austinbrown124 | 0:9edd6ec0f56a | 28 | |
austinbrown124 | 0:9edd6ec0f56a | 29 | GPIOStruct gpio; |
austinbrown124 | 0:9edd6ec0f56a | 30 | ControllerStruct controller; |
austinbrown124 | 0:9edd6ec0f56a | 31 | |
austinbrown124 | 0:9edd6ec0f56a | 32 | |
austinbrown124 | 0:9edd6ec0f56a | 33 | //float fake_theta = 0.0; |
austinbrown124 | 0:9edd6ec0f56a | 34 | float theta1 = 0.0f; |
austinbrown124 | 0:9edd6ec0f56a | 35 | //float theta_offset = 4.708f; |
austinbrown124 | 0:9edd6ec0f56a | 36 | //float theta_offset = 3.38f; |
austinbrown124 | 0:9edd6ec0f56a | 37 | //float theta_offset = 4.97f; |
austinbrown124 | 0:9edd6ec0f56a | 38 | float theta_offset = 4.8f; |
austinbrown124 | 0:9edd6ec0f56a | 39 | |
austinbrown124 | 0:9edd6ec0f56a | 40 | int bing1 = 0; |
austinbrown124 | 0:9edd6ec0f56a | 41 | int bing2 = 0; |
austinbrown124 | 0:9edd6ec0f56a | 42 | int bing3 = 0; |
austinbrown124 | 0:9edd6ec0f56a | 43 | |
austinbrown124 | 0:9edd6ec0f56a | 44 | int var1 = 0; |
austinbrown124 | 0:9edd6ec0f56a | 45 | int var2 = 0; |
austinbrown124 | 0:9edd6ec0f56a | 46 | |
austinbrown124 | 0:9edd6ec0f56a | 47 | float a = 0.0f; |
austinbrown124 | 0:9edd6ec0f56a | 48 | float b = 0.0f; |
austinbrown124 | 0:9edd6ec0f56a | 49 | float c = 0.0f; |
austinbrown124 | 0:9edd6ec0f56a | 50 | |
austinbrown124 | 0:9edd6ec0f56a | 51 | volatile int count = 0; |
austinbrown124 | 0:9edd6ec0f56a | 52 | |
austinbrown124 | 0:9edd6ec0f56a | 53 | int controller_state = ONEUPTWODOWN;//VMODESINE; |
austinbrown124 | 0:9edd6ec0f56a | 54 | |
austinbrown124 | 0:9edd6ec0f56a | 55 | Serial pc(PA_2, PA_3); |
austinbrown124 | 0:9edd6ec0f56a | 56 | |
austinbrown124 | 0:9edd6ec0f56a | 57 | PositionSensorEncoder encoder(ENC_TICKS_PER_REV, 0, POLE_PAIRS); |
austinbrown124 | 0:9edd6ec0f56a | 58 | |
austinbrown124 | 0:9edd6ec0f56a | 59 | |
austinbrown124 | 0:9edd6ec0f56a | 60 | // Main 20khz loop Interrupt /// |
austinbrown124 | 0:9edd6ec0f56a | 61 | /// This runs at 20 kHz, regardless of of the mode the controller is in, because it is triggered by hw timers /// |
austinbrown124 | 0:9edd6ec0f56a | 62 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
austinbrown124 | 0:9edd6ec0f56a | 63 | //clockpin = 1; |
austinbrown124 | 0:9edd6ec0f56a | 64 | |
austinbrown124 | 0:9edd6ec0f56a | 65 | if (TIM1->SR & TIM_SR_UIF ) { |
austinbrown124 | 0:9edd6ec0f56a | 66 | |
austinbrown124 | 0:9edd6ec0f56a | 67 | |
austinbrown124 | 0:9edd6ec0f56a | 68 | ///Sample current always /// |
austinbrown124 | 0:9edd6ec0f56a | 69 | ADC1->CR2 |= 0x40000000; //Begin sample and conversion |
austinbrown124 | 0:9edd6ec0f56a | 70 | //volatile int delay; |
austinbrown124 | 0:9edd6ec0f56a | 71 | //for (delay = 0; delay < 55; delay++); |
austinbrown124 | 0:9edd6ec0f56a | 72 | controller.adc2_raw = ADC2->DR; |
austinbrown124 | 0:9edd6ec0f56a | 73 | controller.adc1_raw = ADC1->DR; |
austinbrown124 | 0:9edd6ec0f56a | 74 | |
austinbrown124 | 0:9edd6ec0f56a | 75 | |
austinbrown124 | 0:9edd6ec0f56a | 76 | |
austinbrown124 | 0:9edd6ec0f56a | 77 | /// Check state machine state, and run the appropriate function /// |
austinbrown124 | 0:9edd6ec0f56a | 78 | //printf("%d\n\r", state); |
austinbrown124 | 0:9edd6ec0f56a | 79 | switch(controller_state){ |
austinbrown124 | 0:9edd6ec0f56a | 80 | case REST_MODE: //nothing |
austinbrown124 | 0:9edd6ec0f56a | 81 | if(count > 1000){ |
austinbrown124 | 0:9edd6ec0f56a | 82 | count = 0; |
austinbrown124 | 0:9edd6ec0f56a | 83 | printf("Rest Mode"); |
austinbrown124 | 0:9edd6ec0f56a | 84 | printf("\n\r"); |
austinbrown124 | 0:9edd6ec0f56a | 85 | } |
austinbrown124 | 0:9edd6ec0f56a | 86 | break; |
austinbrown124 | 0:9edd6ec0f56a | 87 | |
austinbrown124 | 0:9edd6ec0f56a | 88 | case OLSINE: // open loop sines |
austinbrown124 | 0:9edd6ec0f56a | 89 | |
austinbrown124 | 0:9edd6ec0f56a | 90 | //theta1+= 0.1f*pot_in; |
austinbrown124 | 0:9edd6ec0f56a | 91 | theta1+= 0.001f; |
austinbrown124 | 0:9edd6ec0f56a | 92 | if (theta1 > 2*PI) { theta1-=(2*PI); } |
austinbrown124 | 0:9edd6ec0f56a | 93 | |
austinbrown124 | 0:9edd6ec0f56a | 94 | //trigger clock pin sychronous with electrical revolutions |
austinbrown124 | 0:9edd6ec0f56a | 95 | if (theta1 > PI) { clockpin = 1; } |
austinbrown124 | 0:9edd6ec0f56a | 96 | else {clockpin = 0;} |
austinbrown124 | 0:9edd6ec0f56a | 97 | |
austinbrown124 | 0:9edd6ec0f56a | 98 | a = cosf(theta1)/2 + 0.5f; |
austinbrown124 | 0:9edd6ec0f56a | 99 | b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f; |
austinbrown124 | 0:9edd6ec0f56a | 100 | c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f; |
austinbrown124 | 0:9edd6ec0f56a | 101 | |
austinbrown124 | 0:9edd6ec0f56a | 102 | TIM1->CCR1 = 0x1194*(a); |
austinbrown124 | 0:9edd6ec0f56a | 103 | TIM1->CCR2 = 0x1194*(b); |
austinbrown124 | 0:9edd6ec0f56a | 104 | TIM1->CCR3 = 0x1194*(c); |
austinbrown124 | 0:9edd6ec0f56a | 105 | break; |
austinbrown124 | 0:9edd6ec0f56a | 106 | |
austinbrown124 | 0:9edd6ec0f56a | 107 | case ONEUPTWODOWN: // one up, two down |
austinbrown124 | 0:9edd6ec0f56a | 108 | |
austinbrown124 | 0:9edd6ec0f56a | 109 | a = 0.8f; |
austinbrown124 | 0:9edd6ec0f56a | 110 | b = 0.2f; |
austinbrown124 | 0:9edd6ec0f56a | 111 | c = 0.2f; |
austinbrown124 | 0:9edd6ec0f56a | 112 | |
austinbrown124 | 0:9edd6ec0f56a | 113 | count++; |
austinbrown124 | 0:9edd6ec0f56a | 114 | if(count > 500){ |
austinbrown124 | 0:9edd6ec0f56a | 115 | count = 0; |
austinbrown124 | 0:9edd6ec0f56a | 116 | //printf("%f ",encoder.GetElecPosition()); |
austinbrown124 | 0:9edd6ec0f56a | 117 | } |
austinbrown124 | 0:9edd6ec0f56a | 118 | |
austinbrown124 | 0:9edd6ec0f56a | 119 | TIM1->CCR1 = 0x1194*(a); |
austinbrown124 | 0:9edd6ec0f56a | 120 | TIM1->CCR2 = 0x1194*(b); |
austinbrown124 | 0:9edd6ec0f56a | 121 | TIM1->CCR3 = 0x1194*(c); |
austinbrown124 | 0:9edd6ec0f56a | 122 | |
austinbrown124 | 0:9edd6ec0f56a | 123 | break; |
austinbrown124 | 0:9edd6ec0f56a | 124 | |
austinbrown124 | 0:9edd6ec0f56a | 125 | case VMODESINE: // position mode sines |
austinbrown124 | 0:9edd6ec0f56a | 126 | theta1 = encoder.GetElecPosition() + theta_offset; |
austinbrown124 | 0:9edd6ec0f56a | 127 | |
austinbrown124 | 0:9edd6ec0f56a | 128 | if (theta1 > 2*PI) { |
austinbrown124 | 0:9edd6ec0f56a | 129 | theta1-=(2*PI); |
austinbrown124 | 0:9edd6ec0f56a | 130 | } |
austinbrown124 | 0:9edd6ec0f56a | 131 | |
austinbrown124 | 0:9edd6ec0f56a | 132 | //trigger clock pin sychronous with electrical revolutions |
austinbrown124 | 0:9edd6ec0f56a | 133 | if (encoder.GetElecPosition() > PI) { clockpin = 1; } |
austinbrown124 | 0:9edd6ec0f56a | 134 | else {clockpin = 0;} |
austinbrown124 | 0:9edd6ec0f56a | 135 | |
austinbrown124 | 0:9edd6ec0f56a | 136 | a = cosf(theta1)/2 + 0.5f; |
austinbrown124 | 0:9edd6ec0f56a | 137 | b = cosf(( 2.0f*PI/3.0f)+theta1)/2 + 0.5f; |
austinbrown124 | 0:9edd6ec0f56a | 138 | c = cosf((-2.0f*PI/3.0f)+theta1)/2 + 0.5f; |
austinbrown124 | 0:9edd6ec0f56a | 139 | |
austinbrown124 | 0:9edd6ec0f56a | 140 | TIM1->CCR1 = 0x1194*(a); |
austinbrown124 | 0:9edd6ec0f56a | 141 | TIM1->CCR2 = 0x1194*(b); |
austinbrown124 | 0:9edd6ec0f56a | 142 | TIM1->CCR3 = 0x1194*(c); |
austinbrown124 | 0:9edd6ec0f56a | 143 | |
austinbrown124 | 0:9edd6ec0f56a | 144 | |
austinbrown124 | 0:9edd6ec0f56a | 145 | //remove this code later |
austinbrown124 | 0:9edd6ec0f56a | 146 | //print Q and D values |
austinbrown124 | 0:9edd6ec0f56a | 147 | count++; |
austinbrown124 | 0:9edd6ec0f56a | 148 | // Run current loop |
austinbrown124 | 0:9edd6ec0f56a | 149 | //spi.Sample(); // Sample position sensor |
austinbrown124 | 0:9edd6ec0f56a | 150 | if(count > 1000){ |
austinbrown124 | 0:9edd6ec0f56a | 151 | count = 0; |
austinbrown124 | 0:9edd6ec0f56a | 152 | //printf("%f ",controller.i_q); |
austinbrown124 | 0:9edd6ec0f56a | 153 | //printf("%f ",controller.i_d); |
austinbrown124 | 0:9edd6ec0f56a | 154 | //printf("\n\r"); |
austinbrown124 | 0:9edd6ec0f56a | 155 | } |
austinbrown124 | 0:9edd6ec0f56a | 156 | |
austinbrown124 | 0:9edd6ec0f56a | 157 | break; |
austinbrown124 | 0:9edd6ec0f56a | 158 | |
austinbrown124 | 0:9edd6ec0f56a | 159 | case TORQUE_MODE: |
austinbrown124 | 0:9edd6ec0f56a | 160 | |
austinbrown124 | 0:9edd6ec0f56a | 161 | // Run torque control |
austinbrown124 | 0:9edd6ec0f56a | 162 | count++; |
austinbrown124 | 0:9edd6ec0f56a | 163 | controller.theta_elec = encoder.GetElecPosition() + theta_offset; |
austinbrown124 | 0:9edd6ec0f56a | 164 | commutate(&controller, &gpio, controller.theta_elec); // Run current loop |
austinbrown124 | 0:9edd6ec0f56a | 165 | //spi.Sample(); // Sample position sensor |
austinbrown124 | 0:9edd6ec0f56a | 166 | if(count > 1000){ |
austinbrown124 | 0:9edd6ec0f56a | 167 | count = 0; |
austinbrown124 | 0:9edd6ec0f56a | 168 | //readCAN(); |
austinbrown124 | 0:9edd6ec0f56a | 169 | //controller.i_q_ref = ((float)(canCmd-1000))/100; |
austinbrown124 | 0:9edd6ec0f56a | 170 | controller.i_q_ref = 2; |
austinbrown124 | 0:9edd6ec0f56a | 171 | //pc.printf("%f\n\r ", controller.theta_elec); |
austinbrown124 | 0:9edd6ec0f56a | 172 | |
austinbrown124 | 0:9edd6ec0f56a | 173 | //printf("%i ",controller.adc1_raw); |
austinbrown124 | 0:9edd6ec0f56a | 174 | //printf("%i \n\r ",controller.adc2_raw); |
austinbrown124 | 0:9edd6ec0f56a | 175 | } |
austinbrown124 | 0:9edd6ec0f56a | 176 | } |
austinbrown124 | 0:9edd6ec0f56a | 177 | |
austinbrown124 | 0:9edd6ec0f56a | 178 | |
austinbrown124 | 0:9edd6ec0f56a | 179 | ///* |
austinbrown124 | 0:9edd6ec0f56a | 180 | |
austinbrown124 | 0:9edd6ec0f56a | 181 | controller.theta_elec = encoder.GetElecPosition() + theta_offset; |
austinbrown124 | 0:9edd6ec0f56a | 182 | commutate(&controller, &gpio, controller.theta_elec); |
austinbrown124 | 0:9edd6ec0f56a | 183 | |
austinbrown124 | 0:9edd6ec0f56a | 184 | |
austinbrown124 | 0:9edd6ec0f56a | 185 | |
austinbrown124 | 0:9edd6ec0f56a | 186 | //*/ |
austinbrown124 | 0:9edd6ec0f56a | 187 | |
austinbrown124 | 0:9edd6ec0f56a | 188 | } |
austinbrown124 | 0:9edd6ec0f56a | 189 | TIM1->SR = 0x0; |
austinbrown124 | 0:9edd6ec0f56a | 190 | //clockpin = 0; // reset the status register |
austinbrown124 | 0:9edd6ec0f56a | 191 | } |
austinbrown124 | 0:9edd6ec0f56a | 192 | |
austinbrown124 | 0:9edd6ec0f56a | 193 | int main() { |
austinbrown124 | 0:9edd6ec0f56a | 194 | //float meas; |
austinbrown124 | 0:9edd6ec0f56a | 195 | pc.baud(115200); |
austinbrown124 | 0:9edd6ec0f56a | 196 | printf("\nStarting Hardware\n"); |
austinbrown124 | 0:9edd6ec0f56a | 197 | Init_All_HW(&gpio); |
austinbrown124 | 0:9edd6ec0f56a | 198 | |
austinbrown124 | 0:9edd6ec0f56a | 199 | zero_current(&controller.adc1_offset, &controller.adc2_offset); // Setup PWM, ADC, GPIO |
austinbrown124 | 0:9edd6ec0f56a | 200 | wait(0.1); |
austinbrown124 | 0:9edd6ec0f56a | 201 | |
austinbrown124 | 0:9edd6ec0f56a | 202 | |
austinbrown124 | 0:9edd6ec0f56a | 203 | while(1) { |
austinbrown124 | 0:9edd6ec0f56a | 204 | //printf("AnalogIn example\n"); |
austinbrown124 | 0:9edd6ec0f56a | 205 | //printf("%f ",a); |
austinbrown124 | 0:9edd6ec0f56a | 206 | //printf("\n"); |
austinbrown124 | 0:9edd6ec0f56a | 207 | wait(0.1); |
austinbrown124 | 0:9edd6ec0f56a | 208 | |
austinbrown124 | 0:9edd6ec0f56a | 209 | if (0==0) { |
austinbrown124 | 0:9edd6ec0f56a | 210 | //printf("%i ", TIM3->CNT); |
austinbrown124 | 0:9edd6ec0f56a | 211 | |
austinbrown124 | 0:9edd6ec0f56a | 212 | printf("%f ",encoder.GetElecPosition()); |
austinbrown124 | 0:9edd6ec0f56a | 213 | //printf("%f ",encoder.GetMechPosition()); |
austinbrown124 | 0:9edd6ec0f56a | 214 | |
austinbrown124 | 0:9edd6ec0f56a | 215 | printf("%f ",theta_offset); |
austinbrown124 | 0:9edd6ec0f56a | 216 | |
austinbrown124 | 0:9edd6ec0f56a | 217 | printf("%i ",controller.adc1_raw); |
austinbrown124 | 0:9edd6ec0f56a | 218 | printf("%i ",controller.adc2_raw); |
austinbrown124 | 0:9edd6ec0f56a | 219 | |
austinbrown124 | 0:9edd6ec0f56a | 220 | printf("%i ",controller.adc1_offset); |
austinbrown124 | 0:9edd6ec0f56a | 221 | printf("%i ",controller.adc2_offset); |
austinbrown124 | 0:9edd6ec0f56a | 222 | |
austinbrown124 | 0:9edd6ec0f56a | 223 | printf("%f ",controller.i_q); |
austinbrown124 | 0:9edd6ec0f56a | 224 | printf("%f ",controller.i_d); |
austinbrown124 | 0:9edd6ec0f56a | 225 | |
austinbrown124 | 0:9edd6ec0f56a | 226 | printf("%f ",controller.i_a); |
austinbrown124 | 0:9edd6ec0f56a | 227 | printf("%f ",controller.i_b); |
austinbrown124 | 0:9edd6ec0f56a | 228 | |
austinbrown124 | 0:9edd6ec0f56a | 229 | //printf("%f ",controller.theta_elec); |
austinbrown124 | 0:9edd6ec0f56a | 230 | |
austinbrown124 | 0:9edd6ec0f56a | 231 | |
austinbrown124 | 0:9edd6ec0f56a | 232 | |
austinbrown124 | 0:9edd6ec0f56a | 233 | printf("\n\r"); |
austinbrown124 | 0:9edd6ec0f56a | 234 | |
austinbrown124 | 0:9edd6ec0f56a | 235 | } |
austinbrown124 | 0:9edd6ec0f56a | 236 | |
austinbrown124 | 0:9edd6ec0f56a | 237 | |
austinbrown124 | 0:9edd6ec0f56a | 238 | |
austinbrown124 | 0:9edd6ec0f56a | 239 | |
austinbrown124 | 0:9edd6ec0f56a | 240 | } |
austinbrown124 | 0:9edd6ec0f56a | 241 | } |