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:
- 23:94b5746e52f5
- Parent:
- 22:14f5161d7d7b
- Child:
- 24:a61c2cadbd36
--- a/main.cpp Fri Oct 31 16:21:39 2014 +0000 +++ b/main.cpp Fri Oct 31 16:30:03 2014 +0000 @@ -8,7 +8,7 @@ #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 (3.5) +#define K_P2 (0.7) #define K_I2 (0.01 *TSAMP) #define I_LIMIT 1. //#define PI 3.14159265 @@ -300,18 +300,18 @@ { 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 > (11.3*2.3*2.0*PI/360)) { //setpoint in graden + setpoint1 = (11.3*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 < -(11.3*2.3*2.0*PI/360)) { + setpoint1 = -(11.3*2.3*2.0*PI/360); } prev_setpoint1 = setpoint1; } void batje_rechts () { - speed1_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht) + 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); @@ -342,7 +342,7 @@ void batje_begin_rechts () { - speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht) + 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); @@ -355,48 +355,48 @@ void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN { - speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht) + speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht) setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (360.0*2.0*PI/360)) { //setpoint in graden - setpoint2 = (360.0*2.0*PI/360); + if(setpoint2 > (155.0*2.0*PI/360)) { //setpoint in graden + setpoint2 = (155.0*2.0*PI/360); } - if(setpoint2 < -(360.0*2.0*PI/360)) { - setpoint2 = -(360.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 >= (360.0*2.0*PI/360)-0.1) { + if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { staat2 = 1; } } void arm_mid () { - speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht) + speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht) setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - 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)) { //setpoint in graden + setpoint2 = (155.0*2.0*PI/360); } - if(setpoint2 < -(180.0*2.0*PI/360)) { - setpoint2 = -(180.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 >= (180.0*2.0*PI/360)-0.1) { + if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { staat2 = 1; } } void arm_laag () { - speed2_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht) + speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht) setpoint2 = prev_setpoint2 + TSAMP * speed2_rad; - if(setpoint2 > (90*2.0*PI/360)) { //setpoint in graden - setpoint2 = (90*2.0*PI/360); + if(setpoint2 > (155*2.0*PI/360)) { //setpoint in graden + setpoint2 = (155*2.0*PI/360); } - if(setpoint2 < -(90.0*2.0*PI/360)) { - setpoint2 = -(90.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 >= (90.0*2.0*PI/360)-0.1) { + if(setpoint2 >= (155.0*2.0*PI/360)-0.1) { staat2 = 1; } } @@ -460,7 +460,7 @@ wait_iterator1 = 0; } else if(staat1 ==1) { wait_iterator1++; - if(wait_iterator1 > 600) + if(wait_iterator1 > 1000) staat1 = 2; } else { batje_begin_rechts(); @@ -473,7 +473,7 @@ wait_iterator1 = 0; } else if(staat1 ==1) { wait_iterator1++; - if(wait_iterator1 > 600) + if(wait_iterator1 > 1000) staat1 = 2; } else { batje_begin_links (); @@ -487,7 +487,7 @@ wait_iterator2 = 0; } else if(staat2 == 1) { wait_iterator2++; - if(wait_iterator2 > 200) + if(wait_iterator2 > 400) staat2 = 2; } else { arm_begin(); @@ -499,7 +499,7 @@ wait_iterator2 = 0; } else if(staat2 == 1) { wait_iterator2++; - if(wait_iterator2 > 200) + if(wait_iterator2 > 400) staat2 = 2; } else { arm_begin(); @@ -511,7 +511,7 @@ wait_iterator2 = 0; } else if(staat2 == 1) { wait_iterator2++; - if(wait_iterator2 > 200) + if(wait_iterator2 > 400) staat2 = 2; } else { arm_begin();