Diff: PROJECT_main.cpp

Revision:

7:ca1ade91bd14

Parent:

6:14051758db6f

Child:

8:75980dc35763

--- a/PROJECT_main.cpp	Mon Nov 03 13:05:31 2014 +0000
+++ b/PROJECT_main.cpp	Mon Nov 03 18:22:20 2014 +0000
@@ -8,9 +8,13 @@
 #define TIMEB4NEXTCHOICE 1  // sec keuzelampje blijft aan
 #define TIMEBETWEENBLINK 100 // sec voor volgende blink
 #define TSAMP_EMG 0.002 //sample frequency emg
-#define KALIBRATIONTIME 1000 // 10 sec voor bepalen van maximale biceps/triceps waarde
+#define KALIBRATIONTIME 500 // 10 sec voor bepalen van maximale biceps/triceps waarde
 #define FACTOR 0.6 //factor*max_waarde = threshold emg
+
 //Define objects
+
+HIDScope scope(2);
+
 AnalogIn    emg0(PTB1);         //Analog input biceps
 AnalogIn    emg1(PTB2);         //Analog input triceps
 
@@ -91,8 +95,10 @@
 PwmOut pwm_motor2(PTA5);
 
 float integral = 0;
+float derivative = 0;
 float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
 float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
+float kalibratie = 0;
 float controlerror = 0;
 float previouserror = 0;
 float pwm = 0;
@@ -107,17 +113,17 @@
 float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
 float Kd1 = 0.0;
 
-float Kp3 = 0.09; //Kp en Ki van motor1, voor de return
-float Ki3 = 0.05;
-float Kd3 = 0.0;
+float Kp3 = 0.15; //Kp en Ki van motor1, voor de return
+float Ki3 = 0.05; //0.09, 0.05
+float Kd3 = 0.1;
 
-float Kp2 = 0.30; //Kp en Ki van motor2, voor in het positie brengen en voor de return
-float Ki2 = 0.20;
-float Kd2 = 0.0;
+float Kp2 = 1.0; //Kp en Ki van motor2, voor in het positie brengen en voor de return
+float Ki2 = 0.8; //0.30 en 0.20
+float Kd2 = 0.1;
 
-float Kp4 = 0.30; //Kp en Ki van motor2, voor de return
-float Ki4 = 0.20;
-float Kd4 = 0.0;
+float Kp4 = 1.0; //Kp en Ki van motor2, voor de return
+float Ki4 = 0.8;
+float Kd4 = 0.1;
 
 volatile bool looptimerflag; //voor motorcontrol TSAMP
 
@@ -126,6 +132,9 @@
 void setlooptimerflag(void)
 {
     looptimerflag = true;
+    scope.set(0, motor2.getPosition()*omrekenfactor2);
+    scope.set(0, motor1.getPosition()*omrekenfactor1);
+    scope.send(); 
 
 }
 
@@ -281,7 +290,7 @@
     blink2.attach(kaltri, 0.2);
 
     //calibration motor 2
-    pwm_motor2.write(0.6); //lage PWM
+    pwm_motor2.write(0.65); //lage PWM
     motor2dir = 0; //rechtsom
     wait(1);                // anders wordt de while(1) meteen onderbroken
     while(1) {
@@ -294,8 +303,8 @@
     }
 motor1cal:
     //calibration motor 1
-    pwm_motor1.write(0.55); //lage PWM
-    motor1dir = 1; //linksom
+    pwm_motor1.write(0.65); //lage PWM
+    motor1dir = 0; //linksom
     wait(1);                // anders wordt de while(1) meteen onderbroken
     while(1) {
         if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen 
@@ -314,6 +323,7 @@
     wait (1);
     for1 = for2 = for3 = 0;
 
+if(kalibratie==0) {
     //biceps kalibratie
     blink.attach(kalbi, 0.2);
     for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
@@ -340,6 +350,8 @@
     pc.printf("kaltri ");
     wait (1);
     for1 = for2 = for3 = 0;
+    kalibratie = 1;
+}
 
 directionchoice:
     log_timer.attach(looper, TSAMP_EMG);
@@ -505,13 +517,13 @@
 
     switch (direction) {
         case 1:
-            setpoint2 = -1*0.436332313+0.197222205;    //25 graden + 11,3 graden, slag naar linkerdoel
+            setpoint2 = (0.436332313+0.197222205);    //25 graden + 11,3 graden, slag naar linkerdoel
             break;
         case 2:
-            setpoint2 = -1*0.436332313;                //25 graden vanaf nul-punt (precies midden)
+            setpoint2 = (0.436332313);                //25 graden vanaf nul-punt (precies midden)
             break;
         case 3:
-            setpoint2 = -1*0.436332313-0.197222205;    // 25 graden - 11,3 graden, slag naar rechterdoel
+            setpoint2 = (0.436332313-0.197222205);    // 25 graden - 11,3 graden, slag naar rechterdoel
             break;
     }
 
@@ -538,17 +550,18 @@
         pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
         previouserror = controlerror; 
 
-        keep_in_range(&pwm, -1,1);
-        pwm_motor2.write(abs(pwm));
+        keep_in_range(&pwm, -1,1);    
+        pwm_motor2.write(abs(pwm));    
         if(pwm > 0) {
             motor2dir = 1;
         } else {
             motor2dir = 0;
         }
 
+
         //controleert of batje positie heeft bepaald
         if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
-            if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
+            if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) {
                 batjeset = 0;
             } else {
                 batjeset++;
@@ -573,9 +586,7 @@
             pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
             previouserror = controlerror; 
         } else {            //regelaar motor1, bepaalt positie
-            pwm_motor1.write(0);
             balhit = integral = derivative = previouserror = 0;
-            wait(1); // wait voordat arm weer naar beginpositie terugkeert
             goto resetpositionmotor1;
         }
 
@@ -590,8 +601,8 @@
 
         //controleert of batje balletje heeft bereikt
         //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
-        if (motor1.getPosition()*omrekenfactor1 > 1.10) {
-            balhit = 1;
+        if (motor1.getPosition()*omrekenfactor1 > 1.60) {
+            balhit = 1; 
         }
     }
 
@@ -618,7 +629,7 @@
 
         //controleert of arm terug in positie is
         if(batjeset < 200) {
-            if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
+            if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
                 batjeset = 0;
             } else {
                 batjeset++;
@@ -655,7 +666,7 @@
 
         //controleert of batje positie heeft bepaald
         if(batjeset < 200) {                                // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
-            if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
+            if (motor2.getPosition()*omrekenfactor2 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) {
                 batjeset = 0;
             } else {
                 batjeset++;
@@ -665,7 +676,7 @@
             batjeset = integral = derivative = previouserror = 0;
             wait(1);
             direction = force = 0;
-            goto directionchoice;
+            goto motor1cal;
         }
     }
 } // end main
\ No newline at end of file