Austin Brown / Mbed 2 deprecated MikeEtekController

Mike Etek Controller

Dependencies:   mbed

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?

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