Kim GiJeong
eeprom_test
Diff: main.cpp
- Revision:
- 17:1865016ca2e7
- Parent:
- 16:903b5a4433b4
- Child:
- 18:b8adf1582ea3
--- a/main.cpp Mon Sep 02 13:32:33 2019 +0000
+++ b/main.cpp Tue Sep 03 11:59:55 2019 +0000
@@ -67,6 +67,9 @@
double PWM_out=0.0;
+int timer_while = 0;
+int while_index = 0;
+
// =============================================================================
// =============================================================================
@@ -127,8 +130,6 @@
/*********************************
*** Initialization
*********************************/
- pc.printf("a00000");
-
LED = 1;
pc.baud(9600);
@@ -192,14 +193,19 @@
*** Program is operating!
*************************************/
while(1) {
-
- //spi _ enc
- //int a = spi_enc_read();
- //pc.printf("Message received: %d\n", msg.data[0]);
- //pc.printf("Message received: %d\n", 13);
- //i2c
- //read_field(i2c_slave_addr1);
- //wait(1);
+ if(timer_while==1){
+ //i2c
+ read_field(i2c_slave_addr1);
+ if(DIR_VALVE_ENC < 0) value = 1023 - value;
+
+ if(LED==1) {
+ LED=0;
+ } else
+ LED = 1;
+ timer_while = 0;
+
+ }
+ timer_while ++;
}
}
@@ -304,8 +310,8 @@
*******************************************************************************/
unsigned long CNT_TMR4 = 0;
-double FREQ_TMR4 = (double)FREQ_20k;
-double DT_TMR4 = (double)DT_20k;
+double FREQ_TMR4 = (double)FREQ_10k;
+double DT_TMR4 = (double)DT_10k;
extern "C" void TIM4_IRQHandler(void)
{
if ( TIM4->SR & TIM_SR_UIF ) {
@@ -313,39 +319,42 @@
*** Sensor Read & Data Handling
********************************************************/
- if((CNT_TMR4%2)==0) {
- //Using LoadCell
+
+ //Using LoadCell
// ADC1->CR2 |= 0x40000000; // adc _ 12bit
// //while((ADC1->SR & 0b10));
// double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
// double torque_new = ((double)ADC1->DR - PRES_A_NULL) / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0;
// torq.sen = torq.sen*(1.0-alpha_update_torque)+torque_new*(alpha_update_torque);
-
- //Pressure sensor A
- ADC1->CR2 |= 0x40000000; // adc _ 12bit
- //while((ADC1->SR & 0b10));
- double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
- double pres_A_new = ((double)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.0;
- pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
- //Pressure sensor 1B
- ADC2->CR2 |= 0x40000000; // adc _ 12bit
- //while((ADC2->SR & 0b10));
- double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
- double pres_B_new = ((double)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.0;
- pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
- torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
-
-/*
- //Current
- ADC3->CR2 |= 0x40000000; // adc _ 12bit
+
+ //Pressure sensor A
+ ADC1->CR2 |= 0x40000000; // adc _ 12bit
+ //while((ADC1->SR & 0b10));
+ double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+ double pres_A_new = ((double)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
+ pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+
+ //Pressure sensor 1B
+ //ADC2->CR2 |= 0x40000000; // adc _ 12bit
+ //while((ADC2->SR & 0b10));
+ double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+ double pres_B_new = ((double)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR;
+ pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+ torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
+
+
+
+ //Current
+ //ADC3->CR2 |= 0x40000000; // adc _ 12bit
// a1=ADC2->DR;
-// int raw_cur = ADC3->DR;
- while((ADC3->SR & 0b10));
- double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
- double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
- cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
-*/
- }
+ int raw_cur = ADC3->DR;
+ //while((ADC3->SR & 0b10));
+ double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
+ double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
+ cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
+ cur.sen = raw_cur;
+
+
/*******************************************************
*** Timer Counting & etc.
@@ -494,6 +503,7 @@
*/
// CONTROL LOOP ------------------------------------------------------------
+
switch (CONTROL_MODE) {
case MODE_NO_ACT: {
V_out = 0;
@@ -887,7 +897,7 @@
//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
- pc.printf("%f\n", TORQUE_VREF);
+ //pc.printf("%f\n", TORQUE_VREF);
dac_1 = TORQUE_VREF / 3.3;
}
@@ -1187,7 +1197,7 @@
dac_1 = PRES_A_VREF / 3.3;
dac_2 = PRES_B_VREF / 3.3;
- pc.printf("nulling end");
+ //pc.printf("nulling end");
}
TMR3_COUNT_PRES_NULL++;
break;
@@ -1228,13 +1238,13 @@
case MODE_ROTARY_FRICTION_TUNING: {
if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05);
- VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001);
- if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL;
- else VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
+ V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001);
+ if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL;
+ else V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL;
TMR3_COUNT_ROTARY_FRIC_TUNE++;
if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
- VALVE_PWM_RAW = 0;
+ V_out = 0;
CONTROL_MODE = MODE_NO_ACT;
}
break;
@@ -1244,9 +1254,9 @@
VALVE_ID_timer = VALVE_ID_timer + 1;
if(VALVE_ID_timer < TMR_FREQ_5k*1) {
- VALVE_PWM_RAW = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100);
+ V_out = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100);
} else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
- VALVE_PWM_RAW = 1000*(ID_index_array[ID_index]);
+ V_out = 1000*(ID_index_array[ID_index]);
} else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
VALVE_POS_TMP = 0;
data_num = 0;
@@ -1259,13 +1269,13 @@
ID_index= ID_index +1;
}
- if(ID_index>=18) {
+ if(ID_index>=25) {
int i;
VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
- for(i=0; i<18; i++) {
+ for(i=0; i<25; i++) {
VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
- ROM_RESET_DATA();
+ //ROM_RESET_DATA();
//spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i]));
if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
@@ -1685,7 +1695,7 @@
else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138;
else V_out = CUR_PWM_DZ;
} else {
- CUR_PWM = V_out;
+ V_out = V_out;
}
//VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
@@ -1699,16 +1709,16 @@
if (flag_data_request[0] == HIGH) {
//position+velocity
- CAN_TX_POSITION((int32_t) PRES_A_VREF, (int32_t) PRES_B_VREF);
+ CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
//pc.printf("can good");
- // CAN_TX_POSITION((int) (PRES_A_VREF * 100.), (int) (PRES_B_VREF * 100.));
// CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
}
if (flag_data_request[1] == HIGH) {
//torque
//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
- CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
+ //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
+ CAN_TX_TORQUE((int16_t) (cur.sen));
// CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
}
@@ -1765,14 +1775,20 @@
// if(V_out > 0.0) V_out = 1000.0;
// else if(V_out < 0.0) V_out = -1000.0;
+ //V_out = 2000.0 * sin(2 * 3.14159 * (double) CNT_TMR3 / (double) 5000.0);
+
/*******************************************************
*** PWM
********************************************************/
PWM_out= V_out/12000.0; // Full duty : 12000.0mV
// Saturation of output voltage to 5.0V
- if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
- else if (PWM_out < -0.41667) PWM_out=-0.41667;
+// if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
+// else if (PWM_out < -0.41667) PWM_out=-0.41667;
+
+ // Saturation of output voltage to 12.0V
+ if(PWM_out > 1.0) PWM_out=1.0;
+ else if (PWM_out < -1.0) PWM_out=-1.0;
if (PWM_out>0.0) {
dtc_v=0.0;
@@ -1802,16 +1818,16 @@
// }
if((CNT_TMR3%5000)==0) {
- if(LED==1) {
- LED=0;
- } else
- LED = 1;
+// if(LED==1) {
+// LED=0;
+// } else
+// LED = 1;
// LED != LED;
// pc.printf("A %f\n", (double) pres_A.sen);
// pc.printf("A %f\n", PRES_SENSOR_A_PULSE_PER_BAR);
// pc.printf("B %f\n", (double) pres_B.sen);
// pc.printf("B %f\n", PRES_SENSOR_B_PULSE_PER_BAR);
- pc.printf("preAVref %d\n", (int) PRES_A_VREF);
+// pc.printf("preAVref %d\n", (int) PRES_A_VREF);
}
/*******************************************************
@@ -1819,7 +1835,9 @@
********************************************************/
CNT_TMR3++;
}
+
TIM3->SR = 0x0; // reset the status register
+
}
/*