Revision 21:674fafb6301d, committed 2014-10-31

Comitter:

irisl

Date:

Fri Oct 31 16:01:22 2014 +0000

Parent:

20:d40b6cba4280

Child:

22:14f5161d7d7b

Commit message:

HOPELIJK WERKT HET!! Motoren zijn geintegreerd, we moeten helaas op Anke wachten

Changed in this revision

--- a/main.cpp	Wed Oct 29 15:08:30 2014 +0000
+++ b/main.cpp	Fri Oct 31 16:01:22 2014 +0000
@@ -3,12 +3,16 @@
 #include "arm_math.h"
 #include "MODSERIAL.h"
 #include "encoder.h"
+#include "PwmOut.h"
 
-#define TSAMP 0.01
-#define K_P (0.1)
-#define K_I (0.03  *TSAMP)
-#define K_D (0.001 /TSAMP)
+#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_I2 (0.01 *TSAMP)
 #define I_LIMIT 1.
+//#define PI 3.14159265
+#define l_arm 0.5
 
 #define M1_PWM PTC8
 #define M1_DIR PTC9
@@ -19,36 +23,41 @@
 
 Serial pc(USBTX, USBRX);
 
-DigitalOut myled1(LED_RED);
-DigitalOut myled2(LED_GREEN);
-DigitalOut myled3(LED_BLUE);
+DigitalOut myledred(PTB3);
+DigitalOut myledgreen(PTB1);
+DigitalOut myledblue(PTB2);
 
 //Define objects
-AnalogIn    emg0(PTB1); //Analog input
-AnalogIn    emg1(PTB2); //Analog input
+AnalogIn    emg0(PTB0); //Analog input
+AnalogIn    emg1(PTC2); //Analog input
 HIDScope scope(2);
 
 //motor 25D
-Encoder motor1(PTD3,PTD5,true); //wit, geel
+Encoder motor1(PTD3,PTD5); //wit, geel
 PwmOut pwm_motor1(M2_PWM);
 DigitalOut motordir1(M2_DIR);
 
 //motor2 37D
-Encoder motor2(PTD2, PTD0,true); //wit, geel
+Encoder motor2(PTD2, PTD0); //wit, geel
 PwmOut pwm_motor2(M1_PWM);
 DigitalOut motordir2(M1_DIR);
 
-float speed1;
-float hoek1;
-float speed2;
-float hoek2;
-
-bool flip=false;
-
-void attime()
-{
-    flip = !flip;
-}
+float pwm1_percentage = 0;
+float pwm2_percentage = 0;
+int cur_pos_motor1;
+int prev_pos_motor1 = 0;
+int cur_pos_motor2;
+int prev_pos_motor2 = 0;
+float speed1_rad;
+float speed2_rad;
+float pos_motor1_rad;
+float pos_motor2_rad;
+int staat1 = 0;
+int staat2 = 0;
+float arm_hoogte;
+float batje_hoek;
+int wait_iterator1 = 0;
+int wait_iterator2 = 0;
 
 
 // EMG
@@ -63,7 +72,7 @@
 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
 arm_biquad_casd_df1_inst_f32 highnotch_deltoid;
 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB
