Diff: main.cpp

Revision:

13:017aa9316f92

Parent:

12:eb2c7103a439

diff -r eb2c7103a439 -r 017aa9316f92 main.cpp
--- a/main.cpp	Thu Oct 30 14:11:21 2014 +0000
+++ b/main.cpp	Thu Oct 30 15:45:22 2014 +0000
@@ -58,28 +58,39 @@
     looptimer.attach(setlooptimerflag,TSAMP);
     pwm_motor1.period_us(100); //10kHz PWM frequency
     pwm_motor2.period_us(100); //10kHz PWM frequency
-/*
+
 if(calibration==0){
     //calibration motor 2
-    pwm_motor2.write(1); //lage PWM
+    pwm_motor2.write(0.6); //lage PWM
     motor2dir = 1;
-    wait(1);                // anders wordt de while(1) meteen onderbroken 
+    wait(2);                // anders wordt de while(1) meteen onderbroken 
     while(1) {
-        if(motor2.getSpeed > -0.5 || motor2.getSpeed < 0.5) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+        if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
             pwm_motor2.write(0); 
             motor2.setPosition(0);
-        }
-        break;   
-    scope.set(0, motor2.getSpeed());
-    scope.send();
-    wait(0.1);
+            goto motor1cal;
+        } 
+        wait(0.01);               
     }
+motor1cal:     
     //kalibration motor 1
-    calibration = 1;
+    pwm_motor1.write(0.55); //lage PWM
+    motor1dir = 1;
+    wait(2);                // anders wordt de while(1) meteen onderbroken
+    while(1) {
+        if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+            pwm_motor1.write(0); 
+            motor1.setPosition(0);
+            calibration = 1;
+            goto motor2control;
+        }   
+        scope.set(0, motor1.getSpeed()*omrekenfactor1);
+        scope.send();
+        wait(0.01);          
+    }
 }
-*/
-// nog een goto motor2control;
-// motor2control:
+
+motor2control:
     while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
@@ -107,7 +118,7 @@
         } else {
             pwm_motor2.write(0);
             batjeset = integral = 0;
-            wait(2);
+            wait(1);
             goto motor1control;
         }
     }
@@ -124,7 +135,7 @@
         } else {            //regelaar motor1, bepaalt positie
             pwm_motor1.write(0);
             balhit = integral = 0;
-            wait(2); // wait voordat arm weer naar beginpositie terugkeert
+            wait(1); // wait voordat arm weer naar beginpositie terugkeert
             goto resetpositionmotor1;
         }
 
@@ -177,7 +188,7 @@
         } else {
             pwm_motor1.write(0);
             batjeset = integral = 0;
-            wait(5);
+            wait(1);
             goto resetpositionmotor2;
         }
     }
@@ -212,7 +223,7 @@
         } else {
             pwm_motor2.write(0);
             batjeset = integral = 0;
-            wait(2);
+            wait(1);
             goto test;
             //direction = force = 0;
             //goto directionchoice;