Kim GiJeong
/
HydraulicControlBoard_LIGHT_GJ
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 17:1865016ca2e7, committed 2019-09-03
- Comitter:
- Lightvalve
- Date:
- Tue Sep 03 11:59:55 2019 +0000
- Parent:
- 16:903b5a4433b4
- Child:
- 18:b8adf1582ea3
- Commit message:
- 190903
Changed in this revision
--- a/CAN/function_CAN.cpp Mon Sep 02 13:32:33 2019 +0000
+++ b/CAN/function_CAN.cpp Tue Sep 03 11:59:55 2019 +0000
@@ -651,7 +651,7 @@
if(address==CID_RX_CMD){
unsigned int CMD = msg.data[0];
ReadCMD(CMD);
- pc.printf("cmd %d\n ",CMD);
+ //pc.printf("cmd %d\n ",CMD);
} else if(address==CID_RX_REF_POSITION) {
int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
--- a/I2C_AS5510/I2C_AS5510.cpp Mon Sep 02 13:32:33 2019 +0000
+++ b/I2C_AS5510/I2C_AS5510.cpp Tue Sep 03 11:59:55 2019 +0000
@@ -3,18 +3,18 @@
void look_for_hardware_i2c()
{
- pc.printf("\r\n\n\n");
- pc.printf("Note I2C address 0x1C used by FXOS8700CQ 3-axis accelerometer and 3-axis magetometer\r\n");
- pc.printf("Start hardware search..... \r\n");
+ //pc.printf("\r\n\n\n");
+ //pc.printf("Note I2C address 0x1C used by FXOS8700CQ 3-axis accelerometer and 3-axis magetometer\r\n");
+ //pc.printf("Start hardware search..... \r\n");
int count = 0;
for (int address=12; address<256; address+=2) {
if (!i2c.write(address, NULL, 0)) { // 0 returned is OK
- pc.printf(" - I2C device found at address 0x%02X\n\r", address >>1);
+ //pc.printf(" - I2C device found at address 0x%02X\n\r", address >>1);
count++;
}
}
- pc.printf("%d devices found \n\r", count);
+ //pc.printf("%d devices found \n\r", count);
}
void init_as5510(int i2c_address)
@@ -23,8 +23,8 @@
char idata[2];
int result=0;
- pc.printf("\r\n");
- pc.printf("Start AS5510 init.. \r\n");
+ //pc.printf("\r\n");
+ //pc.printf("Start AS5510 init.. \r\n");
i2c_adrs= (i2c_address << 1); // AS5510 Slave address lsb= 0 for write
@@ -37,15 +37,15 @@
idata[0]=0x0B; // Register for Sensitivity
idata[1]=0x00; // Byte
result= i2c.write(i2c_adrs, idata, 2, 0); // Now write_sensitivity
- if (result != 0) pc.printf("No ACK bit! (09)\n\r");
+// if (result != 0) pc.printf("No ACK bit! (09)\n\r");
//----------- Operation mode selection------------------------
idata[0]=0x02; // 0x02 address setup register for operation, speed, polarity
idata[1]=0x04; // Normal Operation, Slow mode (1), NORMAL Polarity (0), Power Up (0)
result= i2c.write(i2c_adrs, idata, 2, 0); // Now write_operation
- if (result != 0) pc.printf("No ACK bit! (11)\n\r");
+// if (result != 0) pc.printf("No ACK bit! (11)\n\r");
- pc.printf("AS5510 init done\r\n");
+ //pc.printf("AS5510 init done\r\n");
}
@@ -59,7 +59,7 @@
// First, now Write pointer to register 0x00----------------------------
adrss= (i2c_address << 1); // AS5510 Slave address lsb= 0 for write
oresult= i2c.write(adrss, 0x00, 1, 0); // write one byte
- if (oresult != 0) pc.printf("No ACK bit! (33)\n\r");
+ if (oresult != 0) //pc.printf("No ACK bit! (33)\n\r");
// Second, now Read register 0x00 and 0x01--------------------------------
memset(off_data, 0, sizeof(off_data));
@@ -96,4 +96,5 @@
msb= rx_data[1]&0x03; // need only 2 low bits og MSB
value = ((msb & 0x03)<<8) + lsb;
// pc.printf("I2C adres= 0x%02X, Magnetic Field => msb= 0x%02X, lsb= 0x%02X, decimal 10-bit value = %u \r\n ", i2c_address, rx_data[0],rx_data[1], value);
+
}
\ No newline at end of file
--- a/function_utilities/function_utilities.cpp Mon Sep 02 13:32:33 2019 +0000
+++ b/function_utilities/function_utilities.cpp Tue Sep 03 11:59:55 2019 +0000
@@ -21,12 +21,12 @@
uint8_t SETTING_SWITCH_OLD = 0;
uint8_t REFERENCE_MODE = 0;
uint16_t CAN_FREQ = 500;
-uint8_t DIR_JOINT_ENC = 0;
-uint8_t DIR_VALVE = 0;
-uint8_t DIR_VALVE_ENC = 0;
+int16_t DIR_JOINT_ENC = 0;
+int16_t DIR_VALVE = 0;
+int16_t DIR_VALVE_ENC = 0;
double SUPPLY_VOLTAGE = 12.0;
-double VALVE_VOLTAGE_LIMIT = 5.0;
+double VALVE_VOLTAGE_LIMIT = 12.0;
double P_GAIN_VALVE_POSITION = 0.0;
double I_GAIN_VALVE_POSITION= 0.0;
@@ -148,7 +148,7 @@
int VALVE_INPUT_PWM;
double VALVE_GAIN_LPM_PER_V[10];
-double VALVE_POS_VS_PWM[18];
+double VALVE_POS_VS_PWM[25];
long JOINT_VEL[100];
int VALVE_MAX_POS;
@@ -196,8 +196,8 @@
double CUR_PRES_B_mean = 0.0;
double CUR_TORQUE_sum = 0.0;
double CUR_TORQUE_mean = 0.0;
-double PRES_A_NULL = 10.0;
-double PRES_B_NULL = 10.0;
+double PRES_A_NULL = 1.0;
+double PRES_B_NULL = 1.0;
double TORQUE_NULL = 3900;
double Ref_Valve_Pos_Old = 0.0;
@@ -456,7 +456,7 @@
VALVE_GAIN_LPM_PER_V[5] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01;
VALVE_GAIN_LPM_PER_V[7] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01;
VALVE_GAIN_LPM_PER_V[9] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01;
- for(int i=0; i<18; i++)
+ for(int i=0; i<25; i++)
{
VALVE_POS_VS_PWM[i] = (double) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
}
--- 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
+
}
/*
--- a/setting.h Mon Sep 02 13:32:33 2019 +0000 +++ b/setting.h Tue Sep 03 11:59:55 2019 +0000 @@ -108,9 +108,9 @@ extern uint8_t SETTING_SWITCH_OLD; extern uint8_t REFERENCE_MODE; extern uint16_t CAN_FREQ; -extern uint8_t DIR_JOINT_ENC; -extern uint8_t DIR_VALVE; -extern uint8_t DIR_VALVE_ENC; +extern int16_t DIR_JOINT_ENC; +extern int16_t DIR_VALVE; +extern int16_t DIR_VALVE_ENC; extern double SUPPLY_VOLTAGE; extern double VALVE_VOLTAGE_LIMIT; @@ -235,7 +235,7 @@ extern int VALVE_INPUT_PWM; extern double VALVE_GAIN_LPM_PER_V[10]; -extern double VALVE_POS_VS_PWM[18]; +extern double VALVE_POS_VS_PWM[25]; extern long JOINT_VEL[100]; extern int VALVE_MAX_POS;