Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Rainbow_v1_2_copy

2011

Dependencies:   mbed FastPWM

main.cpp@2:a1c0a37df760, 2019-08-20 (annotated)

Committer:
jobuuu
Date:
Tue Aug 20 07:38:55 2019 +0000
Revision:
2:a1c0a37df760
Parent:
1:e04e563be5ce
Child:
5:a4319f79457b
HydraulicControlBoard_190820

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 2:a1c0a37df760 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
GiJeongKim 0:51c43836c1d7 40
GiJeongKim 0:51c43836c1d7 41
jobuuu 2:a1c0a37df760 42 double a1;
jobuuu 2:a1c0a37df760 43 double PWM_out;
jobuuu 2:a1c0a37df760 44 double temp_cur=0.0;
GiJeongKim 0:51c43836c1d7 45 int main()
GiJeongKim 0:51c43836c1d7 46 {
jobuuu 1:e04e563be5ce 47 /*******************************************************
jobuuu 1:e04e563be5ce 48 *** Initialization
jobuuu 1:e04e563be5ce 49 ********************************************************/
jobuuu 1:e04e563be5ce 50
jobuuu 2:a1c0a37df760 51 indi_led = 0;
jobuuu 1:e04e563be5ce 52 pc.baud(9600);
jobuuu 2:a1c0a37df760 53
jobuuu 2:a1c0a37df760 54 //Timer t;
jobuuu 2:a1c0a37df760 55 //t.start();
jobuuu 2:a1c0a37df760 56 //t.stop();
jobuuu 2:a1c0a37df760 57 //pc.printf("The time taken was %f seconds\n",t.read());
jobuuu 2:a1c0a37df760 58
GiJeongKim 0:51c43836c1d7 59 // i2c init
GiJeongKim 0:51c43836c1d7 60 // i2c.frequency(400 * 1000); // 0.4 mHz
GiJeongKim 0:51c43836c1d7 61 // wait_ms(2); // Power Up wait
GiJeongKim 0:51c43836c1d7 62 // look_for_hardware_i2c(); // Hardware present
GiJeongKim 0:51c43836c1d7 63 // init_as5510(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 64
GiJeongKim 0:51c43836c1d7 65 // // spi init
GiJeongKim 0:51c43836c1d7 66 eeprom.format(8,3);
GiJeongKim 0:51c43836c1d7 67 eeprom.frequency(5000000); //5M
GiJeongKim 0:51c43836c1d7 68 enc.format(8,0);
GiJeongKim 0:51c43836c1d7 69 enc.frequency(5000000); //5M
jobuuu 2:a1c0a37df760 70
GiJeongKim 0:51c43836c1d7 71 // ADC init
jobuuu 2:a1c0a37df760 72 // Init_ADC();
jobuuu 2:a1c0a37df760 73
GiJeongKim 0:51c43836c1d7 74 // Pwm init
GiJeongKim 0:51c43836c1d7 75 Init_PWM();
GiJeongKim 0:51c43836c1d7 76 TIM4->CR1 ^= TIM_CR1_UDIS;
jobuuu 2:a1c0a37df760 77
GiJeongKim 0:51c43836c1d7 78 // //SPI
GiJeongKim 0:51c43836c1d7 79 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 80 // spi_eeprom_write(0x1,0x112);
GiJeongKim 0:51c43836c1d7 81 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 82 // int i = spi_eeprom_read(0x1);
jobuuu 2:a1c0a37df760 83
GiJeongKim 0:51c43836c1d7 84 // CAN
jobuuu 2:a1c0a37df760 85 can.attach(&CAN_RX_HANDLER);
jobuuu 2:a1c0a37df760 86
GiJeongKim 0:51c43836c1d7 87 // spi _ enc
GiJeongKim 0:51c43836c1d7 88 spi_enc_set_init();
jobuuu 1:e04e563be5ce 89
jobuuu 1:e04e563be5ce 90
jobuuu 1:e04e563be5ce 91 /*******************************************************
jobuuu 1:e04e563be5ce 92 *** Program is operating!
jobuuu 1:e04e563be5ce 93 ********************************************************/
GiJeongKim 0:51c43836c1d7 94 while(1) {
GiJeongKim 0:51c43836c1d7 95 // dac_1=0.5;
GiJeongKim 0:51c43836c1d7 96 // dac_2=0.1;
GiJeongKim 0:51c43836c1d7 97 check_2 = 1;
GiJeongKim 0:51c43836c1d7 98 //spi _ eeprom
GiJeongKim 0:51c43836c1d7 99 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 100 // spi_eeprom_write(0x0001,0xFFFFFFFF);
GiJeongKim 0:51c43836c1d7 101 // spi_eeprom_ready();
GiJeongKim 0:51c43836c1d7 102 // int a=spi_eeprom_read(0x0001);
GiJeongKim 0:51c43836c1d7 103 //spi _ enc
GiJeongKim 0:51c43836c1d7 104 int a = spi_enc_read();
GiJeongKim 0:51c43836c1d7 105 // read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 106
GiJeongKim 0:51c43836c1d7 107 check_2=0;
jobuuu 2:a1c0a37df760 108
jobuuu 2:a1c0a37df760 109 // pc.printf("%f\n",PWM_out);
jobuuu 2:a1c0a37df760 110 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 111
jobuuu 2:a1c0a37df760 112 // wait(0.01f);
GiJeongKim 0:51c43836c1d7 113 }
jobuuu 1:e04e563be5ce 114 }
jobuuu 1:e04e563be5ce 115
jobuuu 2:a1c0a37df760 116 /*******************************************************************************
jobuuu 2:a1c0a37df760 117 TIMER INTERRUPT
jobuuu 2:a1c0a37df760 118 *******************************************************************************/
jobuuu 2:a1c0a37df760 119
jobuuu 2:a1c0a37df760 120 unsigned long CNT_TMR4 = 0;
jobuuu 2:a1c0a37df760 121 double FREQ_TMR4 = (double)FREQ_20k;
jobuuu 1:e04e563be5ce 122 extern "C" void TIM4_IRQHandler(void)
jobuuu 1:e04e563be5ce 123 {
jobuuu 2:a1c0a37df760 124 if ( TIM4->SR & TIM_SR_UIF ) {
jobuuu 1:e04e563be5ce 125 /*******************************************************
jobuuu 1:e04e563be5ce 126 *** Sensor Read & Data Handling
jobuuu 1:e04e563be5ce 127 ********************************************************/
jobuuu 1:e04e563be5ce 128
jobuuu 2:a1c0a37df760 129 if((CNT_TMR4%2)==0){
jobuuu 2:a1c0a37df760 130 //spi
jobuuu 2:a1c0a37df760 131 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 132 // eeprom.write(0xff);
jobuuu 2:a1c0a37df760 133 // ready();
jobuuu 2:a1c0a37df760 134 // read(1);
jobuuu 2:a1c0a37df760 135
jobuuu 2:a1c0a37df760 136 //i2c
jobuuu 2:a1c0a37df760 137 //// read_field(i2c_slave_addr1);
jobuuu 2:a1c0a37df760 138
jobuuu 2:a1c0a37df760 139 //ADC
jobuuu 2:a1c0a37df760 140 // ADC1->CR2 |= 0x40000000; // adc _ 12bit
jobuuu 2:a1c0a37df760 141 // a1=ADC1->DR;
jobuuu 2:a1c0a37df760 142 // a1=ADC2->DR;
jobuuu 2:a1c0a37df760 143 // int raw_cur = ADC3->DR;
jobuuu 2:a1c0a37df760 144 double raw_cur=(double)adc3*4096.0;
jobuuu 2:a1c0a37df760 145
jobuuu 2:a1c0a37df760 146 double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*100.0)); // f_cutoff : 500Hz
jobuuu 2:a1c0a37df760 147 double new_a1 = (double)raw_cur;
jobuuu 2:a1c0a37df760 148 a1=a1*(1.0-alpha_update_cur)+new_a1*(alpha_update_cur);
jobuuu 2:a1c0a37df760 149 // a1=new_a1;
jobuuu 2:a1c0a37df760 150 }
jobuuu 1:e04e563be5ce 151
jobuuu 1:e04e563be5ce 152 //DAC
jobuuu 1:e04e563be5ce 153 // dac_1 = ADC1->DR;
jobuuu 1:e04e563be5ce 154 // dac_2 = ADC2->DR;
jobuuu 1:e04e563be5ce 155
jobuuu 1:e04e563be5ce 156 /*******************************************************
jobuuu 1:e04e563be5ce 157 *** Valve Control
jobuuu 1:e04e563be5ce 158 ********************************************************/
jobuuu 1:e04e563be5ce 159
jobuuu 2:a1c0a37df760 160 // PWM_out : -1.0~1.0(full duty)
jobuuu 2:a1c0a37df760 161 PWM_out = -0.0104;
jobuuu 2:a1c0a37df760 162 double t = (double)CNT_TMR4/FREQ_TMR4;
jobuuu 2:a1c0a37df760 163 double T = 10.0;
jobuuu 2:a1c0a37df760 164 // PWM_out = 0.36*sin(2.0*PI*t/T);
jobuuu 2:a1c0a37df760 165 PWM_out = 0.04*sin(2.0*PI*t/T);
jobuuu 2:a1c0a37df760 166 // if(PWM_out > 0.0) PWM_out = 0.02;
jobuuu 2:a1c0a37df760 167 // else if(PWM_out < 0.0) PWM_out = -0.02;
jobuuu 2:a1c0a37df760 168
jobuuu 2:a1c0a37df760 169 // Saturation of output voltage to 5.0V
jobuuu 2:a1c0a37df760 170 if(PWM_out > (5.0/12.0)) PWM_out=(5.0/12.0);
jobuuu 2:a1c0a37df760 171 else if (PWM_out < (-5.0/12.0)) PWM_out=(-5.0/12.0);
jobuuu 2:a1c0a37df760 172
jobuuu 2:a1c0a37df760 173 if (PWM_out>0.0) {
jobuuu 2:a1c0a37df760 174 dtc_v=0.0;
jobuuu 1:e04e563be5ce 175 dtc_w=PWM_out;
jobuuu 1:e04e563be5ce 176 } else {
jobuuu 2:a1c0a37df760 177 dtc_v=-PWM_out;
jobuuu 2:a1c0a37df760 178 dtc_w=0.0;
jobuuu 1:e04e563be5ce 179 }
jobuuu 1:e04e563be5ce 180
jobuuu 1:e04e563be5ce 181 //pwm
jobuuu 2:a1c0a37df760 182 TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu 2:a1c0a37df760 183 TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
jobuuu 1:e04e563be5ce 184
jobuuu 1:e04e563be5ce 185 /*******************************************************
jobuuu 2:a1c0a37df760 186 *** Data Send (CAN) & Print out (UART)
jobuuu 1:e04e563be5ce 187 ********************************************************/
jobuuu 2:a1c0a37df760 188 if((CNT_TMR4%40)==0){
jobuuu 2:a1c0a37df760 189 msg.id = 50;
jobuuu 2:a1c0a37df760 190 msg.len = 4;
jobuuu 2:a1c0a37df760 191 int temp_CUR = (int)(a1);
jobuuu 2:a1c0a37df760 192 msg.data[0]=0x00FF&temp_CUR;
jobuuu 2:a1c0a37df760 193 msg.data[1]=0x00FF&(temp_CUR>>8);
jobuuu 2:a1c0a37df760 194 int temp_PWM = (int)(PWM_out*10000.0);
jobuuu 2:a1c0a37df760 195 msg.data[2]=0x00FF&temp_PWM;
jobuuu 2:a1c0a37df760 196 msg.data[3]=0x00FF&(temp_PWM>>8);
jobuuu 2:a1c0a37df760 197 can.write(msg);
jobuuu 2:a1c0a37df760 198 }
jobuuu 1:e04e563be5ce 199
jobuuu 2:a1c0a37df760 200 // if((CNT_TMR4%4000)==0){
jobuuu 2:a1c0a37df760 201 // pc.printf("%d\n",a1);
jobuuu 2:a1c0a37df760 202 // }
jobuuu 1:e04e563be5ce 203
jobuuu 1:e04e563be5ce 204 /*******************************************************
jobuuu 1:e04e563be5ce 205 *** Timer Counting & etc.
jobuuu 1:e04e563be5ce 206 ********************************************************/
jobuuu 2:a1c0a37df760 207 CNT_TMR4++;
jobuuu 1:e04e563be5ce 208 }
jobuuu 1:e04e563be5ce 209 TIM4->SR = 0x0; // reset the status register
GiJeongKim 0:51c43836c1d7 210 }