Diff: main.cpp

Revision:

5:4842219cb77c

Parent:

4:377ddd65e4a6

Child:

6:a7379a681adf

--- a/main.cpp	Thu Oct 30 12:59:47 2014 +0000
+++ b/main.cpp	Fri Oct 31 08:36:46 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 (100)
-#define K_I (50 * TSAMP)
-#define K_D (1)
+#define K_P (5)
+#define K_I (0.1 * TSAMP)
+#define K_D (0)
 //#define K_D (0.0005 /TSAMP)
 #define I_LIMIT 100.
 #define PI 3.1415926535897
@@ -70,6 +70,7 @@
 int beginpos_motor2_new;
 int previous_pos_motor1 = 0;
 int current_pos_motor1;
+int EMG = 1;
 int delta_pos_motor1_puls;
 void clamp(float * in, float min, float max);
 volatile bool looptimerflag;
@@ -123,10 +124,11 @@
 }
 
 void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
+    
     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)
+    //delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls)
     pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
-    delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
+    //delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
     //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
     //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen
     //scope.set(0, snelheid_motor1_rad);
@@ -134,26 +136,30 @@
     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
     
     //nu gaan we positie regelen i.p.v. snelheid.
-    pc.printf("%f\n\r",pos_motor1_rad - position_setpoint_rad);
-    if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.2) //if position error < ...rad, then stop.
+    
+    if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop.
     {
         speed_radpersecond = 0; 
         setpoint = pos_motor1_rad;
         current_herhalingen = previous_herhalingen + 1; 
         previous_herhalingen = current_herhalingen;
         pc.printf("stop\n\r");
+        PWM1_percentage = 0;
     }
     else if(pos_motor1_rad - position_setpoint_rad < 0)
     {    
-        setpoint = prev_setpoint +( TSAMP * speed_radpersecond);       
+        setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
+        PWM1_percentage = pid(setpoint, pos_motor1_rad);       
     }
     else
     {
-        setpoint = prev_setpoint -( TSAMP * speed_radpersecond);       
+        setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
+        PWM1_percentage = pid(setpoint, pos_motor1_rad);       
     }
     /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
+    pc.printf("%f\n\r",PWM1_percentage);
     
-    PWM1_percentage = pid(setpoint, pos_motor1_rad);
+    
     //scope.set(1, setpoint-pos_motor1_rad); 
     
     if (PWM1_percentage < -100)
@@ -214,11 +220,12 @@
         case RUST: {
             rust();
             /*voorwaarde wanneer hij door kan naar de volgende case*/
-            if (current_herhalingen == 600) 
+            if (current_herhalingen == 100 && EMG == 1) 
             {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 state = ARM_KALIBRATIE;
+                EMG = 0;
             }
             break;
             /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/
@@ -233,7 +240,7 @@
         case ARM_KALIBRATIE: 
         {
             arm_kalibratie();
-            if (current_herhalingen == 200) 
+            if (current_herhalingen == 100) 
             {
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
@@ -249,7 +256,7 @@
         case EMG_KALIBRATIE: 
         {
             emg_kalibratie();
-            if (current_herhalingen == 200) 
+            if (current_herhalingen == 100) 
             {
                 state = METEN_RICHTING;
                 current_herhalingen = 0;
@@ -261,7 +268,7 @@
         case METEN_RICHTING: 
         {
             meten_richting();
-            if (current_herhalingen == 200) 
+            if (current_herhalingen == 100) 
             {
                 state = METEN_HOOGTE;
                 current_herhalingen = 0;
@@ -273,7 +280,7 @@
         case METEN_HOOGTE: 
         {
             meten_hoogte();
-            if (current_herhalingen == 200) 
+            if (current_herhalingen == 100) 
             {
                 state = INSTELLEN_RICHTING;
                 current_herhalingen = 0;
@@ -285,7 +292,7 @@
         case INSTELLEN_RICHTING: 
         {
             instellen_richting();
-            if (current_herhalingen == 200) 
+            if (current_herhalingen == 100) 
             {
                 state = SLAAN;
                 current_herhalingen = 0;
@@ -297,8 +304,8 @@
 
         case SLAAN: 
         {
-            GotoPosition(0.7,4);
-            if (current_herhalingen == 200) 
+            GotoPosition(1.5 ,8);
+            if (current_herhalingen == 100) 
             {
                 state = RETURN2RUST;
                 current_herhalingen = 0;
@@ -312,14 +319,14 @@
 
         case RETURN2RUST: 
         {
-            GotoPosition(0,1);
-            /*if (current_herhalingen == 200) 
+            GotoPosition(0,2);
+            if (current_herhalingen == 100) 
             {
                 state = RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
             }
-            */
+            
             break;
         }
         
@@ -335,7 +342,7 @@
     if(state==RUST){
         lcd.cls(); 
         lcd.locate(0,0);
-        lcd.printf("S.T.I.E.N.E.N.");   //regel 1 LCD scherm
+        lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
         lcd.locate(0,1);
         lcd.printf("  GROEP 7   ");
     }