Revision 28:c33a0658605e, committed 2014-11-01

Comitter:

irisl

Date:

Sat Nov 01 16:19:53 2014 +0000

Parent:

27:691779624530

Child:

29:d0dab8921e9d

Commit message:

Commentaar bijgevoegd

Changed in this revision

--- a/main.cpp	Sat Nov 01 13:43:21 2014 +0000
+++ b/main.cpp	Sat Nov 01 16:19:53 2014 +0000
@@ -21,13 +21,15 @@
 
 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
 
+// Define objects
 Serial pc(USBTX, USBRX);
 
+// LED
 DigitalOut myledred(PTB3);
 DigitalOut myledgreen(PTB1);
 DigitalOut myledblue(PTB2);
 
-//Define objects
+//EMG
 AnalogIn    emg0(PTB0); //Analog input
 AnalogIn    emg1(PTC2); //Analog input
 HIDScope scope(4);
@@ -42,6 +44,7 @@
 PwmOut pwm_motor2(M1_PWM);
 DigitalOut motordir2(M1_DIR);
 
+// Motor variabelen
 float pwm1_percentage = 0;
 float pwm2_percentage = 0;
 int cur_pos_motor1;
@@ -60,8 +63,9 @@
 int wait_iterator2 = 0;
 
 
-// EMG
+// EMG Filters (settings en variabelen)
 
+// Filters
 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
 arm_biquad_casd_df1_inst_f32 lowpass_deltoid;
 //lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
@@ -110,21 +114,7 @@
     }
 }
 
-/** Looper function
-* functions used for Ticker and Timeout should be of type void <name>(void)
-* i.e. no input arguments, no output arguments.
-* if you want to change a variable that you use in other places (for example in main)
-* you will have to make that variable global in order to be able to reach it both from
-* the function called at interrupt time, and in the main function.
-* To make a variable global, define it under the includes.
-* variables that are changed in the interrupt routine (written to) should be made
-* 'volatile' to let the compiler know that those values may change outside the current context.
-* i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value"
-* in the example below, the variable is not re-used in the main function, and is thus declared
-* local in the looper function only.
-**/
-
-
+// EMG looper
 void looper()
 {
     /*variable to store value in*/
@@ -164,9 +154,6 @@
 
 // LED AANSTURING
 
-Ticker ledticker;
-
-
 void BlinkRed(int n)
 {
     for (int i=0; i<n; i++) {
@@ -181,6 +168,9 @@
     }
 }
 
+// Ticker voor groen knipperen, zodat tijdens dit knipperen presets gekozen kunnen worden
+Ticker ledticker;
+
 void greenblink()
 {
     if(myledgreen.read())
@@ -209,35 +199,7 @@
     ledticker.detach();
 }
 
-
-void BlinkGreen1 ()
-{
-
-    myledred = 0;
-    myledgreen = 0;
-    myledblue = 0;
-    wait(0.1);
-    myledred = 0;
-    myledgreen = 1;
-    myledblue = 0;
-    wait(0.1);
-}
-
-
-void BlinkBlue(int n)
-{
-    for (int i=0; i<n; i++) {
-        myledred = 0;
-        myledgreen = 0;
-        myledblue = 0;
-        wait(0.1);
-        myledred = 0;
-        myledgreen = 0;
-        myledblue = 1;
-        wait(0.1);
-    }
-}
-
+// Groen schijnen
 void ShineGreen ()
 {
     myledred = 0;
@@ -245,6 +207,7 @@
     myledblue = 0;
 }
 
+// Blauw schijnen
 void ShineBlue ()
 {
     myledred = 0;
@@ -252,6 +215,7 @@
     myledblue = 1;
 }
 
+// Rood schijnen
 void ShineRed ()
 {
     myledred = 1;
@@ -261,6 +225,9 @@
 
 // MOTORFUNCTIES
 
+// 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.
 {
@@ -268,6 +235,7 @@
     // *in = het getal dat staat op locatie van in --> waarde van new_pwm
 }
 
+// PI-regelaar motor1: batje
 float pid1(float setpoint1, float measurement1)
 {
     float error1;
@@ -280,6 +248,7 @@
     return out_p1 + out_i1;
 }
 
+// PI-regelaar motor2: arm
 float pid2(float setpoint2, float measurement2)
 {
     float error2;
@@ -291,16 +260,21 @@
     clamp(&out_i2,-I_LIMIT,I_LIMIT);
     return out_p2 + out_i2;
 }
+
+// Variabelen
 float prev_setpoint1 = 0;
 float setpoint1 = 0;
 float prev_setpoint2 = 0;
 float setpoint2 = 0;
 
+// Functies motoren
+
+// Motor1 links draaien
 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
+    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)) {
@@ -309,6 +283,7 @@
     prev_setpoint1 = setpoint1;
 }
 
