Kim GiJeong
eeprom_test
Diff: main.cpp
- Revision:
- 21:e5f1a43ea6f9
- Parent:
- 20:806196fda269
- Child:
- 23:59218d4a256d
diff -r 806196fda269 -r e5f1a43ea6f9 main.cpp
--- a/main.cpp Mon Sep 09 06:57:31 2019 +0000
+++ b/main.cpp Mon Sep 09 10:10:31 2019 +0000
@@ -121,7 +121,7 @@
MODE_TEST_CURRENT_CONTROL, //9
MODE_TEST_PWM_CONTROL, //10
-
+
MODE_CURRENT_CONTROL, //11
MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12
MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13
@@ -147,7 +147,7 @@
*********************************/
//LED = 1;
//pc.baud(9600);
-
+
// i2c init
i2c.frequency(400 * 1000); // 0.4 mHz
wait_ms(2); // Power Up wait
@@ -161,7 +161,7 @@
enc.format(8,0);
enc.frequency(5000000); //5M
make_delay();
-
+
//rom
ROM_CALL_DATA();
make_delay();
@@ -179,18 +179,18 @@
Init_TMR3();
TIM3->CR1 ^= TIM_CR1_UDIS;
make_delay();
-
+
// TMR5 init
Init_TMR5();
TIM5->CR1 ^= TIM_CR1_UDIS;
make_delay();
-
+
// CAN
can.attach(&CAN_RX_HANDLER);
CAN_ID_INIT();
make_delay();
can.filter(msg.id, 0xFFFFF000, CANStandard);
-
+
// spi _ enc
spi_enc_set_init();
make_delay();
@@ -211,17 +211,17 @@
*** Program is operating!
*************************************/
while(1) {
- if(timer_while==1000){
+ if(timer_while==1000) {
//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 ++;
}
@@ -282,8 +282,6 @@
}
}
- //VALVE_PWM_RAW_FF = 0.0;
-
V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
}
@@ -337,13 +335,13 @@
extern "C" void TIM4_IRQHandler(void)
{
if (TIM4->SR & TIM_SR_UIF ) {
-
- /*******************************************************
- *** Sensor Read & Data Handling
- ********************************************************/
+
+ /*******************************************************
+ *** Sensor Read & Data Handling
+ ********************************************************/
- //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));
@@ -352,38 +350,38 @@
- //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 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;
+ //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
+ //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);
- cur.sen = raw_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.
- ********************************************************/
- //CNT_TMR4++;
- }
+ /*******************************************************
+ *** Timer Counting & etc.
+ ********************************************************/
+ //CNT_TMR4++;
+ }
TIM4->SR = 0x0; // reset the status register
}
@@ -393,7 +391,7 @@
double FREQ_TMR3 = (double)FREQ_5k;
double DT_TMR3 = (double)DT_5k;
extern "C" void TIM3_IRQHandler(void)
-{
+{
if (TIM3->SR & TIM_SR_UIF ) {
ENC_UPDATE();
@@ -1593,70 +1591,70 @@
//VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
//PWM_out = CUR_PWM;
-/*
+ /*
- //CAN ----------------------------------------------------------------------
- //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
- if (TMR3_COUNT_CAN_TX % 1000 == 0) {
+ //CAN ----------------------------------------------------------------------
+ //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+ if (TMR3_COUNT_CAN_TX % 1000 == 0) {
- if (flag_data_request[0] == HIGH) {
- //position+velocity
- CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
- //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
- //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
- //pc.printf("can good");
- // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
- }
+ if (flag_data_request[0] == HIGH) {
+ //position+velocity
+ CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
+ //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
+ //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
+ //pc.printf("can good");
+ // 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.sen));
- //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
- // CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
- }
+ 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.sen));
+ //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
+ // CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
+ }
- if (flag_data_request[2] == HIGH) {
- //pressure A and B
- //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
- CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
- // CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
- // CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
- // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
+ if (flag_data_request[2] == HIGH) {
+ //pressure A and B
+ //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+ CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+ // CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
+ // CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
+ // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
- }
+ }
- if (flag_data_request[3] == HIGH) {
- //PWM
- CAN_TX_PWM((int16_t) CUR_PWM);
- //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
- // CAN_TX_PWM((int16_t) cnt_vel_findhome);
- // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
- // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
+ if (flag_data_request[3] == HIGH) {
+ //PWM
+ CAN_TX_PWM((int16_t) CUR_PWM);
+ //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
+ // CAN_TX_PWM((int16_t) cnt_vel_findhome);
+ // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
+ // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
- }
+ }
- if (flag_data_request[4] == HIGH) {
- //valve position
- CAN_TX_VALVE_POSITION((int16_t) (value));
- //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
- //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
+ if (flag_data_request[4] == HIGH) {
+ //valve position
+ CAN_TX_VALVE_POSITION((int16_t) (value));
+ //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
+ //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
- //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
- // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));
- // CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
- // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
- // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
- }
+ //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
+ // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));
+ // CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
+ // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
+ // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
+ }
- TMR3_COUNT_CAN_TX = 0;
+ TMR3_COUNT_CAN_TX = 0;
- }
- TMR3_COUNT_CAN_TX++;
+ }
+ TMR3_COUNT_CAN_TX++;
-*/
+ */
/*******************************************************
@@ -1727,10 +1725,10 @@
*** Timer Counting & etc.
********************************************************/
//CNT_TMR3++;
-
+
}
TIM3->SR = 0x0; // reset the status register
-
+
}
@@ -1739,9 +1737,9 @@
//double FREQ_TMR5 = (double)FREQ_500;
//double DT_TMR5 = (double)DT_500;
extern "C" void TIM5_IRQHandler(void)
-{
+{
if (TIM5->SR & TIM_SR_UIF ) {
-
+
//CAN ----------------------------------------------------------------------
if (flag_data_request[0] == HIGH) {
//position+velocity
@@ -1832,4 +1830,4 @@
V_out = -V_MAX;
cur.err_int = cur.err_int - V_rem*DT_5k;
}
-}
\ No newline at end of file
+}