Diff: begintotaalscript.cpp

Revision:

6:d4f355d72f66

Parent:

5:4adfd729a291

--- a/begintotaalscript.cpp	Tue Nov 05 10:01:57 2013 +0000
+++ b/begintotaalscript.cpp	Wed Nov 06 16:37:36 2013 +0000
@@ -1,8 +1,23 @@
+/* - - - - - - - - - - - - - - - - - - - - - - */
+/*                                             */
+/*  BRONCODE TEKENROBOT GROEP2: THE DRAWESOME  */
+/*  -----------------------------------------  */
+/*  Belinda Brandwacht   s1226290              */              
+/*  Esther Keulers       s1255444              */  
+/*  Maaike Schotman      s1274104              */  
+/*  Gerjan Wolterink     s1197797              */  
+/*  Roelof van Zwol      s1240811              */  
+/*                                             */
+/* - - - - - - - - - - - - - - - - - - - - - - */
+
+//libraries aanroepen.
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "encoder.h"
 
-volatile bool looptimerflag;
+// Looptimerflag instellen
+
+volatile bool looptimerflag;   
 
 void setlooptimerflag(void)
 {
@@ -14,33 +29,38 @@
 {
     //Communicatie met de pc
     MODSERIAL pc(USBTX,USBRX,64,1024);
-    pc.baud(115200);
+    pc.baud(115200); //zet de baud in realterm ook op dit nummer. 
 
-    //Benoemen inputs
+    //Benoemen inputs van de drie rode knoppen op de robot
     DigitalIn knop1(PTC2);
     DigitalIn knop2(PTB3);
     DigitalIn knop3(PTB2);
-
+    
+    //Input mode van de knoppen instellen. 
+    knop1.mode(PullUp);
+    knop2.mode(PullUp);
+    knop3.mode(PullUp);
+    
     AnalogIn    emg1(PTB0); //Analog input emg1
     AnalogIn    emg2(PTB1); //Analog input emg2
 
-    Encoder motor1(PTD0,PTC9);
+    //Benoemen pinnen waarop de encoder van de motoren is aangeloten.
+    Encoder motor1(PTD0,PTC9); 
     Encoder motor2(PTD2,PTC8);
 
     /* PWM control to motor */
     PwmOut pwm_motor1(PTA12);
     PwmOut pwm_motor2(PTA5);
+   
     /* Direction pin */
     DigitalOut motor1dir(PTD3);
     DigitalOut motor2dir(PTD1);
-
-    knop1.mode(PullUp);
-    knop2.mode(PullUp);
-    knop3.mode(PullUp);
+    
 
     //Variabelen voor menu
     int state = 1;
 
+    // Met bool wordt gezegd dat variablen 'true' of 'false' kunnen zijn. 
     bool calibratie_rust = false;
     bool calibratie_max  = false;
     bool calibratie_motor = false;
@@ -50,12 +70,12 @@
     double yy,z,zabs,w,y1,z1,zabs1,w1,y2,z2,zabs2,w2,e1,e2,e3,f1,f2,g1,g2,g3,h1,h2,byy,bz,bzabs,bw,by1,bz1,bzabs1,bw1,by2,bz2,bzabs2,bw2,be1,be2,be3,bf1,bf2,bg1,bg2,bg3,bh1,bh2;
     double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;
 
-// Variabelen benoemen voor regelaar motor.
+    // Variabelen benoemen voor regelaar motor.
     double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm,pi;
     double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm, dri, dri_1, utot_r, inputsinus;
     double motor1_maxu, motor2_maxu;
 
-
+    
     pi=3.14159265359;
 
     e1= 0.855930601814815;
@@ -107,7 +127,8 @@
     ybegin=0; //beginpositie
     qbegin=5.50;  //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
     MaxsnelheidV=150.0;     //in mm/s
-    MaxsnelheidD=0.5;    //in rad/s eerst 0.26 rad/s (15 deg/sec)
+    MaxsnelheidD=0.5*pi;    //in rad/s eerst 0.26 rad/s (15 deg/sec)
+    
 //constanten regelaar
     kp_r = 0.006;
     ki_r = 0.005;
@@ -117,6 +138,7 @@
 
     while (true) {              //oneindige while loop
 
+        //Loop die allen gestart wordt als de looptimer is verlopen. 
         while(looptimerflag != true);
         looptimerflag = false;
 
@@ -166,7 +188,7 @@
         if (state == 2) {
             pc.printf("state 2 cal_motor    |    knop1 = terug naar rust \n\r");
 
-
+            //Als de robot arm met pen met de hand op de begin posisie gezet is moet knop 1 weer worden ingedrukt.
             if (knop1 == false ) {
                 state=1;
                 wait(0.05);
@@ -174,8 +196,8 @@
                 while(knop1 == false) {}
                 wait(0.05);
 
-                motor1.setPosition(200.0);
-                motor2.setPosition(597.15);
+                motor1.setPosition(200.0);   //Zeg dat de motor die de arm verplaatst 200 counts heeft, dit komt overeen met (200/CPR)/gearatio = (200/CPR)/50=0.125 dat komt overeen met een hoek van .25 pi
+                motor2.setPosition(597.15);  //
 
                 x=0;
                 y=0;