Sungwoo Kim
for learning
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
--- 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;
}
--- 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
--- 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);
--- 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));
// }