Ian McNanie / Mbed 2 deprecated prius_voltage_mode_ima

motor controller for honda civic ima motor with geartooth sensors

Dependencies:   mbed

Fork of mbed_blinky by Mbed

main.cpp@21:f2277c454d40, 2018-08-08 (annotated)

Committer:
imcnanie
Date:
Wed Aug 08 19:26:14 2018 +0000
Revision:
21:f2277c454d40
Parent:
4:81cea7a352b0 
Voltage Mode Motor Controller; ; TODO:; - Current Sensors; - DQ PIDs; - Sinewave interpolator;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dan 0:7dec7e9ac085 1 #include "mbed.h"
dan 0:7dec7e9ac085 2
imcnanie 21:f2277c454d40 3 PwmOut gate_u(PB_3);
imcnanie 21:f2277c454d40 4 PwmOut gate_v(PB_4);
imcnanie 21:f2277c454d40 5 PwmOut gate_w(PB_5);
imcnanie 21:f2277c454d40 6
imcnanie 21:f2277c454d40 7 //int hallpin_1 = PB_6;
imcnanie 21:f2277c454d40 8 //int hallpin_2 = PC_7;
imcnanie 21:f2277c454d40 9 #define hallpin_2 PB_6
imcnanie 21:f2277c454d40 10 #define hallpin_1 PC_7
imcnanie 21:f2277c454d40 11 #define hallpin_3 PC_9
imcnanie 21:f2277c454d40 12
imcnanie 21:f2277c454d40 13 DigitalIn h1(hallpin_1);
imcnanie 21:f2277c454d40 14 DigitalIn h2(hallpin_2);
imcnanie 21:f2277c454d40 15 DigitalIn h3(hallpin_3);
imcnanie 21:f2277c454d40 16
imcnanie 21:f2277c454d40 17 int hall_1_state = h1;
imcnanie 21:f2277c454d40 18 int hall_2_state = h2;
imcnanie 21:f2277c454d40 19 int hall_3_state = h3;
imcnanie 21:f2277c454d40 20
imcnanie 21:f2277c454d40 21 InterruptIn hall_1(hallpin_1);
imcnanie 21:f2277c454d40 22 InterruptIn hall_2(hallpin_2);
imcnanie 21:f2277c454d40 23 InterruptIn hall_3(hallpin_3);
imcnanie 21:f2277c454d40 24
imcnanie 21:f2277c454d40 25 InterruptIn button1(PA_0);
imcnanie 21:f2277c454d40 26
imcnanie 21:f2277c454d40 27 AnalogIn reverse(PA_4);
imcnanie 21:f2277c454d40 28 AnalogIn throttle(PA_6);
imcnanie 21:f2277c454d40 29
imcnanie 21:f2277c454d40 30 // Array for log
imcnanie 21:f2277c454d40 31 //pc.printf("halls %d %d %d quadrant %d theta %f\n", hall_1_state, hall_2_state, hall_3_state, quadrant, theta);
imcnanie 21:f2277c454d40 32 const int NUM_LOGS = 1;
imcnanie 21:f2277c454d40 33 int log_cnt = 0;
imcnanie 21:f2277c454d40 34 float log_detheta_array[NUM_LOGS];
imcnanie 21:f2277c454d40 35 float log_theta_array[NUM_LOGS];
imcnanie 21:f2277c454d40 36
imcnanie 21:f2277c454d40 37 // PWM properties
imcnanie 21:f2277c454d40 38 float pwm_freq = 8000.0f; //hz
imcnanie 21:f2277c454d40 39 float ff;
imcnanie 21:f2277c454d40 40 float pi = 3.14159;
imcnanie 21:f2277c454d40 41 int bin_theta = 0;
imcnanie 21:f2277c454d40 42 int quadrant = 0;
imcnanie 21:f2277c454d40 43 float theta = 0.0f;
imcnanie 21:f2277c454d40 44
imcnanie 21:f2277c454d40 45 float Ia = 0.0f;
imcnanie 21:f2277c454d40 46 float Ib = 0.0f;
imcnanie 21:f2277c454d40 47 float Ic = 0.0f;
imcnanie 21:f2277c454d40 48
imcnanie 21:f2277c454d40 49 float Id = 0.0f;
imcnanie 21:f2277c454d40 50 float Iq = -0.2f;
imcnanie 21:f2277c454d40 51
imcnanie 21:f2277c454d40 52 // setup pwm interrupt. put freq in this area soon. figure out how to do center aligned pwm!!
imcnanie 21:f2277c454d40 53 Ticker pwm_interrupt;
imcnanie 21:f2277c454d40 54 float datheta = 0.0f;
imcnanie 21:f2277c454d40 55
imcnanie 21:f2277c454d40 56 // This board is whack, needs a custom serial setup
imcnanie 21:f2277c454d40 57 Serial pc(PA_2, PA_3);
imcnanie 21:f2277c454d40 58
imcnanie 21:f2277c454d40 59 // hall interrupts
imcnanie 21:f2277c454d40 60 void hall_1_rise(){ hall_1_state = 1; }
imcnanie 21:f2277c454d40 61 void hall_2_rise(){ hall_2_state = 1; }
imcnanie 21:f2277c454d40 62 void hall_3_rise(){ hall_3_state = 1; }
imcnanie 21:f2277c454d40 63
imcnanie 21:f2277c454d40 64 void hall_1_fall(){ hall_1_state = 0; }
imcnanie 21:f2277c454d40 65 void hall_2_fall(){ hall_2_state = 0; }
imcnanie 21:f2277c454d40 66 void hall_3_fall(){ hall_3_state = 0; }
imcnanie 21:f2277c454d40 67
imcnanie 21:f2277c454d40 68 void button_1_rise(){ //pc.printf("detheta %f theta: %f halls: %d %d %d\n", log_detheta_array[0], log_theta_array[0], hall_1_state, hall_2_state, hall_3_state);
imcnanie 21:f2277c454d40 69 pc.printf("detheta %f theta: %f\n", log_detheta_array[0], log_theta_array[0]);}
imcnanie 21:f2277c454d40 70
imcnanie 21:f2277c454d40 71 void check_commutation() {
imcnanie 21:f2277c454d40 72
imcnanie 21:f2277c454d40 73 bin_theta = hall_1_state<<2 | hall_2_state<< 1 | hall_3_state;
imcnanie 21:f2277c454d40 74
imcnanie 21:f2277c454d40 75 if (bin_theta == 0b001) { quadrant = 5; }
imcnanie 21:f2277c454d40 76 if (bin_theta == 0b101) { quadrant = 4; }
imcnanie 21:f2277c454d40 77 if (bin_theta == 0b100) { quadrant = 3; }
imcnanie 21:f2277c454d40 78 if (bin_theta == 0b110) { quadrant = 2; }
imcnanie 21:f2277c454d40 79 if (bin_theta == 0b010) { quadrant = 1; }
imcnanie 21:f2277c454d40 80 if (bin_theta == 0b011) { quadrant = 0; }
imcnanie 21:f2277c454d40 81 // Position sensor is offset in this case by 180 degrees
imcnanie 21:f2277c454d40 82 theta = (float)quadrant+3.0f*pi/3.0f;
imcnanie 21:f2277c454d40 83
imcnanie 21:f2277c454d40 84 log_detheta_array[0] = datheta;
imcnanie 21:f2277c454d40 85 log_theta_array[0] = theta;
imcnanie 21:f2277c454d40 86
imcnanie 21:f2277c454d40 87 Iq = throttle.read() - reverse.read();
imcnanie 21:f2277c454d40 88
imcnanie 21:f2277c454d40 89 //theta = datheta;
imcnanie 21:f2277c454d40 90 //theta = theta + 6.0f*pi/3.0f;
imcnanie 21:f2277c454d40 91 Ia = Id*cosf(theta) - Iq*sinf(theta);
imcnanie 21:f2277c454d40 92 Ib = Id*cosf(theta-(2.0f/3.0f)*pi) - Iq*sinf(theta-(2.0f/3.0f)*pi);
imcnanie 21:f2277c454d40 93 Ic = Id*cosf(theta+(2.0f/3.0f)*pi) - Iq*sinf(theta+(2.0f/3.0f)*pi);
imcnanie 21:f2277c454d40 94
imcnanie 21:f2277c454d40 95 // This will only give 80% of total available power, eventually use svm
imcnanie 21:f2277c454d40 96 // https://en.wikipedia.org/wiki/Space_vector_modulation
imcnanie 21:f2277c454d40 97 gate_u.write(0.5f+Ia/2.0f);
imcnanie 21:f2277c454d40 98 gate_v.write(0.5f+Ib/2.0f);
imcnanie 21:f2277c454d40 99 gate_w.write(0.5f+Ic/2.0f);
imcnanie 21:f2277c454d40 100
imcnanie 21:f2277c454d40 101 // log it all
imcnanie 21:f2277c454d40 102 //pc.printf("halls %d %d %d quadrant %d theta %f\n", hall_1_state, hall_2_state, hall_3_state, quadrant, theta);
imcnanie 21:f2277c454d40 103
imcnanie 21:f2277c454d40 104
imcnanie 21:f2277c454d40 105 }
dan 0:7dec7e9ac085 106
dan 0:7dec7e9ac085 107 int main() {
imcnanie 21:f2277c454d40 108 // specify period first, then everything else
imcnanie 21:f2277c454d40 109 gate_u.period(1.0f/pwm_freq); // 4 second period
imcnanie 21:f2277c454d40 110 gate_v.period(1.0f/pwm_freq); // 4 second period
imcnanie 21:f2277c454d40 111 gate_w.period(1.0f/pwm_freq); // 4 second period
imcnanie 21:f2277c454d40 112
imcnanie 21:f2277c454d40 113 gate_u.write(0.0f); // 50% duty cycle
imcnanie 21:f2277c454d40 114 gate_v.write(0.0f); // 50% duty cycle
imcnanie 21:f2277c454d40 115 gate_w.write(0.0f); // 50% duty cycle
imcnanie 21:f2277c454d40 116
imcnanie 21:f2277c454d40 117 hall_1.rise(&hall_1_rise);
imcnanie 21:f2277c454d40 118 hall_1.fall(&hall_1_fall);
imcnanie 21:f2277c454d40 119
imcnanie 21:f2277c454d40 120 hall_2.rise(&hall_2_rise);
imcnanie 21:f2277c454d40 121 hall_2.fall(&hall_2_fall);
imcnanie 21:f2277c454d40 122
imcnanie 21:f2277c454d40 123 hall_3.rise(&hall_3_rise);
imcnanie 21:f2277c454d40 124 hall_3.fall(&hall_3_fall);
imcnanie 21:f2277c454d40 125
imcnanie 21:f2277c454d40 126 button1.rise(&button_1_rise);
imcnanie 21:f2277c454d40 127
imcnanie 21:f2277c454d40 128 hall_1.mode(PullUp);
imcnanie 21:f2277c454d40 129 hall_2.mode(PullUp);
imcnanie 21:f2277c454d40 130 hall_3.mode(PullUp);
imcnanie 21:f2277c454d40 131
imcnanie 21:f2277c454d40 132 // Make this center aligned eventually
imcnanie 21:f2277c454d40 133 pwm_interrupt.attach(&check_commutation, 1.0f/pwm_freq);
imcnanie 21:f2277c454d40 134
imcnanie 21:f2277c454d40 135 int loop_cnt = 0;
imcnanie 21:f2277c454d40 136 while (1) {
imcnanie 21:f2277c454d40 137 //pc.printf("nice\n");
imcnanie 21:f2277c454d40 138 //pc.printf("Halls: %d %d %d\n", hall_1_state, hall_2_state, hall_3_state);
imcnanie 21:f2277c454d40 139 //pc.printf("Angle: %f Ia Ib Ic %f %f %f\n", theta, Ia, Ib, Ic);
imcnanie 21:f2277c454d40 140 //pc.printf("Angle: %f \n", theta);
imcnanie 21:f2277c454d40 141 //pc.printf("halls %d %d %d quadrant %d theta %f\n", hall_1_state, hall_2_state, hall_3_state, quadrant, theta);
imcnanie 21:f2277c454d40 142 datheta += 0.05f;
imcnanie 21:f2277c454d40 143 wait(0.001);
imcnanie 21:f2277c454d40 144 if (datheta > 2.0f*pi) {
imcnanie 21:f2277c454d40 145 datheta = 0.0f;
imcnanie 21:f2277c454d40 146 }
imcnanie 21:f2277c454d40 147 loop_cnt++;
imcnanie 21:f2277c454d40 148 if (loop_cnt > 800) {
imcnanie 21:f2277c454d40 149 //pc.printf("detheta %f theta: %f\n", log_detheta_array[0], log_theta_array[0]);
imcnanie 21:f2277c454d40 150 loop_cnt = 0;
imcnanie 21:f2277c454d40 151 }
imcnanie 21:f2277c454d40 152 //pc.printf("Theta: %f Halls: %d %d %d\n", theta, hall_1_state, hall_2_state, hall_3_state);
imcnanie 21:f2277c454d40 153 //update_commutation();
imcnanie 21:f2277c454d40 154
imcnanie 21:f2277c454d40 155
imcnanie 21:f2277c454d40 156 //pc.printf("Ia %f Ib %f Ic %f\n", Ia, Ib, Ic);
imcnanie 21:f2277c454d40 157 //pc.printf("Reverse: %f Throttle: %f\n", reverse.read(), throttle.read());
stevep 4:81cea7a352b0 158 }
dan 0:7dec7e9ac085 159 }