Austin Brown
/
MikeEtekController
Mike Etek Controller
main.cpp@1:94193b31f0ee, 2019-04-06 (annotated)
- Committer:
- austinbrown124
- Date:
- Sat Apr 06 02:16:12 2019 +0000
- Revision:
- 1:94193b31f0ee
- Parent:
- 0:9edd6ec0f56a
first commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
austinbrown124 | 0:9edd6ec0f56a | 1 | #include "mbed.h" |
austinbrown124 | 1:94193b31f0ee | 2 | |
austinbrown124 | 1:94193b31f0ee | 3 | #include "fw_config.h" |
austinbrown124 | 1:94193b31f0ee | 4 | #include "math_ops.h" |
austinbrown124 | 1:94193b31f0ee | 5 | #include "Inverter.h" |
austinbrown124 | 1:94193b31f0ee | 6 | #include "ServoInput.h" |
austinbrown124 | 1:94193b31f0ee | 7 | |
austinbrown124 | 1:94193b31f0ee | 8 | |
austinbrown124 | 1:94193b31f0ee | 9 | /* |
austinbrown124 | 1:94193b31f0ee | 10 | |
austinbrown124 | 1:94193b31f0ee | 11 | |
austinbrown124 | 1:94193b31f0ee | 12 | PA2 goes to servo input |
austinbrown124 | 1:94193b31f0ee | 13 | |
austinbrown124 | 1:94193b31f0ee | 14 | PA3-7, PA0-1 goes to analog input (on of those) |
austinbrown124 | 0:9edd6ec0f56a | 15 | |
austinbrown124 | 1:94193b31f0ee | 16 | use two of either PA8, 9, 10, |
austinbrown124 | 1:94193b31f0ee | 17 | slaved with PA7, PB0, PB1 |
austinbrown124 | 1:94193b31f0ee | 18 | |
austinbrown124 | 1:94193b31f0ee | 19 | PA7 and PA8 for high and low sides |
austinbrown124 | 1:94193b31f0ee | 20 | |
austinbrown124 | 1:94193b31f0ee | 21 | PB6, 7 used for serial |
austinbrown124 | 1:94193b31f0ee | 22 | PA13, 14, reset used for programming |
austinbrown124 | 0:9edd6ec0f56a | 23 | |
austinbrown124 | 1:94193b31f0ee | 24 | some other random pin used for LED |
austinbrown124 | 1:94193b31f0ee | 25 | some other random pin used for clockpin, for debugging |
austinbrown124 | 1:94193b31f0ee | 26 | |
austinbrown124 | 1:94193b31f0ee | 27 | |
austinbrown124 | 1:94193b31f0ee | 28 | ====== |
austinbrown124 | 1:94193b31f0ee | 29 | pins finally used by mike: |
austinbrown124 | 1:94193b31f0ee | 30 | PA2: RC PWM |
austinbrown124 | 1:94193b31f0ee | 31 | PA5: clockpin |
austinbrown124 | 1:94193b31f0ee | 32 | PA8: gate drive |
austinbrown124 | 0:9edd6ec0f56a | 33 | |
austinbrown124 | 0:9edd6ec0f56a | 34 | |
austinbrown124 | 0:9edd6ec0f56a | 35 | |
austinbrown124 | 1:94193b31f0ee | 36 | */ |
austinbrown124 | 0:9edd6ec0f56a | 37 | |
austinbrown124 | 1:94193b31f0ee | 38 | |
austinbrown124 | 1:94193b31f0ee | 39 | |
austinbrown124 | 1:94193b31f0ee | 40 | DigitalOut clockpin(PA_5); |
austinbrown124 | 1:94193b31f0ee | 41 | DigitalOut led1(PB_1); |
austinbrown124 | 1:94193b31f0ee | 42 | DigitalIn gpio1(PA_6); |
austinbrown124 | 1:94193b31f0ee | 43 | |
austinbrown124 | 1:94193b31f0ee | 44 | //DigitalIn gpio2(PA_2); |
austinbrown124 | 0:9edd6ec0f56a | 45 | |
austinbrown124 | 0:9edd6ec0f56a | 46 | |
austinbrown124 | 0:9edd6ec0f56a | 47 | // controller modes |
austinbrown124 | 0:9edd6ec0f56a | 48 | #define REST_MODE 0 |
austinbrown124 | 1:94193b31f0ee | 49 | #define VOLTAGE_MODE 1 |
austinbrown124 | 1:94193b31f0ee | 50 | #define CURRENT_MODE 2 |
austinbrown124 | 1:94193b31f0ee | 51 | |
austinbrown124 | 0:9edd6ec0f56a | 52 | |
austinbrown124 | 1:94193b31f0ee | 53 | volatile int count = 0; |
austinbrown124 | 1:94193b31f0ee | 54 | volatile int main_int_count = 0; |
austinbrown124 | 1:94193b31f0ee | 55 | volatile unsigned int main_int_clock = 0; |
austinbrown124 | 1:94193b31f0ee | 56 | |
austinbrown124 | 1:94193b31f0ee | 57 | int controller_state = 0; |
austinbrown124 | 0:9edd6ec0f56a | 58 | |
austinbrown124 | 0:9edd6ec0f56a | 59 | |
austinbrown124 | 1:94193b31f0ee | 60 | Serial pc(PB_6, PB_7); |
austinbrown124 | 1:94193b31f0ee | 61 | Inverter inverter; |
austinbrown124 | 1:94193b31f0ee | 62 | ServoTimer servoinp; |
austinbrown124 | 0:9edd6ec0f56a | 63 | |
austinbrown124 | 1:94193b31f0ee | 64 | |
austinbrown124 | 1:94193b31f0ee | 65 | float a1 = 0.0f; |
austinbrown124 | 1:94193b31f0ee | 66 | float a2 = 0.0f; |
austinbrown124 | 1:94193b31f0ee | 67 | |
austinbrown124 | 0:9edd6ec0f56a | 68 | |
austinbrown124 | 1:94193b31f0ee | 69 | float current = 0.0f; |
austinbrown124 | 1:94193b31f0ee | 70 | float current_raw = 0.0f; |
austinbrown124 | 1:94193b31f0ee | 71 | float servo_cmd = 0.0f; |
austinbrown124 | 0:9edd6ec0f56a | 72 | |
austinbrown124 | 0:9edd6ec0f56a | 73 | |
austinbrown124 | 1:94193b31f0ee | 74 | int servo_low = 890; |
austinbrown124 | 1:94193b31f0ee | 75 | int servo_high = 1800; |
austinbrown124 | 1:94193b31f0ee | 76 | int servo_range = servo_high - servo_low; |
austinbrown124 | 0:9edd6ec0f56a | 77 | |
austinbrown124 | 0:9edd6ec0f56a | 78 | |
austinbrown124 | 0:9edd6ec0f56a | 79 | |
austinbrown124 | 0:9edd6ec0f56a | 80 | |
austinbrown124 | 0:9edd6ec0f56a | 81 | // Main 20khz loop Interrupt /// |
austinbrown124 | 1:94193b31f0ee | 82 | |
austinbrown124 | 1:94193b31f0ee | 83 | extern "C" void TIM1_UP_TIM16_IRQHandler(void) { |
austinbrown124 | 0:9edd6ec0f56a | 84 | |
austinbrown124 | 0:9edd6ec0f56a | 85 | if (TIM1->SR & TIM_SR_UIF ) { |
austinbrown124 | 1:94193b31f0ee | 86 | |
austinbrown124 | 1:94193b31f0ee | 87 | main_int_count++; |
austinbrown124 | 1:94193b31f0ee | 88 | main_int_clock++; |
austinbrown124 | 1:94193b31f0ee | 89 | |
austinbrown124 | 1:94193b31f0ee | 90 | // |
austinbrown124 | 1:94193b31f0ee | 91 | //current_raw = float(ADC1->DR) - float(inverter.adc1_offset); |
austinbrown124 | 1:94193b31f0ee | 92 | current_raw = float(ADC1->DR) - 2000; |
austinbrown124 | 1:94193b31f0ee | 93 | //current_raw = float(ADC1->DR)); |
austinbrown124 | 1:94193b31f0ee | 94 | //current = current_raw*0.1f; //depends on shunts |
austinbrown124 | 1:94193b31f0ee | 95 | |
austinbrown124 | 1:94193b31f0ee | 96 | inverter.adc2_raw = ADC2->DR; |
austinbrown124 | 1:94193b31f0ee | 97 | inverter.adc1_raw = ADC1->DR; |
austinbrown124 | 1:94193b31f0ee | 98 | clockpin = 1; |
austinbrown124 | 1:94193b31f0ee | 99 | servoinp.update_servo_input(); |
austinbrown124 | 0:9edd6ec0f56a | 100 | |
austinbrown124 | 0:9edd6ec0f56a | 101 | |
austinbrown124 | 0:9edd6ec0f56a | 102 | /// Check state machine state, and run the appropriate function /// |
austinbrown124 | 0:9edd6ec0f56a | 103 | switch(controller_state){ |
austinbrown124 | 0:9edd6ec0f56a | 104 | case REST_MODE: //nothing |
austinbrown124 | 1:94193b31f0ee | 105 | TIM1->CCR1 = 6400; |
austinbrown124 | 1:94193b31f0ee | 106 | count++; |
austinbrown124 | 1:94193b31f0ee | 107 | if(count > 20000){ |
austinbrown124 | 0:9edd6ec0f56a | 108 | count = 0; |
austinbrown124 | 1:94193b31f0ee | 109 | led1 = !led1; |
austinbrown124 | 0:9edd6ec0f56a | 110 | } |
austinbrown124 | 1:94193b31f0ee | 111 | servo_cmd = servoinp.get_servo_input(); |
austinbrown124 | 1:94193b31f0ee | 112 | |
austinbrown124 | 1:94193b31f0ee | 113 | break; |
austinbrown124 | 1:94193b31f0ee | 114 | |
austinbrown124 | 0:9edd6ec0f56a | 115 | |
austinbrown124 | 1:94193b31f0ee | 116 | case VOLTAGE_MODE: |
austinbrown124 | 1:94193b31f0ee | 117 | |
austinbrown124 | 1:94193b31f0ee | 118 | servo_cmd = servoinp.get_servo_input(); |
austinbrown124 | 0:9edd6ec0f56a | 119 | |
austinbrown124 | 1:94193b31f0ee | 120 | TIM1->CCR1 = 6400 - constrain((servo_cmd-880)*8.0f , 0, 6150); //0 to 96% duty cycle |
austinbrown124 | 0:9edd6ec0f56a | 121 | |
austinbrown124 | 1:94193b31f0ee | 122 | if(count > 10000){ count = 0; } |
austinbrown124 | 1:94193b31f0ee | 123 | |
austinbrown124 | 1:94193b31f0ee | 124 | break; |
austinbrown124 | 1:94193b31f0ee | 125 | |
austinbrown124 | 1:94193b31f0ee | 126 | case CURRENT_MODE: |
austinbrown124 | 0:9edd6ec0f56a | 127 | |
austinbrown124 | 1:94193b31f0ee | 128 | servo_cmd = servoinp.get_servo_input(); |
austinbrown124 | 1:94193b31f0ee | 129 | /* |
austinbrown124 | 1:94193b31f0ee | 130 | float current_setpoint = (servo_cmd-880)/100; //0 to 10 amps |
austinbrown124 | 1:94193b31f0ee | 131 | if (current > current_setpoint) { |
austinbrown124 | 1:94193b31f0ee | 132 | //set bridge state to LOW |
austinbrown124 | 1:94193b31f0ee | 133 | TIM1->CCR1 = 6100; |
austinbrown124 | 1:94193b31f0ee | 134 | } |
austinbrown124 | 1:94193b31f0ee | 135 | else { |
austinbrown124 | 1:94193b31f0ee | 136 | TIM1->CCR1 = 300; |
austinbrown124 | 1:94193b31f0ee | 137 | }*/ |
austinbrown124 | 1:94193b31f0ee | 138 | |
austinbrown124 | 0:9edd6ec0f56a | 139 | |
austinbrown124 | 1:94193b31f0ee | 140 | // servo inputs go from 880 to 1700 |
austinbrown124 | 1:94193b31f0ee | 141 | |
austinbrown124 | 1:94193b31f0ee | 142 | // the constant in the next line is set 1800/servo_range |
austinbrown124 | 1:94193b31f0ee | 143 | //float current_setpoint_raw = (servo_cmd-servo_low)*2.25f; //scales 800 range to 1800 |
austinbrown124 | 1:94193b31f0ee | 144 | float current_setpoint_raw = constrain((servo_cmd-servo_low)*2.0f , 0, 800); //scales 800 range to 1800 |
austinbrown124 | 1:94193b31f0ee | 145 | if (current_raw > current_setpoint_raw) { |
austinbrown124 | 1:94193b31f0ee | 146 | //set bridge state to LOW |
austinbrown124 | 1:94193b31f0ee | 147 | TIM1->CCR1 = 6400; |
austinbrown124 | 1:94193b31f0ee | 148 | } |
austinbrown124 | 1:94193b31f0ee | 149 | else { |
austinbrown124 | 1:94193b31f0ee | 150 | TIM1->CCR1 = 250; |
austinbrown124 | 0:9edd6ec0f56a | 151 | } |
austinbrown124 | 0:9edd6ec0f56a | 152 | |
austinbrown124 | 0:9edd6ec0f56a | 153 | |
austinbrown124 | 1:94193b31f0ee | 154 | if ((servo_cmd < servo_low) & (current_raw < 200) & (current_raw > -200)) { |
austinbrown124 | 1:94193b31f0ee | 155 | TIM1->CCR1 = 6400; |
austinbrown124 | 0:9edd6ec0f56a | 156 | } |
austinbrown124 | 0:9edd6ec0f56a | 157 | |
austinbrown124 | 0:9edd6ec0f56a | 158 | |
austinbrown124 | 1:94193b31f0ee | 159 | if(count > 10000){ count = 0; } |
austinbrown124 | 0:9edd6ec0f56a | 160 | |
austinbrown124 | 1:94193b31f0ee | 161 | break; |
austinbrown124 | 0:9edd6ec0f56a | 162 | |
austinbrown124 | 1:94193b31f0ee | 163 | |
austinbrown124 | 1:94193b31f0ee | 164 | } |
austinbrown124 | 0:9edd6ec0f56a | 165 | |
austinbrown124 | 0:9edd6ec0f56a | 166 | } |
austinbrown124 | 1:94193b31f0ee | 167 | //b = TIM1->CNT; |
austinbrown124 | 0:9edd6ec0f56a | 168 | TIM1->SR = 0x0; |
austinbrown124 | 1:94193b31f0ee | 169 | clockpin = 0; // reset the status register |
austinbrown124 | 0:9edd6ec0f56a | 170 | } |
austinbrown124 | 0:9edd6ec0f56a | 171 | |
austinbrown124 | 0:9edd6ec0f56a | 172 | int main() { |
austinbrown124 | 1:94193b31f0ee | 173 | wait_ms(200); |
austinbrown124 | 1:94193b31f0ee | 174 | pc.baud(256000); |
austinbrown124 | 1:94193b31f0ee | 175 | printf("\rStarting Hardware\n"); |
austinbrown124 | 1:94193b31f0ee | 176 | gpio1.mode(PullUp); |
austinbrown124 | 1:94193b31f0ee | 177 | |
austinbrown124 | 0:9edd6ec0f56a | 178 | |
austinbrown124 | 1:94193b31f0ee | 179 | inverter.Init(); // Setup PWM, ADC |
austinbrown124 | 1:94193b31f0ee | 180 | wait(0.1); |
austinbrown124 | 1:94193b31f0ee | 181 | controller_state = REST_MODE; |
austinbrown124 | 1:94193b31f0ee | 182 | wait(0.1); |
austinbrown124 | 1:94193b31f0ee | 183 | inverter.zero_current(); |
austinbrown124 | 0:9edd6ec0f56a | 184 | wait(0.1); |
austinbrown124 | 1:94193b31f0ee | 185 | pc.printf("ADCs zeroed at "); |
austinbrown124 | 1:94193b31f0ee | 186 | pc.printf("%i, %i \n",inverter.adc1_offset,inverter.adc2_offset); |
austinbrown124 | 1:94193b31f0ee | 187 | wait(0.1); |
austinbrown124 | 1:94193b31f0ee | 188 | inverter.ADCsync(); |
austinbrown124 | 1:94193b31f0ee | 189 | |
austinbrown124 | 0:9edd6ec0f56a | 190 | |
austinbrown124 | 1:94193b31f0ee | 191 | |
austinbrown124 | 1:94193b31f0ee | 192 | //controller_state = VOLTAGE_MODE; |
austinbrown124 | 1:94193b31f0ee | 193 | controller_state = CURRENT_MODE; |
austinbrown124 | 1:94193b31f0ee | 194 | |
austinbrown124 | 0:9edd6ec0f56a | 195 | |
austinbrown124 | 0:9edd6ec0f56a | 196 | while(1) { |
austinbrown124 | 1:94193b31f0ee | 197 | |
austinbrown124 | 1:94193b31f0ee | 198 | wait_us(2); |
austinbrown124 | 0:9edd6ec0f56a | 199 | |
austinbrown124 | 1:94193b31f0ee | 200 | while (1==1) { |
austinbrown124 | 1:94193b31f0ee | 201 | |
austinbrown124 | 1:94193b31f0ee | 202 | wait_us(30); |
austinbrown124 | 1:94193b31f0ee | 203 | |
austinbrown124 | 1:94193b31f0ee | 204 | pc.printf("%f, ", servo_cmd); |
austinbrown124 | 0:9edd6ec0f56a | 205 | |
austinbrown124 | 1:94193b31f0ee | 206 | pc.printf("%f, ", current_raw); |
austinbrown124 | 0:9edd6ec0f56a | 207 | |
austinbrown124 | 1:94193b31f0ee | 208 | pc.printf("%f, ", float(inverter.adc1_offset)); |
austinbrown124 | 1:94193b31f0ee | 209 | pc.printf("%f, ", a1); |
austinbrown124 | 1:94193b31f0ee | 210 | pc.printf("%f, ", a2); |
austinbrown124 | 0:9edd6ec0f56a | 211 | |
austinbrown124 | 0:9edd6ec0f56a | 212 | |
austinbrown124 | 1:94193b31f0ee | 213 | //printf("%i, ", b3); |
austinbrown124 | 1:94193b31f0ee | 214 | //printf("%i, ", b4); |
austinbrown124 | 1:94193b31f0ee | 215 | |
austinbrown124 | 1:94193b31f0ee | 216 | //pc.printf("%i, ", TIM15->CNT); |
austinbrown124 | 1:94193b31f0ee | 217 | //pc.printf("%x ", GPIOA->AFR[0]); |
austinbrown124 | 1:94193b31f0ee | 218 | |
austinbrown124 | 1:94193b31f0ee | 219 | pc.printf("%i, ", ADC1->DR); |
austinbrown124 | 1:94193b31f0ee | 220 | pc.printf("%i, ", ADC2->DR); |
austinbrown124 | 1:94193b31f0ee | 221 | //printf(" %i, %i %i", a1, a2, a3); |
austinbrown124 | 1:94193b31f0ee | 222 | //if (gpio2) {pc.printf("low ");} |
austinbrown124 | 1:94193b31f0ee | 223 | pc.printf("\r"); |
austinbrown124 | 1:94193b31f0ee | 224 | |
austinbrown124 | 1:94193b31f0ee | 225 | |
austinbrown124 | 0:9edd6ec0f56a | 226 | } |
austinbrown124 | 1:94193b31f0ee | 227 | |
austinbrown124 | 0:9edd6ec0f56a | 228 | } |
austinbrown124 | 0:9edd6ec0f56a | 229 | } |
austinbrown124 | 1:94193b31f0ee | 230 | |
austinbrown124 | 1:94193b31f0ee | 231 | |
austinbrown124 | 1:94193b31f0ee | 232 | |
austinbrown124 | 1:94193b31f0ee | 233 | |
austinbrown124 | 1:94193b31f0ee | 234 | |
austinbrown124 | 1:94193b31f0ee | 235 | |
austinbrown124 | 1:94193b31f0ee | 236 |