Skip to content

Commit c3932c8

Browse files
jg2562jg2562
committed
Auto formatted every file
1 parent da81c5d commit c3932c8

19 files changed

+502
-500
lines changed

‎Auto_KF.h

Lines changed: 21 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@ void Auto_KF(Leg* leg) {
1313
// Serial.println(leg->Input);
1414
leg->Torque_Sum_90P += leg->Input; // Sum torques
1515
leg->count_err ++; // Increment counter
16-
// Serial.println(leg->Torque_Sum_90P);
17-
// Serial.println(leg->count_err);
16+
// Serial.println(leg->Torque_Sum_90P);
17+
// Serial.println(leg->count_err);
1818
leg->auto_KF_update = true; // Raise auto KF flag
1919
//Serial.println("INSIDE");
2020
} else {
@@ -23,47 +23,47 @@ void Auto_KF(Leg* leg) {
2323
}// end if state=3
2424
//
2525

26-
26+
2727
if (leg->state == 1 && leg->auto_KF_update) {
2828
//
2929
// // Here we update the KF and then we disable the conditions that activate the if statement
3030
//
3131
leg->Mean_Torque_90P = leg->Torque_Sum_90P / leg->count_err;
3232

3333

34-
// Serial.print(" Ref ");
35-
// Serial.print(leg->Setpoint_Ankle * leg->coef_in_3_steps);
36-
// Serial.print(" , 90% Setpoint Mean Torque");
37-
// Serial.print(leg->Mean_Torque_90P);
38-
// Serial.println(" ");
34+
// Serial.print(" Ref ");
35+
// Serial.print(leg->Setpoint_Ankle * leg->coef_in_3_steps);
36+
// Serial.print(" , 90% Setpoint Mean Torque");
37+
// Serial.print(leg->Mean_Torque_90P);
38+
// Serial.println(" ");
3939
if (leg->Mean_Torque_90P * leg->Setpoint_Ankle * leg->coef_in_3_steps <= 0) {
4040
// if the sign are not concord, no auto update of KF
41-
// Serial.println("Signals not concord");
41+
// Serial.println("Signals not concord");
4242
leg->auto_KF_update = false; //added after test of 11/7/18
4343
return;
4444
}
4545

4646
if ( abs((leg->Mean_Torque_90P - (leg->Setpoint_Ankle * leg->coef_in_3_steps)) / (leg->Setpoint_Ankle * leg->coef_in_3_steps)) < 0.01) {
47-
// Serial.println("Stop Auto KF");
47+
// Serial.println("Stop Auto KF");
4848
leg->auto_KF_update = false; //added after test of 11/7/18
4949
return;
5050
}// if the error is less than 1% no need to change KF
5151

5252
if (leg->coef_in_3_steps > 0) {
53-
leg->ERR = leg-> ERR + (leg->Mean_Torque_90P - (leg->Setpoint_Ankle*leg->coef_in_3_steps))/(leg->Setpoint_Ankle*leg->coef_in_3_steps); //Calculate a running sum of the relative error
53+
leg->ERR = leg-> ERR + (leg->Mean_Torque_90P - (leg->Setpoint_Ankle * leg->coef_in_3_steps)) / (leg->Setpoint_Ankle * leg->coef_in_3_steps); //Calculate a running sum of the relative error
5454
leg->KF = leg->KF - ((leg->Mean_Torque_90P - (leg->Setpoint_Ankle * leg->coef_in_3_steps)) / (leg->Setpoint_Ankle * leg->coef_in_3_steps) * 0.6 + leg->ERR * 0.01); //changed from 0.4 to 0.6 after test of 11/7/18
5555
}
5656

5757
if isnan(leg->KF) {
58-
leg->KF=1;
58+
leg->KF = 1;
5959
}
60-
61-
// Serial.print("Setpoint Ankle");
62-
// Serial.print(leg->Setpoint_Ankle);
63-
// Serial.print("coeff");
64-
// Serial.print(leg->coef_in_3_steps);
65-
// Serial.print("Desired leg->KF ");
66-
// Serial.println(leg->KF);
60+
61+
// Serial.print("Setpoint Ankle");
62+
// Serial.print(leg->Setpoint_Ankle);
63+
// Serial.print("coeff");
64+
// Serial.print(leg->coef_in_3_steps);
65+
// Serial.print("Desired leg->KF ");
66+
// Serial.println(leg->KF);
6767

6868
if (leg->KF >= leg->max_KF) {
6969
leg->KF = leg->max_KF;
@@ -76,8 +76,8 @@ void Auto_KF(Leg* leg) {
7676
leg->Mean_Torque_90P = 0;
7777
leg->Torque_Sum_90P = 0;
7878
leg->count_err = 0;
79-
// Serial.print("Actual leg->KF ");
80-
// Serial.println(leg->KF);
79+
// Serial.print("Actual leg->KF ");
80+
// Serial.println(leg->KF);
8181
leg->auto_KF_update = false; // to be able to do this cycle just once every step, i.e. during the whole state 1 I have to execute this cycle just once
8282
}// end of if state=1
8383

‎Balance_Ctrl.ino

Lines changed: 30 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -17,19 +17,19 @@ void Steady_Balance_Baseline() {
1717

1818
count_steady_baseline++;
1919
}
20-
20+
2121
} else {
2222

2323
left_leg->FSR_Toe_Steady_Balance_Baseline /= count_steady_baseline;
2424
left_leg->FSR_Heel_Steady_Balance_Baseline /= count_steady_baseline;
2525
right_leg->FSR_Toe_Steady_Balance_Baseline /= count_steady_baseline;
2626
right_leg->FSR_Heel_Steady_Balance_Baseline /= count_steady_baseline;
27-
28-
// Serial.println("Steady Balance Baseline");
29-
// Serial.println(left_leg->FSR_Toe_Steady_Balance_Baseline);
30-
// Serial.println(left_leg->FSR_Heel_Steady_Balance_Baseline);
31-
// Serial.println(right_leg->FSR_Toe_Steady_Balance_Baseline);
32-
// Serial.println(right_leg->FSR_Heel_Steady_Balance_Baseline);
27+
28+
// Serial.println("Steady Balance Baseline");
29+
// Serial.println(left_leg->FSR_Toe_Steady_Balance_Baseline);
30+
// Serial.println(left_leg->FSR_Heel_Steady_Balance_Baseline);
31+
// Serial.println(right_leg->FSR_Toe_Steady_Balance_Baseline);
32+
// Serial.println(right_leg->FSR_Heel_Steady_Balance_Baseline);
3333
FLAG_STEADY_BALANCE_BASELINE = 0;
3434

3535
}
@@ -66,11 +66,11 @@ void Balance_Baseline() {
6666
// left_leg->FSR_Heel_Balance_Baseline /= count_balance;
6767
// right_leg->FSR_Toe_Balance_Baseline /= count_balance;
6868
// right_leg->FSR_Heel_Balance_Baseline /= count_balance;
69-
// Serial.println("Dynamic Balance Baseline");
70-
// Serial.println(left_leg->FSR_Toe_Balance_Baseline);
71-
// Serial.println(left_leg->FSR_Heel_Balance_Baseline);
72-
// Serial.println(right_leg->FSR_Toe_Balance_Baseline);
73-
// Serial.println(right_leg->FSR_Heel_Balance_Baseline);
69+
// Serial.println("Dynamic Balance Baseline");
70+
// Serial.println(left_leg->FSR_Toe_Balance_Baseline);
71+
// Serial.println(left_leg->FSR_Heel_Balance_Baseline);
72+
// Serial.println(right_leg->FSR_Toe_Balance_Baseline);
73+
// Serial.println(right_leg->FSR_Heel_Balance_Baseline);
7474
FLAG_BALANCE_BASELINE = 0;
7575

7676
}
@@ -81,15 +81,15 @@ void Balance_Baseline() {
8181
//-----------------------------------------------------------
8282
double Balance_Torque_ref(Leg * leg) {
8383
// first balance control which is linear to the current level of force
84-
// Serial.print("[ ");
85-
// Serial.print(leg->FSR_Toe_Average);
86-
// Serial.print(", ");
87-
// Serial.print(leg->FSR_Toe_Balance_Baseline);
88-
// Serial.print("] ");
89-
// Serial.print(" ");
90-
// Serial.print((leg->FSR_Toe_Average / leg->FSR_Toe_Balance_Baseline) - (leg->FSR_Heel_Average / leg->FSR_Heel_Balance_Baseline));
91-
// Serial.print(" -> ");
92-
// Serial.println(min(1, (leg->FSR_Toe_Average / leg->FSR_Toe_Balance_Baseline)) - min(1, (leg->FSR_Heel_Average / leg->FSR_Heel_Balance_Baseline)));
84+
// Serial.print("[ ");
85+
// Serial.print(leg->FSR_Toe_Average);
86+
// Serial.print(", ");
87+
// Serial.print(leg->FSR_Toe_Balance_Baseline);
88+
// Serial.print("] ");
89+
// Serial.print(" ");
90+
// Serial.print((leg->FSR_Toe_Average / leg->FSR_Toe_Balance_Baseline) - (leg->FSR_Heel_Average / leg->FSR_Heel_Balance_Baseline));
91+
// Serial.print(" -> ");
92+
// Serial.println(min(1, (leg->FSR_Toe_Average / leg->FSR_Toe_Balance_Baseline)) - min(1, (leg->FSR_Heel_Average / leg->FSR_Heel_Balance_Baseline)));
9393

9494
return (min(1, (leg->FSR_Toe_Average / leg->FSR_Toe_Balance_Baseline)) - min(1, (leg->FSR_Heel_Average / leg->FSR_Heel_Balance_Baseline))) * (leg->Prop_Gain) * (leg->Setpoint_Ankle);
9595

@@ -100,7 +100,7 @@ double Balance_Torque_ref(Leg * leg) {
100100

101101
double Balance_Torque_ref_based_on_Steady(Leg * leg) {
102102
// balance control which depends on the steady and dynamic baseline.
103-
103+
104104
// Serial.print("[ ");
105105
// Serial.print(leg->FSR_Toe_Average);
106106
// Serial.print(", ");
@@ -111,22 +111,22 @@ double Balance_Torque_ref_based_on_Steady(Leg * leg) {
111111
// Serial.print(" -> ");
112112
// Serial.println(min(1, (leg->FSR_Toe_Average - leg->FSR_Toe_Steady_Balance_Baseline) / ( leg->FSR_Toe_Balance_Baseline - FSR_Toe_Steady_Balance_Baseline)) - min(1, (leg->FSR_Heel_Average / leg->FSR_Heel_Balance_Baseline)));
113113

114-
// Steady_multiplier Dynamic_multiplier to increase or decrease the sentitivitiness of the control to the baseline, it works as a gain that allows to engage the max torque earlier or later.
115-
// in other words change the reference dimension of the estimated convex hull
114+
// Steady_multiplier Dynamic_multiplier to increase or decrease the sentitivitiness of the control to the baseline, it works as a gain that allows to engage the max torque earlier or later.
115+
// in other words change the reference dimension of the estimated convex hull
116+
117+
leg->COP_Toe_ratio = (leg->FSR_Toe_Average - leg->FSR_Toe_Steady_Balance_Baseline * leg->Steady_multiplier) / ( leg->FSR_Toe_Balance_Baseline * leg->Dynamic_multiplier - leg->FSR_Toe_Steady_Balance_Baseline * leg->Steady_multiplier);
118+
leg->COP_Heel_ratio = (leg->FSR_Heel_Average - leg->FSR_Heel_Steady_Balance_Baseline * leg->Steady_multiplier) / ( leg->FSR_Heel_Balance_Baseline * leg->Dynamic_multiplier - leg->FSR_Heel_Steady_Balance_Baseline * leg->Steady_multiplier);
119+
116120

117-
leg->COP_Toe_ratio = (leg->FSR_Toe_Average - leg->FSR_Toe_Steady_Balance_Baseline*leg->Steady_multiplier) / ( leg->FSR_Toe_Balance_Baseline*leg->Dynamic_multiplier - leg->FSR_Toe_Steady_Balance_Baseline*leg->Steady_multiplier);
118-
leg->COP_Heel_ratio = (leg->FSR_Heel_Average - leg->FSR_Heel_Steady_Balance_Baseline*leg->Steady_multiplier) / ( leg->FSR_Heel_Balance_Baseline*leg->Dynamic_multiplier - leg->FSR_Heel_Steady_Balance_Baseline*leg->Steady_multiplier);
119-
121+
leg->COP_Foot_ratio = min(1, max(0, leg->COP_Toe_ratio)) - min(1, max(0, leg->COP_Heel_ratio));
120122

121-
leg->COP_Foot_ratio = min(1, max(0, leg->COP_Toe_ratio)) - min(1, max(0, leg->COP_Heel_ratio));
122-
123123
return (leg->COP_Foot_ratio) * (leg->Prop_Gain) * (leg->Setpoint_Ankle);
124124

125125
}
126126

127127
//-------------------------------------------
128128
double Balance_Torque_COP_ref(Leg* leg) {
129-
// balance control based onf the real COP
129+
// balance control based onf the real COP
130130
if (leg->state == 3) {
131131
leg->COP = (leg->FSR_Toe_Average * leg->Toe_Pos + leg->FSR_Heel_Average * leg->Heel_Pos) / (leg->FSR_Heel_Average + leg->FSR_Toe_Average);
132132
}

‎BioFeedback_functions.ino

Lines changed: 28 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -3,16 +3,16 @@ void takeHeelStrikeAngle(Leg* leg) {
33

44
leg->Heel_Strike += (pot(leg->Potentiometer_pin) + leg->Biofeedback_bias); //Take baseline knee angle data
55
leg->Heel_Strike_Count++;
6-
// right_leg->Heel_Strike_Count++;
6+
// right_leg->Heel_Strike_Count++;
77
Serial.print("Heel strike sum and n of steps: ");
88
Serial.print(leg->Heel_Strike);
99
Serial.print(" , ");
10-
// Serial.println(leg->Heel_Strike_Count);
10+
// Serial.println(leg->Heel_Strike_Count);
1111

1212
if (leg->BioFeedback_Baseline_flag == true) {
13-
//if (right_leg->BioFeedback_Baseline_flag == true) {
14-
// Heel_strike_update_and_biofeedback(leg);
15-
Heel_strike_update_and_biofeedback(right_leg);
13+
//if (right_leg->BioFeedback_Baseline_flag == true) {
14+
// Heel_strike_update_and_biofeedback(leg);
15+
Heel_strike_update_and_biofeedback(right_leg);
1616
}
1717
return;
1818
}
@@ -22,12 +22,12 @@ void BioFeedback_Baseline(Leg* leg) {
2222
// calculate the baseline
2323

2424
if (leg->BioFeedback_Baseline_flag == false) {
25-
// if (right_leg->Heel_Strike_Count >= leg->n_step_biofeedback_base)
25+
// if (right_leg->Heel_Strike_Count >= leg->n_step_biofeedback_base)
2626
if (leg->Heel_Strike_Count >= leg->n_step_biofeedback_base)
2727
{
2828
Serial.print("UPDATING BASELINE for the BioFeedback : ");
29-
// Serial.println(leg->Heel_Strike);
30-
// Serial.println(leg->Heel_Strike_Count);
29+
// Serial.println(leg->Heel_Strike);
30+
// Serial.println(leg->Heel_Strike_Count);
3131
leg->Heel_Strike_baseline = leg->Heel_Strike / leg->Heel_Strike_Count;
3232
leg->BioFeedback_Baseline_flag = true;
3333
leg->BIO_BASELINE_FLAG = false;
@@ -44,35 +44,35 @@ void BioFeedback_Baseline(Leg* leg) {
4444
void Heel_strike_update_and_biofeedback(Leg * leg) {
4545
// update the heel strike mean and apply the biofeedback
4646
Serial.println("Inside Heel strike");
47-
48-
if (leg->Heel_Strike_Count >= leg->n_step_biofeedback){
49-
// if (right_leg->Heel_Strike_Count >= leg->n_step_biofeedback){
47+
48+
if (leg->Heel_Strike_Count >= leg->n_step_biofeedback) {
49+
// if (right_leg->Heel_Strike_Count >= leg->n_step_biofeedback){
5050
Serial.println("Inside first if Heel strike");
5151
leg->Heel_Strike_mean = leg->Heel_Strike / leg->Heel_Strike_Count;
52-
// leg->Heel_Strike_mean = leg->Heel_Strike / right_leg->Heel_Strike_Count;
52+
// leg->Heel_Strike_mean = leg->Heel_Strike / right_leg->Heel_Strike_Count;
5353
Serial.print("Heel_Strike_mean = ");
5454
Serial.print(leg->Heel_Strike_mean);
5555
Serial.print(" ; Difference between mean and desired : ");
5656
Serial.println((leg->Heel_Strike_mean - leg->BioFeedback_desired));
5757

58-
if (leg->Heel_Strike_mean >= leg->BioFeedback_desired){ //added
59-
leg->NO_Biofeedback = true;
60-
Serial.println("NO Biofeedback true");
61-
} else {
62-
leg->NO_Biofeedback = false;
63-
Serial.println("NO Biofeedback false");
64-
Serial.println("Inside second if Heel strike");
65-
leg->Frequency =map((leg->BioFeedback_desired - leg->Heel_Strike_mean),100,0,200,1000);//added
66-
if (leg->Frequency>=1000){
67-
leg->Frequency=1000;
68-
} else if (leg->Frequency<=200){
69-
leg->Frequency=200;
70-
}
71-
Serial.print("Updating Freq for the BioFeedback : ");
72-
Serial.println(leg->Frequency);
58+
if (leg->Heel_Strike_mean >= leg->BioFeedback_desired) { //added
59+
leg->NO_Biofeedback = true;
60+
Serial.println("NO Biofeedback true");
61+
} else {
62+
leg->NO_Biofeedback = false;
63+
Serial.println("NO Biofeedback false");
64+
Serial.println("Inside second if Heel strike");
65+
leg->Frequency = map((leg->BioFeedback_desired - leg->Heel_Strike_mean), 100, 0, 200, 1000); //added
66+
if (leg->Frequency >= 1000) {
67+
leg->Frequency = 1000;
68+
} else if (leg->Frequency <= 200) {
69+
leg->Frequency = 200;
70+
}
71+
Serial.print("Updating Freq for the BioFeedback : ");
72+
Serial.println(leg->Frequency);
7373
}
7474
leg->Heel_Strike_Count = 0;
75-
// right_leg->Heel_Strike_Count = 0;
75+
// right_leg->Heel_Strike_Count = 0;
7676
leg->Heel_Strike = 0;
7777
}
7878
return;

‎Calibrate_and_Read_Sensors.ino

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@ void torque_calibration()
1616

1717
left_leg->torque_calibration_value = left_leg->torque_calibration_value / torq_cal_count; // Averages torque over a second
1818
right_leg->torque_calibration_value = right_leg->torque_calibration_value / torq_cal_count; // Averages torque over a second
19-
// Serial.println(left_leg->torque_calibration_value);
20-
// Serial.println(right_leg->torque_calibration_value);
19+
// Serial.println(left_leg->torque_calibration_value);
20+
// Serial.println(right_leg->torque_calibration_value);
2121
}
2222

2323

@@ -30,7 +30,7 @@ void FSR_calibration()
3030
FSR_FIRST_Cycle = 0;
3131

3232
startTime = millis();
33-
// Serial.println("First time");
33+
// Serial.println("First time");
3434
right_leg->Curr_Combined = 0;
3535
left_leg->Curr_Combined = 0;
3636

@@ -97,18 +97,18 @@ void FSR_calibration()
9797

9898
FSR_FIRST_Cycle = 1;
9999
FSR_CAL_FLAG = 0;
100-
// if (FLAG_TWO_TOE_SENSORS) {
101-
// Serial.println(left_leg->fsr_Combined_peak_ref);
102-
// Serial.println(right_leg->fsr_Combined_peak_ref);
103-
// Serial.println(" ");
104-
// } else {
105-
// Serial.println(left_leg->fsr_Toe_peak_ref);
106-
// Serial.println(left_leg->fsr_Heel_peak_ref);
107-
// Serial.println(" ");
108-
// Serial.println(right_leg->fsr_Toe_peak_ref);
109-
// Serial.println(right_leg->fsr_Heel_peak_ref);
110-
// Serial.println(" ");
111-
// }
100+
// if (FLAG_TWO_TOE_SENSORS) {
101+
// Serial.println(left_leg->fsr_Combined_peak_ref);
102+
// Serial.println(right_leg->fsr_Combined_peak_ref);
103+
// Serial.println(" ");
104+
// } else {
105+
// Serial.println(left_leg->fsr_Toe_peak_ref);
106+
// Serial.println(left_leg->fsr_Heel_peak_ref);
107+
// Serial.println(" ");
108+
// Serial.println(right_leg->fsr_Toe_peak_ref);
109+
// Serial.println(right_leg->fsr_Heel_peak_ref);
110+
// Serial.println(" ");
111+
// }
112112
}
113113
}
114114

‎Check_LED_BT.ino

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,9 @@ double Check_LED_BT(const unsigned int LED_BT_PIN_l, double LED_BT_Voltage_l, in
1212
*(p_count_l) = 0;
1313

1414
if ( LED_BT_Voltage_l <= 2 && flag_done_once_bt == false) {
15-
// Serial.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! DISCONNECTED! ");
16-
// Serial.print("VOLTAGE : ");
17-
// Serial.println(LED_BT_Voltage_l);
15+
// Serial.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! DISCONNECTED! ");
16+
// Serial.print("VOLTAGE : ");
17+
// Serial.println(LED_BT_Voltage_l);
1818

1919
bluetooth.flush();
2020
flag_done_once_bt = true;
@@ -23,9 +23,9 @@ double Check_LED_BT(const unsigned int LED_BT_PIN_l, double LED_BT_Voltage_l, in
2323

2424
if (LED_BT_Voltage_l > 2.5 && flag_done_once_bt == true) {
2525
flag_done_once_bt = false;
26-
// Serial.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECONNECTED! ");
27-
// Serial.print("VOLTAGE : ");
28-
// Serial.println(LED_BT_Voltage_l);
26+
// Serial.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECONNECTED! ");
27+
// Serial.print("VOLTAGE : ");
28+
// Serial.println(LED_BT_Voltage_l);
2929
}
3030
}
3131

0 commit comments

Comments
 (0)