+// Motor1 rechts draaien
 void batje_rechts ()
 {
     speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
@@ -326,7 +301,7 @@
     }
 }
 
-
+//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)
@@ -340,6 +315,7 @@
     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)
@@ -353,6 +329,7 @@
     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)
@@ -369,6 +346,7 @@
     }
 }
 
+// Motor2 balletje in het midden slaan
 void arm_mid ()
 {
     speed2_rad = 4.0; //positief is CCW, negatief CW (boven aanzicht)
@@ -385,6 +363,7 @@
     }
 }
 
+// Motor2 balletje op het laagst slaan
 void arm_laag ()
 {
     speed2_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
@@ -401,6 +380,7 @@
     }
 }
 
+// Motor2 arm terug zetten in beginstand
 void arm_begin ()
 {
     speed2_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
@@ -414,6 +394,7 @@
     prev_setpoint2 = setpoint2;
 }
 
+// MOTOR aansturing
 void looper_motor()
 {
     //MOTOR1
@@ -462,7 +443,7 @@
             wait_iterator1++;
             if(wait_iterator1 > 400) {
                 staat1 = 2;
-            
+
                 batje_begin_rechts();
             }
         }
@@ -475,7 +456,7 @@
             wait_iterator1++;
             if(wait_iterator1 > 400) {
                 staat1 = 2;
-           
+
                 batje_begin_links ();
             }
         }
@@ -489,7 +470,7 @@
             wait_iterator2++;
             if(wait_iterator2 > 400) {
                 staat2 = 2;
-            
+
                 arm_begin();
             }
         }
@@ -502,7 +483,7 @@
             wait_iterator2++;
             if(wait_iterator2 > 400) {
                 staat2 = 2;
-           
+
                 arm_begin();
             }
         }
@@ -515,7 +496,7 @@
             wait_iterator2++;
             if(wait_iterator2 > 400) {
                 staat2 = 2;
-            
+
                 arm_begin();
             }
         }
@@ -524,6 +505,7 @@
 }
 
 
+// Hoofdprogramma, hierin staat de aansturing vd LED
 int main()
 {
 
@@ -532,52 +514,47 @@
     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);
-    /**Here you attach the 'void looper(void)' function to the Ticker object
-    * The looper() function will be called every 0.01 seconds.
-    * Please mind that the parentheses after looper are omitted when using attach.
-    */
+    // 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) { //Loop
-        /*Empty!*/
-        /*Everything is handled by the interrupt routine now!*/
+
+    while(1) {
 
         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) {
-                BlinkRed(10);
-                BlinkGreen();
-                while (1) {
+            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) {
+                    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) {
+                    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)
-
-                    {
+                    } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> batje naar rechts
                         stopblinkgreen();
                         pc.printf("ShineRed.\n");
                         ShineRed();
@@ -587,9 +564,9 @@
                     }
                 }
                 BlinkGreen();
-                while (1) {
+                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) {
+                    if (filtered_average_bi > 0.05 && filtered_average_del > 0.045) { // bi en del aanspannen --> hoog slaan
                         stopblinkgreen();
                         pc.printf("ShineGreen.\n");
                         ShineGreen();
@@ -597,16 +574,14 @@
                         wait (4);
                         break;
                     }
-                    if (filtered_average_bi < 0.05 && filtered_average_del > 0.045) {
+                    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)
-
-                    {
+                    } else if (filtered_average_bi > 0.05 && filtered_average_del < 0.045) { // bi aanspannen --> midden slaan
                         stopblinkgreen();
                         pc.printf("ShineRed.\n");
                         ShineRed();