Diff: main.cpp

Revision:

2:de7b6c1d67c4

Parent:

1:b08ac32d1ddc

Child:

3:156c3e536ed4

--- a/main.cpp	Thu Oct 30 09:00:36 2014 +0000
+++ b/main.cpp	Thu Oct 30 11:51:21 2014 +0000
@@ -73,8 +73,9 @@
 int delta_pos_motor1_puls;
 void clamp(float * in, float min, float max);
 volatile bool looptimerflag;
-uint16_t gewenste_snelheid = 2;
-uint16_t gewenste_snelheid_rad = 4; 
+int16_t gewenste_snelheid = 2;
+int16_t gewenste_snelheid_rad = 4; 
+int16_t gewenste_snelheid_rad_return = 1;
 float pid(float setpoint, float measurement);
 float pos_motor1_rad;
 float PWM1_percentage = 0;
@@ -126,8 +127,8 @@
     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)
-    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
+    //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);
     
     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
@@ -136,6 +137,8 @@
     if (current_pos_motor1 >= 400)
     {
         gewenste_snelheid_rad = 0; 
+        current_herhalingen = previous_herhalingen + 1; 
+        previous_herhalingen = current_herhalingen;
     }
         
     setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad;       
@@ -153,7 +156,7 @@
     }
     else {}
     
-    if(PWM1_percentage < 0)
+    if(PWM1_percentage > 0)
     {
         motordir1 = 1;
     }
@@ -165,7 +168,55 @@
     pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
     //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
     prev_setpoint = setpoint;
-    //scope.send();   
+    //scope.send();  
+}
+
+void return2rust () {
+    current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+    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)
+    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)
+    //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);
+    
+    previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
+    
+    //nu gaan we positie regelen i.p.v. snelheid.
+    if (current_pos_motor1 <= 0)
+    {
+        gewenste_snelheid_rad_return = 0;
+    }
+        
+    setpoint = prev_setpoint + (TSAMP * gewenste_snelheid_rad_return);       
+    /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
+    PWM1_percentage = pid(setpoint, pos_motor1_rad);
+    //scope.set(1, setpoint-pos_motor1_rad); 
+    
+    if (PWM1_percentage < -100)
+    {
+        PWM1_percentage = -100;
+    }
+    else if (PWM1_percentage >100)
+    {
+        PWM1_percentage =100;
+    }
+    else {}
+    
+    if(PWM1_percentage > 0)
+    {
+        motordir1 = 1;
+    }
+    else
+    {
+        motordir1 = 0;
+    }
+        
+    pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
+    //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
+    prev_setpoint = setpoint;
+    //scope.send(); 
 }
 
 float pid(float setpoint, float measurement)
@@ -193,9 +244,6 @@
     *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
     // *in = het getal dat staat op locatie van in --> waarde van new_pwm
 }
-
-void return2rust () {
-}
     
 void statemachinefunction()
 {
@@ -227,6 +275,10 @@
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
                 state = EMG_KALIBRATIE;
+                motor1.setPosition(0);
+                motor2.setPosition(0);
+                pwm_motor1.period_us(100);
+                pwm_motor2.period_us(100);
             }
             break;
         }
@@ -276,6 +328,7 @@
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
             }
+        break; 
             
         }
 
@@ -287,19 +340,23 @@
                 state = RETURN2RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
+                prev_setpoint =0; 
+                setpoint =0;
+                
             }
-            break;
+        break;    
         }
 
         case RETURN2RUST: 
         {
             return2rust();
-            if (current_herhalingen == 200) 
+            /*if (current_herhalingen == 200) 
             {
                 state = RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
             }
+            */
             break;
         }
         
@@ -326,10 +383,6 @@
 }
 
 int main() {
-    motor1.setPosition(0);
-    motor2.setPosition(0);
-    pwm_motor1.period_us(100);
-    pwm_motor2.period_us(100);
     statemachine.attach(&statemachinefunction, 0.005); // the address of the function to be attached (flip) and the interval (2 seconds)
     screen.attach(&screenupdate, 0.2);
 }
\ No newline at end of file