-float highnotch_const[] = {0.956542835577484, -1.913085671154968, 0.956542835577484, 1.911196288237583, -0.914975054072353,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
+float highnotch_const[] = {0.9149684297741606, -1.8299368595483212, 0.9149684297741606, 1.8226935021735358, -0.8371802169231065 ,0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
 //state values
 float highnotch_biceps_states[8];
 float highnotch_deltoid_states[8];
@@ -161,29 +170,29 @@
 void BlinkRed(int n)
 {
     for (int i=0; i<n; i++) {
-        myled1 = 1;
-        myled2 = 1;
-        myled3 = 1;
+        myledred = 0;
+        myledgreen = 0;
+        myledblue = 0;
         wait(0.1);
-        myled1 = 0;
-        myled2 = 1;
-        myled3 = 1;
+        myledred = 1;
+        myledgreen = 0;
+        myledblue = 0;
         wait(0.1);
     }
 }
 
 void greenblink()
 {
-    if(myled2.read())
-        myled2 = 0;
+    if(myledgreen.read())
+        myledgreen = 0;
     else
-        myled2 = 1;
+        myledgreen = 1;
 }
 
 void BlinkGreen()
 {
-    myled1= 1;
-    myled3 =1;
+    myledred= 0;
+    myledblue =0;
     ledticker.attach(&greenblink,.5);
     /*  myled1 = 1;
       myled2 = 1;
@@ -204,13 +213,13 @@
 void BlinkGreen1 ()
 {
 
-    myled1 = 1;
-    myled2 = 1;
-    myled3 = 1;
+    myledred = 0;
+    myledgreen = 0;
+    myledblue = 0;
     wait(0.1);
-    myled1 = 1;
-    myled2 = 0;
-    myled3 = 1;
+    myledred = 0;
+    myledgreen = 1;
+    myledblue = 0;
     wait(0.1);
 }
 
@@ -218,125 +227,305 @@
 void BlinkBlue(int n)
 {
     for (int i=0; i<n; i++) {
-        myled1 = 1;
-        myled2 = 1;
-        myled3 = 1;
+        myledred = 0;
+        myledgreen = 0;
+        myledblue = 0;
         wait(0.1);
-        myled1 = 1;
-        myled2 = 1;
-        myled3 = 0;
+        myledred = 0;
+        myledgreen = 0;
+        myledblue = 1;
         wait(0.1);
     }
 }
 
 void ShineGreen ()
 {
-    myled1 = 1;
-    myled2 = 0;
-    myled3 = 1;
+    myledred = 0;
+    myledgreen = 1;
+    myledblue = 0;
 }
 
 void ShineBlue ()
 {
-    myled1 = 1;
-    myled2 = 1;
-    myled3 = 0;
+    myledred = 0;
+    myledgreen = 0;
+    myledblue = 1;
 }
 
 void ShineRed ()
 {
-    myled1 = 0;
-    myled2 = 1;
-    myled3 = 1;
+    myledred = 1;
+    myledgreen = 0;
+    myledblue = 0;
 }
 
 // MOTORFUNCTIES
 
-void motor2_speed_low ()
+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.
+{
+*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
+}
+
+float pid1(float setpoint1, float measurement1)
 {
-    wait(1);
-    speed2 = 1;
-    motordir2=1;
-    pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
-    wait(0.3);             //naar 140 graden
-    pwm_motor2.write(0);    //CCW
-    wait(1);
-    motordir2=0;
-    pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
-    wait(0.20);             //balletje slaan, 160 graden
-    pwm_motor2.write(0);
-    wait(1);
-    motordir2=1;            //CW
-    pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
-    wait(1);            //terug naar begin positie, 20 graden
-    pwm_motor2.write(0);
+    float error1;
+    float           out_p1 = 0;
+    static float    out_i1 = 0;
+    error1  = (setpoint1 - measurement1);
+    out_p1  = error1*K_P1;
+    out_i1 += error1*K_I1;
+    clamp(&out_i1,-I_LIMIT,I_LIMIT);
+    return out_p1 + out_i1;
 }
 
-void motor2_speed_mid ()
+float pid2(float setpoint2, float measurement2)
 {
-    wait(1);
-    speed2 = 1;
-    motordir2=1;
-    pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
-    wait(0.3);             //naar 140 graden
-    pwm_motor2.write(0);    //CCW
-    wait(1);
-    motordir2=0;
-    pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
-    wait(0.20);             //balletje slaan, 160 graden
-    pwm_motor2.write(0);
-    wait(1);
-    motordir2=1;            //CW
-    pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
-    wait(1);            //terug naar begin positie, 20 graden
-    pwm_motor2.write(0);
+    float error2;
+    float           out_p2 = 0;
+    static float    out_i2 = 0;
+    error2  = (setpoint2 - measurement2);
+    out_p2  = error2*K_P2;
+    out_i2 += error2*K_I2;
+    clamp(&out_i2,-I_LIMIT,I_LIMIT);
+    return out_p2 + out_i2;
+}
+float prev_setpoint1 = 0;
+float setpoint1 = 0;
+float prev_setpoint2 = 0;
+float setpoint2 = 0;
+
+void batje_links ()
+{
+    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 < -(180*2.3*2.0*PI/360)) {
+        setpoint1 = -(180*2.3*2.0*PI/360);
+    }
+    prev_setpoint1 = setpoint1;
 }
 
-void motor2_speed_high ()
+void batje_rechts ()
 {
-    wait(1);
-    speed2 = 1;
-    motordir2=1;
-    pwm_motor2.write(0.3); //Deze snelheid kan lager worden ingesteld om accurator te zijn.
-    wait(0.3);             //naar 140 graden
-    pwm_motor2.write(0);    //CCW
-    wait(1);
-    motordir2=0;
-    pwm_motor2.write(speed2); //Deze snelheid moet maximaal om het balletje te slaan.
-    wait(0.20);             //balletje slaan, 160 graden
-    pwm_motor2.write(0);
-    wait(1);
-    motordir2=1;            //CW
-    pwm_motor2.write(0.1); //Deze kan lager worden ingesteld om accurater terug te gaan.
-    wait(1);            //terug naar begin positie, 20 graden
-    pwm_motor2.write(0);
-}
-
-void motor1_links()
-{
-    speed1 = 0.7;
-    hoek1 = 0.09; //in seconde
-    wait(1);
-    motordir1=0;            //aangeven van directie (0 = CCW)
-    pwm_motor1.write(speed1);  //snelheid van de motor
-    wait(hoek1);             //Hierdoor kun je het aantal graden bepalen die de as draait
-    pwm_motor1.write(0);
+    speed1_rad = 3.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);
+    }
+    pwm_motor1.write(abs(pwm1_percentage));
+    prev_setpoint1 = setpoint1;
+    if(setpoint1 >= (11.3*2.3*2.0*PI/360)-0.1) {
+        staat1 = 1;
+    }
 }
 
 
-void motor1_rechts()
+void batje_begin_links ()
+{
+    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 < -(180*2.3*2.0*PI/360)) {
+        setpoint1 = -(180*2.3*2.0*PI/360);
+    }
+    prev_setpoint1 = setpoint1;
+}
+
+void batje_begin_rechts ()
+{
+    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.0*2.3*2.0*PI/360)) {
+        setpoint1 = -(0.0*2.3*2.0*PI/360);
+    }
+    prev_setpoint1 = setpoint1;
+}
+
+void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
+{
+    speed2_rad = 4.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 < -(360.0*2.0*PI/360)) {
+        setpoint2 = -(360.0*2.0*PI/360);
+    }
+    prev_setpoint2 = setpoint2;
+    if(setpoint2 >= (360.0*2.0*PI/360)-0.1) {
+        staat2 = 1;
+    }
+}
+
+void arm_mid ()
+{
+    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;
+    }
+}
+
+void arm_laag ()
+{
+    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;
+    }
+}
+
+void arm_begin ()
+{
+    speed2_rad = -2.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);
+    }
+    if(setpoint2 < -(0.0*2.0*PI/360)) {
+        setpoint2 = -(0.0*2.0*PI/360);
+    }
+    prev_setpoint2 = setpoint2;
+}
+
+void looper_motor()
 {
-    speed1 = 0.7;
-    hoek1 = 0.09; //in seconde
-    wait(1);
-    motordir1=1;            //aangeven van directie (1 = CW)
-    pwm_motor1.write(speed1);  //snelheid van de motor
-    wait(hoek1);             //Hierdoor kun je het aantal graden bepalen die de as draait
-    pwm_motor1.write(0);
+    //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;
+    }
+
+    //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;
+    }
+
+
+    //STATES
+
+    if (batje_hoek == 0) {
+        if(staat1 == 0) {
+            batje_rechts();
+            wait_iterator1 = 0;
+        } else if(staat1 ==1) {
+            wait_iterator1++;
+            if(wait_iterator1 > 600)
+                staat1 = 2;
+        } else {
+            batje_begin_rechts();
+        }
+    }
+
+    if (batje_hoek == 1) {
+        if(staat1 == 0) {
+            batje_links();
+            wait_iterator1 = 0;
+        } else if(staat1 ==1) {
+            wait_iterator1++;
+            if(wait_iterator1 > 600)
+                staat1 = 2;
+        } else {
+            batje_begin_links ();
+        }
+    }
+
+
+    if(arm_hoogte == 0) {
+        if(staat2 == 0) {
+            arm_laag();
+            wait_iterator2 = 0;
+        } else if(staat2 == 1) {
+            wait_iterator2++;
+            if(wait_iterator2 > 200)
+                staat2 = 2;
+        } else {
+            arm_begin();
+        }
+    }
+    if(arm_hoogte == 1) {
+        if(staat2 == 0) {
+            arm_mid();
+            wait_iterator2 = 0;
+        } else if(staat2 == 1) {
+            wait_iterator2++;
+            if(wait_iterator2 > 200)
+                staat2 = 2;
+        } else {
+            arm_begin();
+        }
+    }
+    if(arm_hoogte == 2) {
+        if(staat2 == 0) {
+            arm_hoog();
+            wait_iterator2 = 0;
+        } else if(staat2 == 1) {
+            wait_iterator2++;
+            if(wait_iterator2 > 200)
+                staat2 = 2;
+        } else {
+            arm_begin();
+        }
+    }
 }
 
 int main()
 {
+
+    pwm_motor1.period_us(100);
+    motor1.setPosition(0);
+    pwm_motor2.period_us(100);
+    motor2.setPosition(0);
     pc.baud(115200);
 
     Ticker log_timer;
@@ -350,12 +539,14 @@
     * Please mind that the parentheses after looper are omitted when using attach.
     */
     log_timer.attach(looper, 0.001);
+
+    Ticker looptimer;
+    looptimer.attach(looper_motor,TSAMP);
     while(1) { //Loop
         /*Empty!*/
         /*Everything is handled by the interrupt routine now!*/
 
         while(1) {
-            static float count = 0;
             pc.printf("Span de biceps aan om het instellen te starten.\n");
             do {
                 ShineRed();
@@ -376,7 +567,7 @@
                         stopblinkgreen();
                         pc.printf("ShineBlue.\n");
                         ShineBlue();
-                        motor1_links();
+                        batje_hoek = 1;
                         wait(4);
                         break;
                     } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
@@ -385,7 +576,7 @@
                         stopblinkgreen();
                         pc.printf("ShineRed.\n");
                         ShineRed();
-                        motor1_rechts();
+                        batje_hoek = 0;
                         wait (4);
                         break;
                     }
@@ -397,7 +588,7 @@
                         stopblinkgreen();
                         pc.printf("ShineGreen.\n");
                         ShineGreen();
-                        motor2_speed_mid ();
+                        arm_hoogte = 2;
                         wait (4);
                         break;
                     }
@@ -405,7 +596,7 @@
                         stopblinkgreen();
                         pc.printf("ShineBlue.\n");
                         ShineBlue();
-                        motor2_speed_low ();
+                        arm_hoogte = 0;
                         wait(4);
                         break;
                     } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.05)
@@ -414,7 +605,7 @@
                         stopblinkgreen();
                         pc.printf("ShineRed.\n");
                         ShineRed();
-                        motor2_speed_high ();
+                        arm_hoogte = 1;
                         wait (4);
                         break;
                     }