Revision 76:09ace7f0a0bf, committed 2015-10-22
- Comitter:
- ThomasBNL
- Date:
- Thu Oct 22 13:19:57 2015 +0000
- Parent:
- 75:9d8e665e3e6f
- Child:
- 77:ac32d64602a5
- Commit message:
- LED defined and usable
Changed in this revision
--- a/main.cpp Thu Oct 22 12:37:35 2015 +0000
+++ b/main.cpp Thu Oct 22 13:19:57 2015 +0000
@@ -81,6 +81,8 @@
DigitalIn buttonL1(PTC6) , buttonL2(PTA4) , buttonH1(D2) , buttonH2(D1); // DEBUG/CALIBRATION: 4 buttons for calibration and debugging
Ticker looptimer; // FLOW : Ticker called looptimer to set a looptimerflag that puts the volatile bool control_go to true every sample
+DigitalOut ledgreen1(D0), ledgreen2(D3), ledyellow1(D8), ledyellow2(D11), ledred1(D14), ledred2(D15);
+
volatile bool control_go = false, sample = false, sample_error, sample_error_strike; // EMG : booleans for controlling sample time of moving average and emg signal
volatile bool looptimerflag; // CONTROLLER : boolean that controls the sample time of the whole controller
@@ -106,7 +108,7 @@
double Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, // EMG ACTION: variables to witch the threshold values calculated after the calibration get asigned to
Threshold_Bicep_Right_1, Threshold_Bicep_Right_2;
-double n=0; double c=0; double k=0; double p=0; // FLOW : these flow variables are assigned to certain values through out the script values in order to regulate the flow of the script
+double n=0; double c=0; double k=0; double p=0; double a=0; // FLOW : these flow variables are assigned to certain values through out the script values in order to regulate the flow of the script
// FILTERS EMG
const double low_b0 = 0.05892937945281792 , low_b1 = 0.11785875890563584 , low_b2 = 0.05892937945281792, // Notch 1 LOW : VIA online biquad calculator Lowpass 520-48-0.7071-6
@@ -144,17 +146,16 @@
double pwm_to_motor_turn; double pwm_to_motor_strike; // TURN - STRIKE : variable that is constructed by summing the values of the P, I and D action
-// FILTER: D-action
-// const double a0 = 0.000332685098529822, a1 = 0.000665370197059644, a2 = 0.000332685098529822, b1 = -1.9625271814290315, b2 = 0.9638579218231508;
-const double a0 = 0.000021080713160785432, a1 = 0.000042161426321570865, a2 = 0.000021080713160785432, b1 = -1.990754082898736, b2 = 0.9908384057513788; //(0.75Hz)
-// const double a0 = 0.003543360146633312, a1 = 0.007086720293266624, a2 = 0.003543360146633312, b1 = -1.8704759567901301, b2 = 0.8846493973766635; //(10Hz)
-// const double a0 = 0.0009129521023626334, a1 = 0.0018259042047252668, a2 = 0.0009129521023626334, b1 = -1.9368516414202819, b2 = 0.9405034498297324; //(5Hz)
+// FILTER: D-action
+const double a0 = 0.000021080713160785432, a1 = 0.000042161426321570865, a2 = 0.000021080713160785432, b1 = -1.990754082898736, b2 = 0.9908384057513788; //(0.75Hz)
biquadFilter Lowpassfilter_derivative(b1,b2,a0,a1,a2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
const double cw=0; // MOTOR: turn direction zero is clockwise (front view)
const double ccw=1; // MOTOR: turn direction one is counterclockwise (front view)
const double off=1; // Led off
const double on=0; // Led on
+const double ledon = 1; //
+const double ledoff = 0; //
const int Fs = 512; // sampling frequency (512 Hz)
const double sample_time = 0.001953125; // duration of one sample
double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
@@ -165,7 +166,10 @@
const double change_one_bottle=45;
const double Hit=90; // position when bottle is hit
-
+
+void led12(), led23(), led34(), led45(), led56(), led61(), led1(), led2(), led3(), led4(), led5(), led6(), led_off(), led_on();
+void leds_down_up(), leds_down_up_down();
+
//============================================================================================================================
// ___________________________
// // \\
@@ -426,21 +430,30 @@
if (buttonL2.read() < 0.5)
{ motordirection_turn = cw;
- pwm_motor_turn = 0.02; }
+ pwm_motor_turn = 0.02;
+ //led56();
+ }
if (buttonL1.read() < 0.5)
{ motordirection_turn = ccw;
- pwm_motor_turn = 0.02; }
+ pwm_motor_turn = 0.02;
+ //led12();
+ }
- pwm_motor_turn = 0;
+ pwm_motor_turn = 0;
+ led_on();
if (buttonH2.read() < 0.5)
{ motordirection_strike = cw;
- pwm_motor_strike = 0.02; }
+ pwm_motor_strike = 0.02;
+ //led56();
+ }
if (buttonH1.read() < 0.5)
{ motordirection_strike = ccw;
- pwm_motor_strike = 0.02; }
+ pwm_motor_strike = 0.02;
+ //led12();
+ }
pwm_motor_strike = 0;
@@ -832,3 +845,79 @@
{
*in > min ? *in < max? : *in = max: *in = min;
}
+
+void led12() { ledgreen1 = ledon; ledgreen2 = ledon; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
+void led23() { ledgreen1 = ledoff; ledgreen2 = ledon; ledyellow1 = ledon; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
+void led34() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledon; ledyellow2 = ledon; ledred1 = ledoff; ledred2 = ledoff; }
+void led45() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledon; ledred1 = ledon; ledred2 = ledoff; }
+void led56() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledon; ledred2 = ledon; }
+void led61() { ledgreen1 = ledon; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledon; }
+
+void led_off() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
+void led1() { ledgreen1 = ledon; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
+void led2() { ledgreen1 = ledoff; ledgreen2 = ledon; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
+void led3() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledon; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }
+void led4() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledon; ledred1 = ledoff; ledred2 = ledoff; }
+void led5() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledon; ledred2 = ledoff; }
+void led6() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledon; }
+void led_on() { ledgreen1 = ledon; ledgreen2 = ledon; ledyellow1 = ledon; ledyellow2 = ledon; ledred1 = ledon; ledred2 = ledon; }
+
+void leds_down_up()
+{
+if(a==15)
+{led1();}
+
+if(a==30)
+{led2();}
+
+if(a==45)
+{led3();}
+
+if(a==60)
+{led4();}
+
+if(a==90)
+{led5();}
+
+if(a==90)
+{led6(); a=0;}
+a++;
+}
+
+void leds_down_up_down()
+{
+if(a==15)
+{led1();}
+
+if(a==30)
+{led2();}
+
+if(a==45)
+{led3();}
+
+if(a==60)
+{led4();}
+
+if(a==75)
+{led5();}
+
+if(a==90)
+{led6();}
+
+if(a==105)
+{led5();}
+
+if(a==120)
+{led4();}
+
+if(a==135)
+{led3();}
+
+if(a==150)
+{led2();}
+
+if(a==165)
+{led1(); a=0;}
+
+a++;
+}
Thomas Burgers
