Yeseong Jeong
/
HydraulicControlBoard_Start
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.
Revision 20:806196fda269, committed 2019-09-09
- Comitter:
- Lightvalve
- Date:
- Mon Sep 09 06:57:31 2019 +0000
- Parent:
- 19:23b7c1ad8683
- Child:
- 21:e5f1a43ea6f9
- Commit message:
- 190909_15_57
Changed in this revision
--- a/INIT_HW/INIT_HW.cpp Mon Sep 09 06:29:56 2019 +0000
+++ b/INIT_HW/INIT_HW.cpp Mon Sep 09 06:57:31 2019 +0000
@@ -66,11 +66,29 @@
TIM3->CR1 |= TIM_CR1_ARPE; // autoreload on,
TIM3->RCR |= 0x001; // update event once per up/down count of TIM3
TIM3->EGR |= TIM_EGR_UG;
-
- //PWM Setup
TIM3->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock
TIM3->ARR = TMR3_COUNT; // set auto reload, 5 khz
TIM3->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on.
TIM3->CR1 |= TIM_CR1_CEN; // enable TIM4
+}
+
+void Init_TMR5(){
+ RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; // enable TIM5 clock
+
+ //ISR Setup
+
+ NVIC_EnableIRQ(TIM5_IRQn); //Enable TIM5 IRQ
+
+ TIM5->DIER |= TIM_DIER_UIE; // enable update interrupt
+ TIM5->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode
+ TIM5->CR1 |= TIM_CR1_UDIS;
+ TIM5->CR1 |= TIM_CR1_ARPE; // autoreload on,
+ TIM5->RCR |= 0x001; // update event once per up/down count of TIM5
+ TIM5->EGR |= TIM_EGR_UG;
+
+ TIM5->PSC = 0x0012; // no prescaler, timer counts up in sync with the peripheral clock
+ TIM5->ARR = TMR5_COUNT; // set auto reload, 5 khz
+ TIM5->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on.
+ TIM5->CR1 |= TIM_CR1_CEN; // enable TIM5
}
\ No newline at end of file
--- a/INIT_HW/INIT_HW.h Mon Sep 09 06:29:56 2019 +0000 +++ b/INIT_HW/INIT_HW.h Mon Sep 09 06:57:31 2019 +0000 @@ -7,5 +7,6 @@ void Init_ADC(void); void Init_PWM(); void Init_TMR3(); +void Init_TMR5(); #endif
--- a/main.cpp Mon Sep 09 06:29:56 2019 +0000
+++ b/main.cpp Mon Sep 09 06:57:31 2019 +0000
@@ -180,6 +180,11 @@
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();
@@ -1588,7 +1593,7 @@
//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) {
@@ -1651,7 +1656,7 @@
}
TMR3_COUNT_CAN_TX++;
-
+*/
/*******************************************************
@@ -1728,6 +1733,74 @@
}
+
+
+//unsigned long CNT_TMR5 = 0;
+//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
+ 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[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[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);
+ }
+
+ }
+ TIM5->SR = 0x0; // reset the status register
+}
+
+
+
void CurrentControl()
{
cur.err = cur.ref - cur.sen;
--- a/setting.h Mon Sep 09 06:29:56 2019 +0000 +++ b/setting.h Mon Sep 09 06:57:31 2019 +0000 @@ -9,11 +9,14 @@ //#define PWM_ARR 0x1194 // loop 20k, pwm 10k #define PWM_ARR 0x2328 // loop 10k, pwm 5k #define TMR3_COUNT 0x4650 // loop 5k +#define TMR5_COUNT 0x2710 // loop 500hz with prescale 18 +#define FREQ_500 500.0 #define FREQ_5k 5000.0 #define FREQ_10k 10000.0 #define FREQ_20k 20000.0 #define FREQ_40k 40000.0 +#define DT_500 0.002 #define DT_5k 0.0002 #define DT_10k 0.0001 #define DT_20k 0.00005