[Ver 1.0] The code was given by Seunghoon shin, used for hydraulic quadrupedal robot. Buyoun Cho will revise the code for Post-LIGHT (the robot name is not determined yet).
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 +}