Sungwoo Kim
/
HydraulicControlBoard_Learning
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 256:7c8cc8b56b88, committed 2021-03-03
- Comitter:
- Lightvalve
- Date:
- Wed Mar 03 12:23:13 2021 +0000
- Parent:
- 255:aa424a9ca332
- Commit message:
- 210303
Changed in this revision
diff -r aa424a9ca332 -r 7c8cc8b56b88 CAN/function_CAN.cpp
--- a/CAN/function_CAN.cpp Mon Feb 22 14:09:13 2021 +0000
+++ b/CAN/function_CAN.cpp Wed Mar 03 12:23:13 2021 +0000
@@ -137,12 +137,6 @@
case CRX_SET_DATA_REQUEST: {
int request_type = msg.data[2];
flag_data_request[request_type] = msg.data[1];
- //pc.printf("can middle %d\n", request_type);
-
-// if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
- //if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3;
-// if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
- //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
// Position, Velocity, and Torque (ID:1200)
if (flag_data_request[0] == HIGH) {
@@ -154,8 +148,7 @@
}
} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
if (SENSING_MODE == 0) {
-// CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
- CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (pos.ref*0.001f));
+ CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
} else if (SENSING_MODE == 1) {
CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
}
@@ -173,8 +166,8 @@
double t_value_ref = 0.0f;
if (CURRENT_CONTROL_MODE) {
- t_value = cur.sen;
- t_value_ref = I_REF_fil;
+ t_value = cur.sen * 1000.0f; //Pulse
+ t_value_ref = I_REF_fil * 1000.0f; //Pulse
} else {
if(value>=(float) VALVE_CENTER) {
t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
@@ -189,8 +182,7 @@
}
}
-// CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
- CAN_TX_PRES((int16_t) (VALVE_DEADZONE_PLUS), (int16_t) (t_value_ref)); // 1400
+ CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400
}
// //If it doesn't rest, below can can not work.
@@ -851,7 +843,7 @@
pos.ref = (double)temp_pos * 1.0f;
vel.ref = (double)temp_vel * 10.0f;
} else { //Linear Actuator
- pos.ref = (double)temp_pos * 10.0f;
+ pos.ref = (double)temp_pos * 16.0f;
vel.ref = (double)temp_vel * 256.0f;
}
diff -r aa424a9ca332 -r 7c8cc8b56b88 SPI_EEP_ENC/SPI_EEP_ENC.cpp
--- a/SPI_EEP_ENC/SPI_EEP_ENC.cpp Mon Feb 22 14:09:13 2021 +0000
+++ b/SPI_EEP_ENC/SPI_EEP_ENC.cpp Wed Mar 03 12:23:13 2021 +0000
@@ -82,7 +82,7 @@
// ENCODER
void spi_enc_set_clear(void){
unsigned int temp;
- enc_cs = 1; //sw add
+// enc_cs = 1; //sw add
enc_cs = 0;
temp = enc.write(0b00100000);
enc_cs = 1;
@@ -92,14 +92,14 @@
unsigned int temp, i, temp1, temp2;
// write MDR0 -> 0b11 -> x4 quadrature count mode
- enc_cs = 1; //sw add
+// enc_cs = 1; //sw add
enc_cs = 0;
temp = enc.write(0b10001000); // WR + MDR0
temp = enc.write(0b00000011); // quadratue mode
enc_cs = 1;
// write MDR1 -> 0b10 -> 2-byte counter mode
- for(i=0;i<100;i++);
+ for(i=0;i<10000;i++);
enc_cs = 0;
temp = enc.write(0b10010000); // WR + MDR1
//temp = enc.write(0b00000010); // 2 byte mode
@@ -114,7 +114,7 @@
int spi_enc_read(void){
//for(t_i=0;t_i<100;t_i++);
unsigned int t_dummy, t_b1, t_b2, t_b3, t_b4, t_i;
- enc_cs = 1; //sw add
+// enc_cs = 1; //sw add
enc_cs = 0;
t_dummy = enc.write(0b01100000); // Read Commend
t_b1 = enc.write(0x00); // Dummy data for clock
diff -r aa424a9ca332 -r 7c8cc8b56b88 function_utilities/function_utilities.cpp
--- a/function_utilities/function_utilities.cpp Mon Feb 22 14:09:13 2021 +0000
+++ b/function_utilities/function_utilities.cpp Wed Mar 03 12:23:13 2021 +0000
@@ -625,8 +625,8 @@
//DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
- K_SPRING = spi_eeprom_read(RID_K_SPRING);
- D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
+// K_SPRING = spi_eeprom_read(RID_K_SPRING);
+// D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
diff -r aa424a9ca332 -r 7c8cc8b56b88 main.cpp
--- a/main.cpp Mon Feb 22 14:09:13 2021 +0000
+++ b/main.cpp Wed Mar 03 12:23:13 2021 +0000
@@ -891,12 +891,17 @@
enc_cs = 1; //sw add
enc.format(8,0);
- enc.frequency(10000000); //10M
+ enc.frequency(5000000); //10M
enc_cs = 0; //sw add
make_delay();
-
-// spi_eeprom_write(RID_BNO, (int16_t) 0);
+
+ // spi _ enc
+ spi_enc_set_init();
+ make_delay();
+
+ //Inital BNO setting
+// spi_eeprom_write(RID_BNO, (int16_t) 1);
// make_delay();
//rom
@@ -911,6 +916,14 @@
Init_PWM();
TIM4->CR1 ^= TIM_CR1_UDIS;
make_delay();
+
+ // CAN
+ can.attach(&CAN_RX_HANDLER);
+ CAN_ID_INIT();
+ make_delay();
+
+ //can.reset();
+ can.filter(msg.id, 0xFFFFF000, CANStandard);
// TMR3 init
Init_TMR3();
@@ -922,22 +935,24 @@
// TIM2->CR1 ^= TIM_CR1_UDIS;
// make_delay();
- // CAN
- can.attach(&CAN_RX_HANDLER);
- CAN_ID_INIT();
- make_delay();
+// // CAN
+// can.attach(&CAN_RX_HANDLER);
+// CAN_ID_INIT();
+// make_delay();
+
+
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
//NVIC_SetPriority(TIM2_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 3);
- //can.reset();
- can.filter(msg.id, 0xFFFFF000, CANStandard);
-
- // spi _ enc
- spi_enc_set_init();
- make_delay();
+// //can.reset();
+// can.filter(msg.id, 0xFFFFF000, CANStandard);
+
+ // // spi _ enc
+// spi_enc_set_init();
+// make_delay();
//DAC init
if (SENSING_MODE == 0) {
@@ -2380,16 +2395,15 @@
double torq_ref = 0.0f;
pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
- vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
+ vel.err = (vel.ref - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
//K & D Low Pass Filter
float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz
K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D);
D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D);
-
-// torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N]
- torq_ref = torq.ref;
+ torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N]
+// torq_ref = torq.ref;
// torque feedback
torq.err = torq_ref - torq.sen; //[N]
@@ -2839,7 +2853,7 @@
// }
// } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
// if (SENSING_MODE == 0) {
-// CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
+// CAN_TX_POSITION_FT((int16_t) (pos.sen/16.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
// } else if (SENSING_MODE == 1) {
// CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
// }