Sungwoo Kim
LIGHT2
Diff: main.cpp
- Revision:
- 12:6f2531038ea4
- Parent:
- 11:82d8768d7351
- Child:
- 13:747daba9cf59
diff -r 82d8768d7351 -r 6f2531038ea4 main.cpp
--- a/main.cpp Fri Aug 23 12:28:08 2019 +0000
+++ b/main.cpp Tue Aug 27 05:32:44 2019 +0000
@@ -56,10 +56,68 @@
double PWM_out=0.0;
int ID_index_array[100] = {0};
+
+double CUR_PRES_DIFF_BAR = 0;
+
+double CUR_PRES_A_sum = 0.0;
+double CUR_PRES_B_sum = 0.0;
+double CUR_PRES_A_mean = 0.0;
+double CUR_PRES_B_mean = 0.0;
+double CUR_TORQUE_sum = 0.0;
+double CUR_TORQUE_mean = 0.0;
+double PRES_A_NULL = 200.0;
+double PRES_B_NULL = 200.0;
+double TORQUE_NULL = 3900;
// =============================================================================
// =============================================================================
// =============================================================================
+/*******************************************************************************
+ * REFERENCE MODE
+ ******************************************************************************/
+enum _REFERENCE_MODE{
+ MODE_REF_NO_ACT = 0, //0
+ MODE_REF_DIRECT, //1
+ MODE_REF_COS_INC, //2
+ MODE_REF_LINE_INC, //3
+ MODE_REF_SIN_WAVE, //4
+ MODE_REF_SQUARE_WAVE, //5
+};
+
+/*******************************************************************************
+ * CONTROL MODE
+ ******************************************************************************/
+enum _CONTROL_MODE{
+ //control mode
+ MODE_NO_ACT = 0, //0
+ MODE_VALVE_OPEN_LOOP, //1
+ MODE_VALVE_POSITION_CONTROL, //2
+
+ MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
+ MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
+ MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
+
+ MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
+ MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
+ MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
+
+ MODE_TEST_CURRENT_CONTROL, //9
+ MODE_TEST_PWM_CONTROL, //10
+
+ //utility
+ MODE_TORQUE_SENSOR_NULLING = 20, //20
+ MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
+ MODE_FIND_HOME, //22
+ MODE_VALVE_GAIN_SETTING, //23
+ MODE_PRESSURE_SENSOR_NULLING, //24
+ MODE_PRESSURE_SENSOR_CALIB, //25
+ MODE_ROTARY_FRICTION_TUNING, //26
+
+ MODE_DDV_POS_VS_PWM_ID = 30, //30
+ MODE_DDV_DEADZONE_AND_CENTER, //31
+ MODE_DDV_POS_VS_FLOWRATE, //32
+};
+
int main()
{
/*********************************
@@ -188,6 +246,146 @@
{
if ( TIM3->SR & TIM_SR_UIF ) {
+ ENC_UPDATE();
+ CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.;
+ CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.;
+ //CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; //
+ CUR_TORQUE_NM = CUR_TORQUE; //
+ CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR);
+
+ // Reference Loop
+ switch (REFERENCE_MODE) {
+ case MODE_REF_NO_ACT:
+ {
+ break;
+ }
+
+ case MODE_REF_DIRECT:
+ {
+ if (FLAG_REFERENCE_VALVE_PWM) {
+ Ref_PWM = (double) REF_PWM;
+ }
+ if (FLAG_REFERENCE_VALVE_POSITION) {
+ Ref_Valve_Pos = (double) REF_VALVE_POSITION;
+ }
+ if (FLAG_REFERENCE_JOINT_POSITION) {
+ Ref_Joint_Pos = (double) REF_POSITION;
+ Ref_Joint_Vel = (double) REF_VELOCITY;
+ }
+ if (FLAG_REFERENCE_JOINT_TORQUE) {
+ Ref_Joint_Torq = (double) REF_TORQUE;
+ }
+ break;
+ }
+
+ case MODE_REF_COS_INC:
+ {
+ if (FLAG_REFERENCE_VALVE_PWM) {
+ Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_PWM;
+ }
+ if (FLAG_REFERENCE_VALVE_POSITION) {
+ Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_VALVE_POS;
+ }
+ if (FLAG_REFERENCE_JOINT_POSITION) {
+ Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_POS;
+ Ref_Joint_Vel = 0.0;
+ }
+ if (FLAG_REFERENCE_JOINT_TORQUE) {
+ Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_TORQUE;
+ }
+ TMR2_COUNT_REFERENCE++;
+ if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
+ REFERENCE_MODE = MODE_REF_DIRECT;
+ TMR2_COUNT_REFERENCE = 0;
+ }
+ break;
+ }
+
+
+ case MODE_REF_LINE_INC:
+ {
+ if (FLAG_REFERENCE_VALVE_PWM) {
+ Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_PWM;
+ }
+ if (FLAG_REFERENCE_VALVE_POSITION) {
+ Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_VALVE_POS;
+ }
+ if (FLAG_REFERENCE_JOINT_POSITION) {
+ Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS;
+ Ref_Vel_Test = ((double) REF_POSITION - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k; // pulse/sec
+ //Ref_Vel_Test = 10;
+ }
+ if (FLAG_REFERENCE_JOINT_TORQUE) {
+ Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_TORQUE;
+ }
+ TMR2_COUNT_REFERENCE++;
+ if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) {
+ //REFERENCE_MODE = MODE_REF_DIRECT;
+ Ref_Vel_Test = 1;
+ TMR2_COUNT_REFERENCE = REF_MOVE_TIME_5k;
+ //TMR2_COUNT_REFERENCE = 0;
+ }
+ break;
+ }
+
+ case MODE_REF_SIN_WAVE:
+ {
+ if (FLAG_REFERENCE_VALVE_PWM) {
+ Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
+ }
+ if (FLAG_REFERENCE_VALVE_POSITION) {
+ Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
+ }
+ if (FLAG_REFERENCE_JOINT_POSITION) {
+ Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
+ }
+ if (FLAG_REFERENCE_JOINT_TORQUE) {
+ Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
+ }
+ TMR2_COUNT_REFERENCE++;
+ if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
+ REFERENCE_MODE = MODE_REF_DIRECT;
+ TMR2_COUNT_REFERENCE = 0;
+ }
+ break;
+ }
+
+ case MODE_REF_SQUARE_WAVE:
+ {
+ if (FLAG_REFERENCE_VALVE_PWM) {
+ Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM;
+ if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM;
+ else Ref_PWM = -REF_MAG + REF_PWM;
+ }
+ if (FLAG_REFERENCE_VALVE_POSITION) {
+ Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION;
+ if (Ref_Valve_Pos >= REF_VALVE_POSITION) Ref_Valve_Pos = REF_MAG + REF_VALVE_POSITION;
+ else Ref_Valve_Pos = -REF_MAG + REF_VALVE_POSITION;
+ }
+ if (FLAG_REFERENCE_JOINT_POSITION) {
+ Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION;
+ if (Ref_Joint_Pos >= REF_POSITION) Ref_Valve_Pos = REF_MAG + REF_POSITION;
+ else Ref_Joint_Pos = -REF_MAG + REF_POSITION;
+ }
+ if (FLAG_REFERENCE_JOINT_TORQUE) {
+ Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE;
+ if (Ref_Joint_Torq >= REF_TORQUE) Ref_Valve_Pos = REF_MAG + REF_TORQUE;
+ else Ref_Joint_Torq = -REF_MAG + REF_TORQUE;
+ }
+ TMR2_COUNT_REFERENCE++;
+ if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) {
+ REFERENCE_MODE = MODE_REF_DIRECT;
+ TMR2_COUNT_REFERENCE = 0;
+ }
+ break;
+ }
+
+ default:
+ break;
+ }
+
+
+
/*******************************************************
*** Valve Control
********************************************************/
@@ -253,52 +451,6 @@
TIM3->SR = 0x0; // reset the status register
}
-/*******************************************************************************
- * REFERENCE MODE
- ******************************************************************************/
-enum _REFERENCE_MODE{
- MODE_REF_NO_ACT = 0, //0
- MODE_REF_DIRECT, //1
- MODE_REF_COS_INC, //2
- MODE_REF_LINE_INC, //3
- MODE_REF_SIN_WAVE, //4
- MODE_REF_SQUARE_WAVE, //5
-};
-
-/*******************************************************************************
- * CONTROL MODE
- ******************************************************************************/
-enum _CONTROL_MODE{
- //control mode
- MODE_NO_ACT = 0, //0
- MODE_VALVE_OPEN_LOOP, //1
- MODE_VALVE_POSITION_CONTROL, //2
-
- MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
- MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
- MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
-
- MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
- MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
- MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
-
- MODE_TEST_CURRENT_CONTROL, //9
- MODE_TEST_PWM_CONTROL, //10
-
- //utility
- MODE_TORQUE_SENSOR_NULLING = 20, //20
- MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
- MODE_FIND_HOME, //22
- MODE_VALVE_GAIN_SETTING, //23
- MODE_PRESSURE_SENSOR_NULLING, //24
- MODE_PRESSURE_SENSOR_CALIB, //25
- MODE_ROTARY_FRICTION_TUNING, //26
-
- MODE_DDV_POS_VS_PWM_ID = 30, //30
- MODE_DDV_DEADZONE_AND_CENTER, //31
- MODE_DDV_POS_VS_FLOWRATE, //32
-};
-
void ValveControl(unsigned int ControlMode)
{
switch (ControlMode) {