Jesse Kaiser
Er is uitleg bijgeschreven en pwm_percentage heeft een andere naam
Dependencies: Encoder HIDScope MODSERIAL mbed-dsp mbed
Fork of
Lampje_EMG_Gr6
by 
Iris van Leeuwen
Revision 29:d0dab8921e9d, committed 2014-11-03
- Comitter:
- jessekaiser
- Date:
- Mon Nov 03 15:37:48 2014 +0000
- Parent:
- 28:c33a0658605e
- Child:
- 30:7a0a3c272308
- Commit message:
- wat uitleg bijgeschreven
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Nov 01 16:19:53 2014 +0000
+++ b/main.cpp Mon Nov 03 15:37:48 2014 +0000
@@ -6,12 +6,11 @@
#include "PwmOut.h"
#define TSAMP 0.005
-#define K_P1 (3.5) //voor motor1 is het 3.5
-#define K_I1 (0.01 *TSAMP) //voor motor1 is het 0.01
-#define K_P2 (0.7)
-#define K_I2 (0.01 *TSAMP)
+#define K_P1 (3.5) //Kp waarde voor motor1, van het batje // 7.0
+#define K_I1 (0.01 *TSAMP) //0.1
+#define K_P2 (0.7) //Kp waarde voor motor2, de arm //10.0
+#define K_I2 (0.01 *TSAMP) //3.0
#define I_LIMIT 1.
-//#define PI 3.14159265
#define l_arm 0.5
#define M1_PWM PTC8
@@ -34,7 +33,7 @@
AnalogIn emg1(PTC2); //Analog input
HIDScope scope(4);
-//motor 25D
+//motor1 25D
Encoder motor1(PTD3,PTD5); //wit, geel
PwmOut pwm_motor1(M2_PWM);
DigitalOut motordir1(M2_DIR);
@@ -45,8 +44,8 @@
DigitalOut motordir2(M1_DIR);
// Motor variabelen
-float pwm1_percentage = 0;
-float pwm2_percentage = 0;
+float pwm_out1 = 0;
+float pwm_out2 = 0;
int cur_pos_motor1;
int prev_pos_motor1 = 0;
int cur_pos_motor2;
@@ -87,7 +86,7 @@
float filtered_average_bi;
float filtered_average_del;
-
+//gemiddelde EMG waarden over 250 sample stappen
void average_biceps(float filtered_biceps,float *average)
{
static float total=0;
@@ -184,14 +183,6 @@
myledred= 0;
myledblue =0;
ledticker.attach(&greenblink,.5);
- /* myled1 = 1;
- myled2 = 1;
- myled3 = 1;
- wait(0.1);
- myled1 = 0;
- myled2 = 1;
- myled3 = 1;
- wait(0.1);*/
}
void stopblinkgreen()
@@ -228,11 +219,9 @@
// Motor1 = batje
// Motor2 = arm
-void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
-// maar de locatie van de variabele.
+void clamp(float* in, float min, float max) //Clamp geeft een maximum en minimum limiet aan een functie
{
-*in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
- // *in = het getal dat staat op locatie van in --> waarde van new_pwm
+*in > min ? /*(*/*in < max? : *in = max : *in = min;
}
// PI-regelaar motor1: batje
@@ -273,326 +262,321 @@
void batje_links ()
{
speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
- if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden, nu 11.3. 2.3 is de verhouding van de tanden
- setpoint1 = (11.3*2.3*2.0*PI/360);
- }
- if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
- setpoint1 = -(11.3*2.3*2.0*PI/360);
- }
- prev_setpoint1 = setpoint1;
-}
-
-// Motor1 rechts draaien
-void batje_rechts ()
-{
- speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
- if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden
- setpoint1 = (11.3*2.3*2.0*PI/360);
+ setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; //bepalen van de setpoint
+ if(setpoint1 > (11.3*2.3*2.0*PI/360)) { //Het eerste getal geeft een aantal graden weer, dus alleen dit hoeft aangepast te worden/
+ setpoint1 = (11.3*2.3*2.0*PI/360); //Hier wordt er een grens bepaald voor de hoek.
}
if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
setpoint1 = -(11.3*2.3*2.0*PI/360);
}
- pwm_motor1.write(abs(pwm1_percentage));
- prev_setpoint1 = setpoint1;
- if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
+ if(setpoint1 <= -(11.3*2.3*2.0*PI/360)-0.1) {
staat1 = 1;
- }
-}
-
-//Motor1 na links draaien weer terug laten draaien naar beginstand
-void batje_begin_links ()
-{
- speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
- if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
- setpoint1 = (0*2.3*2.0*PI/360);
- }
- if(setpoint1 < -(0*2.3*2.0*PI/360)) {
- setpoint1 = -(0*2.3*2.0*PI/360);
- }
- prev_setpoint1 = setpoint1;
-}
-
-//Motor1 na links draaien weer terug laten draaien naar beginstand
-void batje_begin_rechts ()
-{
- speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
- if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden
- setpoint1 = (0*2.3*2.0*PI/360);
- }
- if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
- setpoint1 = -(0.0*2.3*2.0*PI/360);
- }
- prev_setpoint1 = setpoint1;
-}
-
-// Motor2 balletje op zn hoogst slaan
-void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
-{
- speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
- if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
- setpoint2 = (155.0*2.0*PI/360);
- }
- if(setpoint2 < -(155.0*2.0*PI/360)) {
- setpoint2 = -(155.0*2.0*PI/360);
- }
- prev_setpoint2 = setpoint2;
- if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
- staat2 = 1;
- }
-}
-
-// Motor2 balletje in het midden slaan
-void arm_mid ()
-{
- speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
- if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden
- setpoint2 = (155.0*2.0*PI/360);
- }
- if(setpoint2 < -(155.0*2.0*PI/360)) {
- setpoint2 = -(155.0*2.0*PI/360);
- }
- prev_setpoint2 = setpoint2;
- if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
- staat2 = 1;
+ prev_setpoint1 = setpoint1;
}
}
+// Motor1 rechts draaien
+ void batje_rechts () {
+ speed1_rad = 1.0;
+ setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+ if(setpoint1 > (11.3*2.3*2.0*PI/360)) {
+ setpoint1 = (11.3*2.3*2.0*PI/360);
+ }
+ if(setpoint1 < -(11.3*2.3*2.0*PI/360)) {
+ setpoint1 = -(11.3*2.3*2.0*PI/360);
+ }
+ prev_setpoint1 = setpoint1;
+ if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
+ staat1 = 1;
+ }
+ }
+
+//Motor1 na links draaien weer terug laten draaien naar beginstand
+ void batje_begin_links () {
+ speed1_rad = 1.0;
+ setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+ if(setpoint1 > (0*2.3*2.0*PI/360)) {
+ setpoint1 = (0*2.3*2.0*PI/360);
+ }
+ if(setpoint1 < -(0*2.3*2.0*PI/360)) {
+ setpoint1 = -(0*2.3*2.0*PI/360);
+ }
+ prev_setpoint1 = setpoint1;
+ }
+
+//Motor1 na links draaien weer terug laten draaien naar beginstand
+ void batje_begin_rechts () {
+ speed1_rad = -1.0;
+ setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+ if(setpoint1 > (0*2.3*2.0*PI/360)) {
+ setpoint1 = (0*2.3*2.0*PI/360);
+ }
+ if(setpoint1 < -(0.0*2.3*2.0*PI/360)) {
+ setpoint1 = -(0.0*2.3*2.0*PI/360);
+ }
+ prev_setpoint1 = setpoint1;
+ }
+
+// Motor2 balletje op zn hoogst slaan
+ void arm_hoog () {
+ speed2_rad = 6.0;
+ setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+ if(setpoint2 > (155.0*2.0*PI/360)) {
+ setpoint2 = (155.0*2.0*PI/360);
+ }
+ if(setpoint2 < -(155.0*2.0*PI/360)) {
+ setpoint2 = -(155.0*2.0*PI/360);
+ }
+ prev_setpoint2 = setpoint2;
+ if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+ staat2 = 1;
+ }
+ }
+
+// Motor2 balletje in het midden slaan
+ void arm_mid () {
+ speed2_rad = 4.0;
+ setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+ if(setpoint2 > (155.0*2.0*PI/360)) {
+ setpoint2 = (155.0*2.0*PI/360);
+ }
+ if(setpoint2 < -(155.0*2.0*PI/360)) {
+ setpoint2 = -(155.0*2.0*PI/360);
+ }
+ prev_setpoint2 = setpoint2;
+ if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+ staat2 = 1;
+ }
+ }
// Motor2 balletje op het laagst slaan
-void arm_laag ()
-{
- speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
- if(setpoint2 > (155*2.0*PI/360)) { //setpoint in graden
- setpoint2 = (155*2.0*PI/360);
+ void arm_laag () {
+ speed2_rad = 2.0;
+ setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+ if(setpoint2 > (155*2.0*PI/360)) {
+ setpoint2 = (155*2.0*PI/360);
+ }
+ if(setpoint2 < -(155.0*2.0*PI/360)) {
+ setpoint2 = -(155.0*2.0*PI/360);
+ }
+ prev_setpoint2 = setpoint2;
+ if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
+ staat2 = 1;
+ }
}
- if(setpoint2 < -(155.0*2.0*PI/360)) {
- setpoint2 = -(155.0*2.0*PI/360);
- }
- prev_setpoint2 = setpoint2;
- if(setpoint2 >= (155.0*2.0*PI/360)-0.1) {
- staat2 = 1;
- }
-}
// Motor2 arm terug zetten in beginstand
-void arm_begin ()
-{
- speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
- setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
- if(setpoint2 > (0.0*2.0*PI/360)) { //setpoint in graden
- setpoint2 = (0.0*2.0*PI/360);
+ void arm_begin () {
+ speed2_rad = 1.0;
+ setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
+ if(setpoint2 > (0.0*2.0*PI/360)) {
+ setpoint2 = (0.0*2.0*PI/360);
+ }
+ if(setpoint2 < -(0.0*2.0*PI/360)) {
+ setpoint2 = -(0.0*2.0*PI/360);
+ }
+ prev_setpoint2 = setpoint2;
}
- if(setpoint2 < -(0.0*2.0*PI/360)) {
- setpoint2 = -(0.0*2.0*PI/360);
- }
- prev_setpoint2 = setpoint2;
-}
// MOTOR aansturing
-void looper_motor()
-{
- //MOTOR1
- pc.printf("%d \r\n", motor1.getPosition());
- cur_pos_motor1 = motor1.getPosition();
- pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
- pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
- if (pwm1_percentage < -1.0) {
- pwm1_percentage = -1.0;
- }
- if (pwm1_percentage > 1.0) {
- pwm1_percentage = 1.0;
- }
- pwm_motor1.write(abs(pwm1_percentage));
- if(pwm1_percentage > 0) {
- motordir1 = 0;
- } else {
- motordir1 = 1;
- }
+ void looper_motor() {
+ pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition()); //Geeft de posities weer van beide motoren met een sample frequentie van 0.005
+
+ //MOTOR1
+ \
+ cur_pos_motor1 = motor1.getPosition();
+ pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //voor 1 rotatie van de motoras geldt 24(aantal cpr vd encoder)*172(gearbox ratio)=4128 counts.
+ pwm_out1 = pid1(setpoint1, pos_motor1_rad);
+ if (pwm_out1 < -1.0) { //Hier wordt de grens voor de pwm waarde ingesteld.
+ pwm_out1 = -1.0;
+ }
+ if (pwm_out1 > 1.0) {
+ pwm_out1 = 1.0;
+ }
+ pwm_motor1.write(abs(pwm_out1));
+ if(pwm_out1 > 0) {
+ motordir1 = 0;
+ } else {
+ motordir1 = 1;
+ }
+
+ //MOTOR2
+ cur_pos_motor2 = motor2.getPosition();
+ pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
+ pwm_out2 = pid2(setpoint2, pos_motor2_rad); //
+ if (pwm_out2 < -1.0) {
+ pwm_out2 = -1.0;
+ }
+ if (pwm_out2 > 1.0) {
+ pwm_out2 = 1.0;
+ }
+ pwm_motor2.write(abs(pwm_out2));
+ if(pwm_out2 > 0) {
+ motordir2 = 0;
+ } else {
+ motordir2 = 1;
+ }
+
+
+ //STATES
+
+ //Het batje draait naar opgegeven positie, doet dan een bepaalde tijd niks (wait_iterator), en draait daarna weer terug
+ if (batje_hoek == 1) {
+ if(staat1 == 0) {
+ batje_rechts();
+ wait_iterator1 = 0;
+ } else if(staat1 ==1) {
+ wait_iterator1++;
+ if(wait_iterator1 > 1200) {
+ staat1 = 2;
- //MOTOR2
- cur_pos_motor2 = motor2.getPosition();
- pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI));
- pwm2_percentage = pid2(setpoint2, pos_motor2_rad); //
- if (pwm2_percentage < -1.0) {
- pwm2_percentage = -1.0;
- }
- if (pwm2_percentage > 1.0) {
- pwm2_percentage = 1.0;
- }
- pwm_motor2.write(abs(pwm2_percentage));
- if(pwm2_percentage > 0) {
- motordir2 = 0;
- } else {
- motordir2 = 1;
+ batje_begin_rechts();
+ }
+ }
+ }
+ if (batje_hoek == 2) {
+ if(staat1 == 0) {
+ batje_links();
+ wait_iterator1 = 0;
+ } else if(staat1 ==1) {
+ wait_iterator1++;
+ if(wait_iterator1 > 1200) {
+ staat1 = 2;
+
+ batje_begin_links ();
+ }
+ }
+ }
+
+ if(arm_hoogte == 1) {
+ if(staat2 == 0) {
+ arm_laag();
+ wait_iterator2 = 0;
+ } else if(staat2 == 1) {
+ wait_iterator2++;
+ if(wait_iterator2 > 400) {
+ staat2 = 2;
+
+ arm_begin();
+ }
+ }
+ }
+ if(arm_hoogte == 2) {
+ if(staat2 == 0) {
+ arm_mid();
+ wait_iterator2 = 0;
+ } else if(staat2 == 1) {
+ wait_iterator2++;
+ if(wait_iterator2 > 400) {
+ staat2 = 2;
+
+ arm_begin();
+ }
+ }
+ }
+ if(arm_hoogte == 3) {
+ if(staat2 == 0) {
+ arm_hoog();
+ wait_iterator2 = 0;
+ } else if(staat2 == 1) {
+ wait_iterator2++;
+ if(wait_iterator2 > 400) {
+ staat2 = 2;
+
+ arm_begin();
+ }
+ }
+ }
+
}
- //STATES
-
- if (batje_hoek == 1) {
- if(staat1 == 0) {
- batje_rechts();
- wait_iterator1 = 0;
- } else if(staat1 ==1) {
- wait_iterator1++;
- if(wait_iterator1 > 400) {
- staat1 = 2;
-
- batje_begin_rechts();
- }
- }
- }
- if (batje_hoek == 2) {
- if(staat1 == 0) {
- batje_links();
- wait_iterator1 = 0;
- } else if(staat1 ==1) {
- wait_iterator1++;
- if(wait_iterator1 > 400) {
- staat1 = 2;
-
- batje_begin_links ();
- }
- }
- }
-
- if(arm_hoogte == 1) {
- if(staat2 == 0) {
- arm_laag();
- wait_iterator2 = 0;
- } else if(staat2 == 1) {
- wait_iterator2++;
- if(wait_iterator2 > 400) {
- staat2 = 2;
+// Hoofdprogramma, hierin staat de aansturing vd LED
+ int main() {
- arm_begin();
- }
- }
- }
- if(arm_hoogte == 2) {
- if(staat2 == 0) {
- arm_mid();
- wait_iterator2 = 0;
- } else if(staat2 == 1) {
- wait_iterator2++;
- if(wait_iterator2 > 400) {
- staat2 = 2;
-
- arm_begin();
- }
- }
- }
- if(arm_hoogte == 3) {
- if(staat2 == 0) {
- arm_hoog();
- wait_iterator2 = 0;
- } else if(staat2 == 1) {
- wait_iterator2++;
- if(wait_iterator2 > 400) {
- staat2 = 2;
+ pwm_motor1.period_us(100);
+ motor1.setPosition(0);
+ pwm_motor2.period_us(100);
+ motor2.setPosition(0);
+ pc.baud(115200);
+ // Ticker EMG signaal meten
+ Ticker log_timer;
+ //set up filters. Use external array for constants
+ arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
+ arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
+ arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
+ arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
+ // Uitvoeren van ticker EMG, sample frequentie 500Hz
+ log_timer.attach(looper, 0.002);
- arm_begin();
- }
- }
- }
-
-}
-
-
-// Hoofdprogramma, hierin staat de aansturing vd LED
-int main()
-{
-
- pwm_motor1.period_us(100);
- motor1.setPosition(0);
- pwm_motor2.period_us(100);
- motor2.setPosition(0);
- pc.baud(115200);
- // Ticker EMG signaal meten
- Ticker log_timer;
- //set up filters. Use external array for constants
- arm_biquad_cascade_df1_init_f32(&lowpass_biceps,1 , lowpass_const, lowpass_biceps_states);
- arm_biquad_cascade_df1_init_f32(&lowpass_deltoid,1 , lowpass_const, lowpass_deltoid_states);
- arm_biquad_cascade_df1_init_f32(&highnotch_biceps,2 ,highnotch_const,highnotch_biceps_states);
- arm_biquad_cascade_df1_init_f32(&highnotch_deltoid,2 ,highnotch_const,highnotch_deltoid_states);
- // Uitvoeren van ticker EMG, sample frequentie 500Hz
- log_timer.attach(looper, 0.002);
-
- // Aanroepen van motoraansturing in motor ticker
- Ticker looptimer;
- looptimer.attach(looper_motor,TSAMP);
-
- while(1) {
+ // Aanroepen van motoraansturing in motor ticker
+ Ticker looptimer;
+ looptimer.attach(looper_motor,TSAMP);
while(1) {
- pc.printf("Span de biceps aan om het instellen te starten.\n");
- do {
- ShineRed();
- } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
- if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen
- BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
- BlinkGreen(); // groen knipperen, meten van spieraanspanning
- while (1) { // eerste loop, keuze voor de positie van het batje
- pc.printf("In de loop.\n");
- if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden
- stopblinkgreen();
- pc.printf("ShineGreen.\n");
- ShineGreen();
- wait (4);
- break;
+
+ while(1) {
+ pc.printf("Span de biceps aan om het instellen te starten.\n");
+ do {
+ ShineRed();
+ } while(filtered_average_bi < 0.05 && filtered_average_del <0.05); // In rust, geen meting
+ if (filtered_average_bi > 0.05) { // Beginnen met meting wanneer biceps wordt aangespannen
+ BlinkRed(10); // 2 seconden rood knipperen, geen signaal verwerking
+ BlinkGreen(); // groen knipperen, meten van spieraanspanning
+ while (1) { // eerste loop, keuze voor de positie van het batje
+ pc.printf("In de loop.\n");
+ if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { //bi en del aangespannen --> batje in het midden
+ stopblinkgreen();
+ pc.printf("ShineGreen.\n");
+ ShineGreen();
+ wait (4);
+ break;
+ }
+ if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links
+ stopblinkgreen();
+ pc.printf("ShineBlue.\n");
+ ShineBlue();
+ batje_hoek = 2;
+ wait(4);
+ break;
+ } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts
+ stopblinkgreen();
+ pc.printf("ShineRed.\n");
+ ShineRed();
+ batje_hoek = 1;
+ wait (4);
+ break;
+ }
}
- if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> batje naar links
- stopblinkgreen();
- pc.printf("ShineBlue.\n");
- ShineBlue();
- batje_hoek = 2;
- wait(4);
- break;
- } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts
- stopblinkgreen();
- pc.printf("ShineRed.\n");
- ShineRed();
- batje_hoek = 1;
- wait (4);
- break;
+ BlinkGreen();
+ while (1) { // loop voor het instellen van de kracht
+ pc.printf("In de loop.\n");
+ if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan
+ stopblinkgreen();
+ pc.printf("ShineGreen.\n");
+ ShineGreen();
+ arm_hoogte = 3;
+ wait (4);
+ break;
+ }
+ if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan
+ stopblinkgreen();
+ pc.printf("ShineBlue.\n");
+ ShineBlue();
+ arm_hoogte = 1;
+ wait(4);
+ break;
+ } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan
+ stopblinkgreen();
+ pc.printf("ShineRed.\n");
+ ShineRed();
+ arm_hoogte = 2;
+ wait (4);
+ break;
+ }
}
- }
- BlinkGreen();
- while (1) { // loop voor het instellen van de kracht
- pc.printf("In de loop.\n");
- if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan
- stopblinkgreen();
- pc.printf("ShineGreen.\n");
- ShineGreen();
- arm_hoogte = 3;
- wait (4);
- break;
- }
- if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) { // del aanspannen --> laag slaan
- stopblinkgreen();
- pc.printf("ShineBlue.\n");
- ShineBlue();
- arm_hoogte = 1;
- wait(4);
- break;
- } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan
- stopblinkgreen();
- pc.printf("ShineRed.\n");
- ShineRed();
- arm_hoogte = 2;
- wait (4);
- break;
- }
+
}
}
-
}
- }
-}
\ No newline at end of file
+ }
\ No newline at end of file