Diff: main.cpp

Revision:

57:4b682f92ab51

Parent:

56:29c31b83c4ce

Child:

58:9a56581d5298

--- a/main.cpp	Mon Nov 03 15:20:07 2014 +0000
+++ b/main.cpp	Mon Nov 03 18:06:35 2014 +0000
@@ -6,14 +6,14 @@
 #include "TouchButton.h"
 
 #define K_P (0.5)
-#define K_I (0.02  *TSAMP1)
+#define K_I (0.0  *TSAMP1)
 #define K_D (0  /TSAMP1)
 #define I_LIMIT 1.
 #define K_Pp (0.003) //-------------------------------Deze drie waarden moeten anders!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 #define K_Ip (0.0  *TSAMP2)//-----------------------------TSAMP1 veranderd naar TSAMP2
 #define K_Dp (0.0000003  /TSAMP2)//--------------------------------TSAMP1 veranderd naar TSAMP2
 
-#define TSAMP1 0.025
+#define TSAMP1 0.01
 #define TSAMP2 0.01
 #define WACHTEN 1
 #define SLAAN 2
@@ -95,6 +95,9 @@
 DigitalOut motordir2(PTC9);
 PwmOut pwm_motor1(PTA5);
 PwmOut pwm_motor2(PTC8);
+float prevgetpos=0;
+float omega;
+float deltacounts;
 
 DigitalOut myled1(LED1);//red
 DigitalOut myled2(LED2);//green
@@ -340,9 +343,7 @@
             //*** INPUT MOTOR 2 ***
             positie=yT1+yT2; */
 
-            positie=2;
-
-            //controle positie op scherm
+            /* //controle positie op scherm
             if (positie==0) {
                 pc.printf("Motor 2 blijft op stand 1\n");
             } else {
@@ -360,8 +361,9 @@
             wait(8);
             looptimer2.detach();
             pc.printf("Detach Motor 1\n");
+            */
 
-    //        ------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
+            //        ------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
             wait(2);
             /* Ticker log_timerB;
 
@@ -405,7 +407,7 @@
             //*** INPUT MOTOR 1 ***
             snelheidsstand=yB1+yB2+yB3; */
 
-            /*snelheidsstand=3;
+            snelheidsstand=3;
 
             //controle snelheidsstand op scherm
             if (snelheidsstand==0) {
@@ -444,7 +446,7 @@
             pidpositie(0,0,true);
             pwm_motor1.write(0);
             toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!
-            */
+
 
             myled1=1;
             myled2=1;
@@ -460,6 +462,8 @@
 
 float pid(float setspeed, float measurement, bool reset )
 {
+
+
     float error;
     static float prev_error = 0;
     float           out_p = 0;
@@ -493,7 +497,7 @@
     out_i += error*K_Ip;
     out_d  = (error-prev_error)*K_Dp;
     prev_error = error;
-    return abs(out_p + out_i + out_d);
+    return out_p + out_i + out_d;
 }
 
 void Triceps()
@@ -698,10 +702,14 @@
 
 void motor1aansturing()
 {
+    deltacounts = motor1.getPosition()- prevgetpos;
+    prevgetpos=motor1.getPosition();
+    omega=(deltacounts/TSAMP1);
+
     switch(toestand) {
         case SLAAN:
             pc.printf("SLAAN\n");
-            new_pwm = pid(setspeed, motor1.getSpeed());
+            new_pwm = pid(setspeed, omega);
             pwm_motor1.write(new_pwm);
             motordir1 = 1;
             if (motor1.getPosition() <= ANGLEMAX) {
@@ -751,34 +759,37 @@
                 }
                 break;*///overbodig!!!
     } //end switch
+
     scope.set(0,motor1.getPosition());
     scope.set(1,ANGLEMAX);
-    scope.set(4,motor1.getSpeed());
-    scope.set(5, setspeed);
+    scope.set(3,omega);
+    scope.set(4, setspeed);
     scope.send();
 }
 
 void motor1aansturingdeel2()
 {
+    deltacounts = motor1.getPosition()- prevgetpos;
+    prevgetpos=motor1.getPosition();
+    omega=(deltacounts/TSAMP1);
+    
     switch(toestand) {
         case TERUGKEREN: //deze case moet blijven ookal is het de enige case
-            if (motor1.getPosition()<=ANGLEMIN) {
-                new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());//new_PWM benaming zorgt mogelijk voor problemen.
-                pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
+            new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());//new_PWM benaming zorgt mogelijk voor problemen
+            if (new_pwmpos > 0) {
                 motordir1 = 0;
                 //pc.printf("motor2.getPosition %d\r\n", motor2.getPosition());
-            }
-            if (motor1.getPosition()>= ANGLEMIN) {
-                new_pwmpos = pidpositie(ANGLEMIN, motor1.getPosition());
-                pwm_motor1.write(new_pwmpos); //mogelijk moet dit -new_pwm zijn???
+            } else {
                 motordir1 = 1;
                 //pc.printf("if2\n");
             }
+            pwm_motor1.write(fabs(new_pwmpos)); //mogelijk moet dit -new_pwm zijn???
             break;
     } //end switch
+
     scope.set(0,motor1.getPosition());        //ruwe data
     scope.set(2,ANGLEMIN);
-    scope.set(4,motor1.getSpeed());
-    scope.set(5, setspeed);
+    scope.set(3, omega);
+    scope.set(4, setspeed);
     scope.send();
 }//let op. Geen pidposition(0,0,true) deze moet zelf zorgen dat hij 0 wordt, en daar genoeg tijd voor hebben!!!
\ No newline at end of file