Austin Brown / Mbed 2 deprecated CurrentModeSine

Slurp

Dependencies:   FastPWM3 mbed

main.cpp@0:9edd6ec0f56a, 2017-05-20 (annotated)

Committer:
austinbrown124
Date:
Sat May 20 21:42:20 2017 +0000
Revision:
0:9edd6ec0f56a
First Commit

Who changed what in which revision?

UserRevisionLine numberNew 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 }