Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_LIGHT

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FastPWM

main.cpp@6:df07d3491e3a, 2019-08-20 (annotated)

Committer:
jobuuu
Date:
Tue Aug 20 10:40:27 2019 +0000
Revision:
6:df07d3491e3a
Parent:
5:a4319f79457b
Child:
7:e9086c72bb22
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 6:df07d3491e3a 66 /*********************************
jobuuu 1:e04e563be5ce 67 *** Initialization
jobuuu 6:df07d3491e3a 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 6:df07d3491e3a 110 /************************************
jobuuu 1:e04e563be5ce 111 *** Program is operating!
jobuuu 6:df07d3491e3a 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 6:df07d3491e3a 135
jobuuu 6:df07d3491e3a 136
jobuuu 6:df07d3491e3a 137
jobuuu 2:a1c0a37df760 138 /*******************************************************************************
jobuuu 2:a1c0a37df760 139 TIMER INTERRUPT
jobuuu 2:a1c0a37df760 140 *******************************************************************************/
jobuuu 2:a1c0a37df760 141
jobuuu 2:a1c0a37df760 142 unsigned long CNT_TMR4 = 0;
jobuuu 2:a1c0a37df760 143 double FREQ_TMR4 = (double)FREQ_20k;
jobuuu 5:a4319f79457b 144 double DT_TMR4 = (double)DT_20k;
jobuuu 1:e04e563be5ce 145 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 146 {
jobuuu 2:a1c0a37df760 147 if ( TIM4->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 148 /*******************************************************
jobuuu 1:e04e563be5ce 149 *** Sensor Read & Data Handling
jobuuu 1:e04e563be5ce 150 ********************************************************/
jobuuu 1:e04e563be5ce 151
jobuuu 2:a1c0a37df760 152 if((CNT_TMR4%2)==0){
jobuuu 2:a1c0a37df760 153 //spi
jobuuu 2:a1c0a37df760 154 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 155 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 156 // ready();
jobuuu 2:a1c0a37df760 157 // read(1);
jobuuu 2:a1c0a37df760 158
jobuuu 2:a1c0a37df760 159 //i2c
jobuuu 2:a1c0a37df760 160 //// read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 161
jobuuu 2:a1c0a37df760 162 //ADC
jobuuu 5:a4319f79457b 163 ADC3->CR2 |= 0x40000000; // adc _ 12bit
jobuuu 2:a1c0a37df760 164 // a1=ADC1->DR;
jobuuu 2:a1c0a37df760 165 // a1=ADC2->DR;
jobuuu 5:a4319f79457b 166 // int raw_cur = ADC3->DR;
jobuuu 5:a4319f79457b 167 while((ADC3->SR & 0b10));
jobuuu 5:a4319f79457b 168
jobuuu 5:a4319f79457b 169 double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
jobuuu 5:a4319f79457b 170 double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
jobuuu 5:a4319f79457b 171 cur=cur*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
jobuuu 2:a1c0a37df760 172 }
jobuuu 1:e04e563be5ce 173
jobuuu 1:e04e563be5ce 174 //DAC
jobuuu 1:e04e563be5ce 175 // dac_1 = ADC1->DR;
jobuuu 1:e04e563be5ce 176 // dac_2 = ADC2->DR;
jobuuu 1:e04e563be5ce 177
jobuuu 1:e04e563be5ce 178 /*******************************************************
jobuuu 6:df07d3491e3a 179 *** Valve Control
jobuuu 1:e04e563be5ce 180 ********************************************************/
jobuuu 1:e04e563be5ce 181
jobuuu 5:a4319f79457b 182 bool FLAG_current_control = false;
jobuuu 5:a4319f79457b 183 if(FLAG_current_control) {
jobuuu 5:a4319f79457b 184 cur_err = cur_ref - cur;
jobuuu 5:a4319f79457b 185 cur_err_int = cur_err_int + cur_err*DT_TMR4;
jobuuu 5:a4319f79457b 186 cur_err_diff = (cur_err - cur_err_old)*FREQ_TMR4;
jobuuu 5:a4319f79457b 187 cur_err_old = cur_err;
jobuuu 5:a4319f79457b 188
jobuuu 5:a4319f79457b 189 double R_model = 150.0; // ohm
jobuuu 5:a4319f79457b 190 double L_model = 0.3;
jobuuu 5:a4319f79457b 191 double w0 = 2.0*3.14*90.0;
jobuuu 5:a4319f79457b 192 double KP_I = L_model*w0;
jobuuu 5:a4319f79457b 193 double KI_I = R_model*w0;
jobuuu 5:a4319f79457b 194 double KD_I = 0.0;
jobuuu 5:a4319f79457b 195
jobuuu 5:a4319f79457b 196 double FF_gain = 0.0;
jobuuu 5:a4319f79457b 197 V_out = (int) (KP_I * cur_err + KI_I * cur_err_int + KD_I * cur_err_diff);
jobuuu 5:a4319f79457b 198 // V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
jobuuu 5:a4319f79457b 199 V_out = V_out + FF_gain * (R_model*cur_ref + L_model*cur_ref_diff); // Unit : mV
jobuuu 5:a4319f79457b 200
jobuuu 5:a4319f79457b 201 double Ka = 5.0/KP_I;
jobuuu 5:a4319f79457b 202 if(V_out > V_MAX) {
jobuuu 5:a4319f79457b 203 V_rem = V_out-V_MAX;
jobuuu 5:a4319f79457b 204 V_rem = Ka*V_rem;
jobuuu 5:a4319f79457b 205 V_out = V_MAX;
jobuuu 5:a4319f79457b 206 cur_err_int = cur_err_int - V_rem*DT_5k;
jobuuu 5:a4319f79457b 207 } else if(V_out < -V_MAX) {
jobuuu 5:a4319f79457b 208 V_rem = V_out-(-V_MAX);
jobuuu 5:a4319f79457b 209 V_rem = Ka*V_rem;
jobuuu 5:a4319f79457b 210 V_out = -V_MAX;
jobuuu 5:a4319f79457b 211 cur_err_int = cur_err_int - V_rem*DT_5k;
jobuuu 5:a4319f79457b 212 }
jobuuu 5:a4319f79457b 213
jobuuu 5:a4319f79457b 214 } else {
jobuuu 5:a4319f79457b 215 // PWM_RAW : -5000.0mV~5000.0mV(full duty)
jobuuu 5:a4319f79457b 216 double t = (double)CNT_TMR4*DT_TMR4;
jobuuu 5:a4319f79457b 217 double T = 5.0;
jobuuu 5:a4319f79457b 218
jobuuu 5:a4319f79457b 219 V_out = 1000.0*sin(2.0*PI*t/T); // Unit : mV
jobuuu 5:a4319f79457b 220 // if(V_out > 0.0) V_out = 1000.0;
jobuuu 5:a4319f79457b 221 // else if(V_out < 0.0) V_out = -1000.0;
jobuuu 5:a4319f79457b 222 }
jobuuu 5:a4319f79457b 223
jobuuu 5:a4319f79457b 224 PWM_out= V_out/12000.0; // Full duty : 12000.0mV
jobuuu 2:a1c0a37df760 225
jobuuu 2:a1c0a37df760 226 // Saturation of output voltage to 5.0V
jobuuu 5:a4319f79457b 227 if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
jobuuu 5:a4319f79457b 228 else if (PWM_out < -0.41667) PWM_out=-0.41667;
jobuuu 2:a1c0a37df760 229
jobuuu 2:a1c0a37df760 230 if (PWM_out>0.0) {
jobuuu 2:a1c0a37df760 231 dtc_v=0.0;
jobuuu 1:e04e563be5ce 232 dtc_w=PWM_out;
jobuuu 1:e04e563be5ce 233 } else {
jobuuu 2:a1c0a37df760 234 dtc_v=-PWM_out;
jobuuu 2:a1c0a37df760 235 dtc_w=0.0;
jobuuu 1:e04e563be5ce 236 }
jobuuu 1:e04e563be5ce 237
jobuuu 1:e04e563be5ce 238 //pwm
jobuuu 2:a1c0a37df760 239 TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu 2:a1c0a37df760 240 TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
jobuuu 1:e04e563be5ce 241
jobuuu 1:e04e563be5ce 242 /*******************************************************
jobuuu 2:a1c0a37df760 243 *** Data Send (CAN) & Print out (UART)
jobuuu 1:e04e563be5ce 244 ********************************************************/
jobuuu 2:a1c0a37df760 245 if((CNT_TMR4%40)==0){
jobuuu 2:a1c0a37df760 246 msg.id = 50;
jobuuu 2:a1c0a37df760 247 msg.len = 4;
jobuuu 5:a4319f79457b 248 int temp_CUR = (int)(cur*1000.0);
jobuuu 2:a1c0a37df760 249 msg.data[0]=0x00FF&temp_CUR;
jobuuu 2:a1c0a37df760 250 msg.data[1]=0x00FF&(temp_CUR>>8);
jobuuu 5:a4319f79457b 251 int temp_PWM = (int)(V_out);
jobuuu 2:a1c0a37df760 252 msg.data[2]=0x00FF&temp_PWM;
jobuuu 2:a1c0a37df760 253 msg.data[3]=0x00FF&(temp_PWM>>8);
jobuuu 2:a1c0a37df760 254 can.write(msg);
jobuuu 2:a1c0a37df760 255 }
jobuuu 1:e04e563be5ce 256
jobuuu 2:a1c0a37df760 257 // if((CNT_TMR4%4000)==0){
jobuuu 2:a1c0a37df760 258 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 259 // }
jobuuu 1:e04e563be5ce 260
jobuuu 1:e04e563be5ce 261 /*******************************************************
jobuuu 1:e04e563be5ce 262 *** Timer Counting & etc.
jobuuu 1:e04e563be5ce 263 ********************************************************/
jobuuu 2:a1c0a37df760 264 CNT_TMR4++;
jobuuu 1:e04e563be5ce 265 }
jobuuu 1:e04e563be5ce 266 TIM4->SR = 0x0; // reset the status register
GiJeongKim 0:51c43836c1d7 267 }