Er is uitleg bijgeschreven en pwm_percentage heeft een andere naam
Dependencies: Encoder HIDScope MODSERIAL mbed-dsp mbed
Fork of Lampje_EMG_Gr6 by
Diff: main.cpp
- Revision:
- 22:14f5161d7d7b
- Parent:
- 21:674fafb6301d
- Child:
- 23:94b5746e52f5
diff -r 674fafb6301d -r 14f5161d7d7b main.cpp --- a/main.cpp Fri Oct 31 16:01:22 2014 +0000 +++ b/main.cpp Fri Oct 31 16:21:39 2014 +0000 @@ -54,8 +54,8 @@ float pos_motor2_rad; int staat1 = 0; int staat2 = 0; -float arm_hoogte; -float batje_hoek; +volatile float arm_hoogte = 0; +volatile float batje_hoek = 0; int wait_iterator1 = 0; int wait_iterator2 = 0; @@ -311,13 +311,13 @@ void batje_rechts () { - speed1_rad = 3.0; //positief is CCW, negatief CW (boven aanzicht) + speed1_rad = 2.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); } - if(setpoint1 < -(360*2.3*2.0*PI/360)) { - setpoint1 = -(360*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); } pwm_motor1.write(abs(pwm1_percentage)); prev_setpoint1 = setpoint1; @@ -331,11 +331,11 @@ { speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht) setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden - setpoint1 = (180*2.3*2.0*PI/360); + if(setpoint1 > (0*2.3*2.0*PI/360)) { //setpoint in graden + setpoint1 = (0*2.3*2.0*PI/360); } - if(setpoint1 < -(180*2.3*2.0*PI/360)) { - setpoint1 = -(180*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; } @@ -344,8 +344,8 @@ { speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht) setpoint1 = prev_setpoint1 + TSAMP * speed1_rad; - if(setpoint1 > (180*2.3*2.0*PI/360)) { //setpoint in graden - setpoint1 = (180*2.3*2.0*PI/360); + 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); @@ -373,14 +373,14 @@ { 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 > (180.0*2.0*PI/360)) { //setpoint in graden + setpoint2 = (180.0*2.0*PI/360); } - if(setpoint2 < -(155.0*2.0*PI/360)) { - setpoint2 = -(155.0*2.0*PI/360); + if(setpoint2 < -(180.0*2.0*PI/360)) { + setpoint2 = -(180.0*2.0*PI/360); } prev_setpoint2 = setpoint2; - if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { + if(setpoint2 >= (180.0*2.0*PI/360)-0.1) { staat2 = 1; } } @@ -389,14 +389,14 @@ { 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 > (90*2.0*PI/360)) { //setpoint in graden + setpoint2 = (90*2.0*PI/360); } - if(setpoint2 < -(155.0*2.0*PI/360)) { - setpoint2 = -(155.0*2.0*PI/360); + if(setpoint2 < -(90.0*2.0*PI/360)) { + setpoint2 = -(90.0*2.0*PI/360); } prev_setpoint2 = setpoint2; - if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { + if(setpoint2 >= (90.0*2.0*PI/360)-0.1) { staat2 = 1; } } @@ -454,7 +454,7 @@ //STATES - if (batje_hoek == 0) { + if (batje_hoek == 1) { if(staat1 == 0) { batje_rechts(); wait_iterator1 = 0; @@ -467,7 +467,7 @@ } } - if (batje_hoek == 1) { + if (batje_hoek == 2) { if(staat1 == 0) { batje_links(); wait_iterator1 = 0; @@ -481,7 +481,7 @@ } - if(arm_hoogte == 0) { + if(arm_hoogte == 1) { if(staat2 == 0) { arm_laag(); wait_iterator2 = 0; @@ -493,7 +493,7 @@ arm_begin(); } } - if(arm_hoogte == 1) { + if(arm_hoogte == 2) { if(staat2 == 0) { arm_mid(); wait_iterator2 = 0; @@ -505,7 +505,7 @@ arm_begin(); } } - if(arm_hoogte == 2) { + if(arm_hoogte == 3) { if(staat2 == 0) { arm_hoog(); wait_iterator2 = 0; @@ -567,7 +567,7 @@ stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); - batje_hoek = 1; + batje_hoek = 2; wait(4); break; } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) @@ -576,7 +576,7 @@ stopblinkgreen(); pc.printf("ShineRed.\n"); ShineRed(); - batje_hoek = 0; + batje_hoek = 1; wait (4); break; } @@ -588,7 +588,7 @@ stopblinkgreen(); pc.printf("ShineGreen.\n"); ShineGreen(); - arm_hoogte = 2; + arm_hoogte = 3; wait (4); break; } @@ -596,7 +596,7 @@ stopblinkgreen(); pc.printf("ShineBlue.\n"); ShineBlue(); - arm_hoogte = 0; + arm_hoogte = 1; wait(4); break; } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05) @@ -605,7 +605,7 @@ stopblinkgreen(); pc.printf("ShineRed.\n"); ShineRed(); - arm_hoogte = 1; + arm_hoogte = 2; wait (4); break; }