Diff: main.cpp

Revision:

7:f016dd2e8ea9

Parent:

6:565b4fc42323

--- a/main.cpp	Fri Oct 31 11:23:37 2014 +0000
+++ b/main.cpp	Fri Oct 31 14:45:43 2014 +0000
@@ -32,9 +32,9 @@
 #define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
 #define TSAMP 0.005
-#define K_P (5)
-#define K_I (0.1 * TSAMP)
-#define K_D (0)
+#define K_P (80000)
+#define K_I (0.01)
+#define K_D (0.01)
 //#define K_D (0.0005 /TSAMP)
 #define I_LIMIT 100.
 #define PI 3.1415926535897
@@ -89,10 +89,7 @@
 float Speed_motor1rad;
 float setpoint = 0;
 float prev_setpoint = 0; 
-float hoek_gewenst = 0; 
-float hoek_nul = 0; 
-float snelheid_slaan = 8; 
-float snelheid_naar_nul = 3;
+float marge = 0; 
 
 //HIDScope scope(6);
 
@@ -127,7 +124,7 @@
     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
 }
 
-void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
+void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge) {
     
     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
     //delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls)
@@ -141,7 +138,7 @@
     
     //nu gaan we positie regelen i.p.v. snelheid.
     
-    if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop.
+    if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) //if position error < ...rad, then stop.
     {
         speed_radpersecond = 0; 
         setpoint = pos_motor1_rad;
@@ -161,7 +158,7 @@
         PWM1_percentage = pid(setpoint, pos_motor1_rad);       
     }
     /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
-    pc.printf("%f\n\r",PWM1_percentage);
+    pc.printf("%d\n\r",current_pos_motor1);
     
     
     //scope.set(1, setpoint-pos_motor1_rad); 
@@ -298,17 +295,17 @@
             instellen_richting();
             if (current_herhalingen == 100) 
             {
-                state = SLAAN;
+                state = RETURN2RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
             }
-        break; 
+            break; 
             
         }
 
         case SLAAN: 
         {
-            GotoPosition(hoek_gewenst , snelheid_slaan);
+            GotoPosition(1.5 ,8, 0.1);
             if (current_herhalingen == 100) 
             {
                 state = RETURN2RUST;
@@ -323,13 +320,13 @@
 
         case RETURN2RUST: 
         {
-            GotoPosition(hoek_nul, snelheid_naar_nul);
-            if (current_herhalingen == 100) 
-            {
-                state = RUST;
-                current_herhalingen = 0;
-                previous_herhalingen = 0;
-            }
+            GotoPosition(0,4, 0.05);
+            //if (current_herhalingen == 100) 
+            //{
+            //    state = RUST;
+            //    current_herhalingen = 0;
+            //    previous_herhalingen = 0;
+            //}
             
             break;
         }