Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_PostLIGHT_Original

Original Version of STM Board

Dependencies:   FastPWM mbed

main.cpp@5:a4319f79457b, 2019-08-20 (annotated)

Committer:
jobuuu
Date:
Tue Aug 20 10:12:00 2019 +0000
Revision:
5:a4319f79457b
Parent:
2:a1c0a37df760
Child:
6:df07d3491e3a
dd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GiJeongKim 0:51c43836c1d7 1 #include "mbed.h"
GiJeongKim 0:51c43836c1d7 2 #include "FastPWM.h"
GiJeongKim 0:51c43836c1d7 3 #include "INIT_HW.h"
jobuuu 2:a1c0a37df760 4 #include "function_CAN.h"
GiJeongKim 0:51c43836c1d7 5 #include "SPI_EEP_ENC.h"
GiJeongKim 0:51c43836c1d7 6 #include "I2C_AS5510.h"
GiJeongKim 0:51c43836c1d7 7 #include "setting.h"
GiJeongKim 0:51c43836c1d7 8
GiJeongKim 0:51c43836c1d7 9 // dac & check
GiJeongKim 0:51c43836c1d7 10 DigitalOut check(PC_2);
GiJeongKim 0:51c43836c1d7 11 DigitalOut check_2(PC_3);
GiJeongKim 0:51c43836c1d7 12 AnalogOut dac_1(PA_4);
GiJeongKim 0:51c43836c1d7 13 AnalogOut dac_2(PA_5);
jobuuu 5:a4319f79457b 14 //AnalogIn adc3(PC_1);
GiJeongKim 0:51c43836c1d7 15
GiJeongKim 0:51c43836c1d7 16 // pwm
jobuuu 2:a1c0a37df760 17 double dtc_v=0.0;
jobuuu 2:a1c0a37df760 18 double dtc_w=0.0;
GiJeongKim 0:51c43836c1d7 19
GiJeongKim 0:51c43836c1d7 20 // I2C
GiJeongKim 0:51c43836c1d7 21 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
GiJeongKim 0:51c43836c1d7 22 const int i2c_slave_addr1 = 0x56;
GiJeongKim 0:51c43836c1d7 23 unsigned int value; // 10bit output of reading sensor AS5510
GiJeongKim 0:51c43836c1d7 24
GiJeongKim 0:51c43836c1d7 25 // SPI
GiJeongKim 0:51c43836c1d7 26 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
GiJeongKim 0:51c43836c1d7 27 DigitalOut eeprom_cs(PB_12);
GiJeongKim 0:51c43836c1d7 28 SPI enc(PC_12,PC_11,PC_10);
GiJeongKim 0:51c43836c1d7 29 DigitalOut enc_cs(PD_2);
jobuuu 2:a1c0a37df760 30 DigitalOut indi_led(PA_15);
GiJeongKim 0:51c43836c1d7 31
GiJeongKim 0:51c43836c1d7 32 // UART
GiJeongKim 0:51c43836c1d7 33 Serial pc(PA_9,PA_10); // _ UART
GiJeongKim 0:51c43836c1d7 34
GiJeongKim 0:51c43836c1d7 35 //CAN
GiJeongKim 0:51c43836c1d7 36 CAN can(PB_8, PB_9, 1000000);
GiJeongKim 0:51c43836c1d7 37 CANMessage msg;
jobuuu 2:a1c0a37df760 38
jobuuu 2:a1c0a37df760 39 // Variables
jobuuu 5:a4319f79457b 40 double cur = 0.0;
jobuuu 5:a4319f79457b 41 double cur_ref = 0.0;
jobuuu 5:a4319f79457b 42 double cur_ref_old = 0.0;
jobuuu 5:a4319f79457b 43 double cur_ref_diff = 0.0;
jobuuu 5:a4319f79457b 44 double cur_err = 0.0;
jobuuu 5:a4319f79457b 45 double cur_err_int = 0.0;
jobuuu 5:a4319f79457b 46 double cur_err_old = 0.0;
jobuuu 5:a4319f79457b 47 double cur_err_diff = 0.0;
GiJeongKim 0:51c43836c1d7 48
jobuuu 5:a4319f79457b 49 double pos = 0.0;
jobuuu 5:a4319f79457b 50 double pos_ref = 0.0;
GiJeongKim 0:51c43836c1d7 51
jobuuu 5:a4319f79457b 52 double vel;
jobuuu 5:a4319f79457b 53 double vel_ref;
jobuuu 5:a4319f79457b 54
jobuuu 5:a4319f79457b 55 double pres_A;
jobuuu 5:a4319f79457b 56 double pres_B;
jobuuu 5:a4319f79457b 57
jobuuu 5:a4319f79457b 58 double V_out=0.0;
jobuuu 5:a4319f79457b 59 double V_rem=0.0; // for anti-windup
jobuuu 5:a4319f79457b 60 double V_MAX = 12000.0; // Maximum Voltage : 12V = 12000mV
jobuuu 5:a4319f79457b 61
jobuuu 5:a4319f79457b 62 double PWM_out=0.0;
jobuuu 5:a4319f79457b 63
GiJeongKim 0:51c43836c1d7 64 int main()
GiJeongKim 0:51c43836c1d7 65 {
jobuuu 1:e04e563be5ce 66 /*******************************************************
jobuuu 1:e04e563be5ce 67 *** Initialization
jobuuu 1:e04e563be5ce 68 ********************************************************/
jobuuu 1:e04e563be5ce 69
jobuuu 2:a1c0a37df760 70 indi_led = 0;
jobuuu 1:e04e563be5ce 71 pc.baud(9600);
jobuuu 2:a1c0a37df760 72
jobuuu 2:a1c0a37df760 73 //Timer t;
jobuuu 2:a1c0a37df760 74 //t.start();
jobuuu 2:a1c0a37df760 75 //t.stop();
jobuuu 2:a1c0a37df760 76 //pc.printf("The time taken was %f seconds\n",t.read());
jobuuu 2:a1c0a37df760 77
GiJeongKim 0:51c43836c1d7 78 // i2c init
GiJeongKim 0:51c43836c1d7 79 // i2c.frequency(400 * 1000); // 0.4 mHz
GiJeongKim 0:51c43836c1d7 80 // wait_ms(2); // Power Up wait
GiJeongKim 0:51c43836c1d7 81 // look_for_hardware_i2c(); // Hardware present
GiJeongKim 0:51c43836c1d7 82 // init_as5510(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 83
GiJeongKim 0:51c43836c1d7 84 // // spi init
GiJeongKim 0:51c43836c1d7 85 eeprom.format(8,3);
GiJeongKim 0:51c43836c1d7 86 eeprom.frequency(5000000); //5M
GiJeongKim 0:51c43836c1d7 87 enc.format(8,0);
GiJeongKim 0:51c43836c1d7 88 enc.frequency(5000000); //5M
jobuuu 2:a1c0a37df760 89
GiJeongKim 0:51c43836c1d7 90 // ADC init
jobuuu 5:a4319f79457b 91 Init_ADC();
jobuuu 2:a1c0a37df760 92
GiJeongKim 0:51c43836c1d7 93 // Pwm init
GiJeongKim 0:51c43836c1d7 94 Init_PWM();
GiJeongKim 0:51c43836c1d7 95 TIM4->CR1 ^= TIM_CR1_UDIS;
jobuuu 2:a1c0a37df760 96
GiJeongKim 0:51c43836c1d7 97 // //SPI
GiJeongKim 0:51c43836c1d7 98 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 99 // spi_eeprom_write(0x1,0x112);
GiJeongKim 0:51c43836c1d7 100 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 101 // int i = spi_eeprom_read(0x1);
jobuuu 2:a1c0a37df760 102
GiJeongKim 0:51c43836c1d7 103 // CAN
jobuuu 2:a1c0a37df760 104 can.attach(&CAN_RX_HANDLER);
jobuuu 2:a1c0a37df760 105
GiJeongKim 0:51c43836c1d7 106 // spi _ enc
GiJeongKim 0:51c43836c1d7 107 spi_enc_set_init();
jobuuu 1:e04e563be5ce 108
jobuuu 1:e04e563be5ce 109
jobuuu 1:e04e563be5ce 110 /*******************************************************
jobuuu 1:e04e563be5ce 111 *** Program is operating!
jobuuu 1:e04e563be5ce 112 ********************************************************/
GiJeongKim 0:51c43836c1d7 113 while(1) {
GiJeongKim 0:51c43836c1d7 114 // dac_1=0.5;
GiJeongKim 0:51c43836c1d7 115 // dac_2=0.1;
GiJeongKim 0:51c43836c1d7 116 check_2 = 1;
GiJeongKim 0:51c43836c1d7 117 //spi _ eeprom
GiJeongKim 0:51c43836c1d7 118 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 119 // spi_eeprom_write(0x0001,0xFFFFFFFF);
GiJeongKim 0:51c43836c1d7 120 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 121 // int a=spi_eeprom_read(0x0001);
GiJeongKim 0:51c43836c1d7 122 //spi _ enc
GiJeongKim 0:51c43836c1d7 123 int a = spi_enc_read();
GiJeongKim 0:51c43836c1d7 124 // read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 125
GiJeongKim 0:51c43836c1d7 126 check_2=0;
jobuuu 2:a1c0a37df760 127
jobuuu 2:a1c0a37df760 128 // pc.printf("%f\n",PWM_out);
jobuuu 2:a1c0a37df760 129 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 130
jobuuu 2:a1c0a37df760 131 // wait(0.01f);
GiJeongKim 0:51c43836c1d7 132 }
jobuuu 1:e04e563be5ce 133 }
jobuuu 1:e04e563be5ce 134
jobuuu 2:a1c0a37df760 135 /*******************************************************************************
jobuuu 2:a1c0a37df760 136 TIMER INTERRUPT
jobuuu 2:a1c0a37df760 137 *******************************************************************************/
jobuuu 2:a1c0a37df760 138
jobuuu 2:a1c0a37df760 139 unsigned long CNT_TMR4 = 0;
jobuuu 2:a1c0a37df760 140 double FREQ_TMR4 = (double)FREQ_20k;
jobuuu 5:a4319f79457b 141 double DT_TMR4 = (double)DT_20k;
jobuuu 1:e04e563be5ce 142 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 143 {
jobuuu 2:a1c0a37df760 144 if ( TIM4->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 145 /*******************************************************
jobuuu 1:e04e563be5ce 146 *** Sensor Read & Data Handling
jobuuu 1:e04e563be5ce 147 ********************************************************/
jobuuu 1:e04e563be5ce 148
jobuuu 2:a1c0a37df760 149 if((CNT_TMR4%2)==0){
jobuuu 2:a1c0a37df760 150 //spi
jobuuu 2:a1c0a37df760 151 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 152 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 153 // ready();
jobuuu 2:a1c0a37df760 154 // read(1);
jobuuu 2:a1c0a37df760 155
jobuuu 2:a1c0a37df760 156 //i2c
jobuuu 2:a1c0a37df760 157 //// read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 158
jobuuu 2:a1c0a37df760 159 //ADC
jobuuu 5:a4319f79457b 160 ADC3->CR2 |= 0x40000000; // adc _ 12bit
jobuuu 2:a1c0a37df760 161 // a1=ADC1->DR;
jobuuu 2:a1c0a37df760 162 // a1=ADC2->DR;
jobuuu 5:a4319f79457b 163 // int raw_cur = ADC3->DR;
jobuuu 5:a4319f79457b 164 while((ADC3->SR & 0b10));
jobuuu 5:a4319f79457b 165
jobuuu 5:a4319f79457b 166 double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
jobuuu 5:a4319f79457b 167 double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
jobuuu 5:a4319f79457b 168 cur=cur*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
jobuuu 2:a1c0a37df760 169 }
jobuuu 1:e04e563be5ce 170
jobuuu 1:e04e563be5ce 171 //DAC
jobuuu 1:e04e563be5ce 172 // dac_1 = ADC1->DR;
jobuuu 1:e04e563be5ce 173 // dac_2 = ADC2->DR;
jobuuu 1:e04e563be5ce 174
jobuuu 1:e04e563be5ce 175 /*******************************************************
jobuuu 5:a4319f79457b 176 *** Valve Current Control
jobuuu 1:e04e563be5ce 177 ********************************************************/
jobuuu 1:e04e563be5ce 178
jobuuu 5:a4319f79457b 179 bool FLAG_current_control = false;
jobuuu 5:a4319f79457b 180 if(FLAG_current_control) {
jobuuu 5:a4319f79457b 181 cur_err = cur_ref - cur;
jobuuu 5:a4319f79457b 182 cur_err_int = cur_err_int + cur_err*DT_TMR4;
jobuuu 5:a4319f79457b 183 cur_err_diff = (cur_err - cur_err_old)*FREQ_TMR4;
jobuuu 5:a4319f79457b 184 cur_err_old = cur_err;
jobuuu 5:a4319f79457b 185
jobuuu 5:a4319f79457b 186 double R_model = 150.0; // ohm
jobuuu 5:a4319f79457b 187 double L_model = 0.3;
jobuuu 5:a4319f79457b 188 double w0 = 2.0*3.14*90.0;
jobuuu 5:a4319f79457b 189 double KP_I = L_model*w0;
jobuuu 5:a4319f79457b 190 double KI_I = R_model*w0;
jobuuu 5:a4319f79457b 191 double KD_I = 0.0;
jobuuu 5:a4319f79457b 192
jobuuu 5:a4319f79457b 193 double FF_gain = 0.0;
jobuuu 5:a4319f79457b 194 V_out = (int) (KP_I * cur_err + KI_I * cur_err_int + KD_I * cur_err_diff);
jobuuu 5:a4319f79457b 195 // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
jobuuu 5:a4319f79457b 196 V_out = V_out + FF_gain * (R_model*cur_ref + L_model*cur_ref_diff); // Unit : mV
jobuuu 5:a4319f79457b 197
jobuuu 5:a4319f79457b 198 double Ka = 5.0/KP_I;
jobuuu 5:a4319f79457b 199 if(V_out > V_MAX) {
jobuuu 5:a4319f79457b 200 V_rem = V_out-V_MAX;
jobuuu 5:a4319f79457b 201 V_rem = Ka*V_rem;
jobuuu 5:a4319f79457b 202 V_out = V_MAX;
jobuuu 5:a4319f79457b 203 cur_err_int = cur_err_int - V_rem*DT_5k;
jobuuu 5:a4319f79457b 204 } else if(V_out < -V_MAX) {
jobuuu 5:a4319f79457b 205 V_rem = V_out-(-V_MAX);
jobuuu 5:a4319f79457b 206 V_rem = Ka*V_rem;
jobuuu 5:a4319f79457b 207 V_out = -V_MAX;
jobuuu 5:a4319f79457b 208 cur_err_int = cur_err_int - V_rem*DT_5k;
jobuuu 5:a4319f79457b 209 }
jobuuu 5:a4319f79457b 210
jobuuu 5:a4319f79457b 211 } else {
jobuuu 5:a4319f79457b 212 // PWM_RAW : -5000.0mV~5000.0mV(full duty)
jobuuu 5:a4319f79457b 213 double t = (double)CNT_TMR4*DT_TMR4;
jobuuu 5:a4319f79457b 214 double T = 5.0;
jobuuu 5:a4319f79457b 215
jobuuu 5:a4319f79457b 216 V_out = 1000.0*sin(2.0*PI*t/T); // Unit : mV
jobuuu 5:a4319f79457b 217 // if(V_out > 0.0) V_out = 1000.0;
jobuuu 5:a4319f79457b 218 // else if(V_out < 0.0) V_out = -1000.0;
jobuuu 5:a4319f79457b 219 }
jobuuu 5:a4319f79457b 220
jobuuu 5:a4319f79457b 221 PWM_out= V_out/12000.0; // Full duty : 12000.0mV
jobuuu 2:a1c0a37df760 222
jobuuu 2:a1c0a37df760 223 // Saturation of output voltage to 5.0V
jobuuu 5:a4319f79457b 224 if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
jobuuu 5:a4319f79457b 225 else if (PWM_out < -0.41667) PWM_out=-0.41667;
jobuuu 2:a1c0a37df760 226
jobuuu 2:a1c0a37df760 227 if (PWM_out>0.0) {
jobuuu 2:a1c0a37df760 228 dtc_v=0.0;
jobuuu 1:e04e563be5ce 229 dtc_w=PWM_out;
jobuuu 1:e04e563be5ce 230 } else {
jobuuu 2:a1c0a37df760 231 dtc_v=-PWM_out;
jobuuu 2:a1c0a37df760 232 dtc_w=0.0;
jobuuu 1:e04e563be5ce 233 }
jobuuu 1:e04e563be5ce 234
jobuuu 1:e04e563be5ce 235 //pwm
jobuuu 2:a1c0a37df760 236 TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu 2:a1c0a37df760 237 TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
jobuuu 1:e04e563be5ce 238
jobuuu 1:e04e563be5ce 239 /*******************************************************
jobuuu 2:a1c0a37df760 240 *** Data Send (CAN) & Print out (UART)
jobuuu 1:e04e563be5ce 241 ********************************************************/
jobuuu 2:a1c0a37df760 242 if((CNT_TMR4%40)==0){
jobuuu 2:a1c0a37df760 243 msg.id = 50;
jobuuu 2:a1c0a37df760 244 msg.len = 4;
jobuuu 5:a4319f79457b 245 int temp_CUR = (int)(cur*1000.0);
jobuuu 2:a1c0a37df760 246 msg.data[0]=0x00FF&temp_CUR;
jobuuu 2:a1c0a37df760 247 msg.data[1]=0x00FF&(temp_CUR>>8);
jobuuu 5:a4319f79457b 248 int temp_PWM = (int)(V_out);
jobuuu 2:a1c0a37df760 249 msg.data[2]=0x00FF&temp_PWM;
jobuuu 2:a1c0a37df760 250 msg.data[3]=0x00FF&(temp_PWM>>8);
jobuuu 2:a1c0a37df760 251 can.write(msg);
jobuuu 2:a1c0a37df760 252 }
jobuuu 1:e04e563be5ce 253
jobuuu 2:a1c0a37df760 254 // if((CNT_TMR4%4000)==0){
jobuuu 2:a1c0a37df760 255 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 256 // }
jobuuu 1:e04e563be5ce 257
jobuuu 1:e04e563be5ce 258 /*******************************************************
jobuuu 1:e04e563be5ce 259 *** Timer Counting & etc.
jobuuu 1:e04e563be5ce 260 ********************************************************/
jobuuu 2:a1c0a37df760 261 CNT_TMR4++;
jobuuu 1:e04e563be5ce 262 }
jobuuu 1:e04e563be5ce 263 TIM4->SR = 0x0; // reset the status register
GiJeongKim 0:51c43836c1d7 264 }