Diff: main.cpp

Revision:

6:2674fe30b610

Parent:

5:81d3b53087c0

Child:

7:ca880e05bb04

--- a/main.cpp	Fri Oct 27 08:44:21 2017 +0000
+++ b/main.cpp	Mon Oct 30 12:26:53 2017 +0000
@@ -212,10 +212,10 @@
         led_B=1;
         led_R=0;
         while(true){
-        //button.fall(ledtX);    
-        if (emgBRcomplete > thresholdBR) {
+        button.fall(ledtX);    
+        /*if (emgBRcomplete > thresholdBR) {
         ledtX();
-        }                       
+        } */                      
         t.start();
         huidigetijdX=t.read();
         if (huidigetijdX>2){
@@ -258,10 +258,10 @@
     led_B=0; 
     led_R=1;
     while(true){
-    //button.fall(ledtY);  
-    if (emgBRcomplete > thresholdBR) {
+    button.fall(ledtY);  
+    /*if (emgBRcomplete > thresholdBR) {
         ledtY();
-        }
+        }*/
     t.start();
     huidigetijdY=t.read();
     if (huidigetijdY>2){
@@ -460,19 +460,19 @@
 double GetReferenceVelocity1()
 {
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
-    double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
-    referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;  
+    double maxVelocity1=fabs(Vex+Vey)/20*8.4; // max 8.4 in rad/s of course!       
+    referenceVelocity1 = speedfactor1 * maxVelocity1;  
                 
-    if (dcounto < (10))
-        {   speedfactor1 = 0.01;
-            if (dcounto > (-10))
-                { printf("kleiner111111111");
+    if (Encoder1.getPulses() < (dcounto+10))
+        {   speedfactor1 = -1;
+            if (Encoder1.getPulses() > (dcounto-10))
+                { printf("kleiner");
                 speedfactor1=0;
                 }
         }
-        else if (dcounto > (-10))
-        {   speedfactor1 = -0.01;
-             if (dcounto < (10))
+        else if ((Encoder1.getPulses()) > (dcounto-10))
+        {   speedfactor1 = 1;
+             if (Encoder1.getPulses() < (dcounto+10))
                 { printf("groter");
                 speedfactor1=0;
                 }
@@ -488,7 +488,7 @@
 double GetReferenceVelocity2()
 {
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
-    double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
+    double maxVelocity2=fabs(Vex*25+Vey*25); // max 8.4 in rad/s of course!       
     referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;  
                 
     if (Encoder2.getPulses() < (dcountb+10))
@@ -516,7 +516,7 @@
 double GetReferenceVelocity3()
 {
     // Returns reference velocity in rad/s. Positive value means clockwise rotation.
-    double maxVelocity3=Vex*25+Vey*25; // max 8.4 in rad/s of course!       
+    double maxVelocity3=fabs(Vex*15+Vey*15); // max 8.4 in rad/s of course!       
     referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;  
                 
     if (Encoder3.getPulses() < (dcountr+10))
@@ -702,9 +702,6 @@
     Psx=(Xin_new)*30+121;
     Psy=(Yin_new)*30+308;     
     pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
-    hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
-    hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
-    hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
     return 0;
 }
  
@@ -718,8 +715,8 @@
         Vey=(Vey/modVe)*Vmax;
     }
     Pst();
-    //pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
-    if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
+    pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
+    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)){
         Move_done=true;
         loop.detach();
         }
@@ -751,7 +748,11 @@
     }
     }
 }
- 
+void setcurrentposition(){ 
+    hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
+    hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
+    hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
+    }
 int main()
 {
     pc.baud(115200);
@@ -764,6 +765,7 @@
         
         tellerX();
         tellerY();
+        setcurrentposition();
         calculator();
         controlmotor1.attach(&MeasureAndControl1, 0.01);
         calculatedelta1.attach(&calcdelta1, 0.01);