Diff: main.cpp

Revision:

3:156c3e536ed4

Parent:

2:de7b6c1d67c4

Child:

4:377ddd65e4a6

--- a/main.cpp	Thu Oct 30 11:51:21 2014 +0000
+++ b/main.cpp	Thu Oct 30 12:21:43 2014 +0000
@@ -122,7 +122,7 @@
     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
 }
 
-void slaan () {
+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)
     pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
@@ -134,14 +134,16 @@
     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 >= 400)
+    if (abs(pos_motor1_rad - position_setpoint_rad) <= 0.1) //if position error < ...rad, then stop.
     {
-        gewenste_snelheid_rad = 0; 
+        speed_radpersecond = 0; 
         current_herhalingen = previous_herhalingen + 1; 
         previous_herhalingen = current_herhalingen;
     }
-        
-    setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad;       
+    else if(pos_motor1_rad - position_setpoint_rad > 0)    
+        setpoint = prev_setpoint + TSAMP * speed_radpersecond;       
+    else
+        setpoint = prev_setpoint -( TSAMP * speed_radpersecond);       
     /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
     PWM1_percentage = pid(setpoint, pos_motor1_rad);
     //scope.set(1, setpoint-pos_motor1_rad); 
@@ -154,7 +156,6 @@
     {
         PWM1_percentage =100;
     }
-    else {}
     
     if(PWM1_percentage > 0)
     {
@@ -165,59 +166,13 @@
         motordir1 = 0;
     }
         
-    pwm_motor1.write(abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
+    pwm_motor1.write(PWM1_percentage/100.);//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();  
 }
 
-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)
 {
@@ -334,7 +289,7 @@
 
         case SLAAN: 
         {
-            slaan();
+            GotoPosition(0.7,4);
             if (current_herhalingen == 200) 
             {
                 state = RETURN2RUST;
@@ -349,7 +304,7 @@
 
         case RETURN2RUST: 
         {
-            return2rust();
+            GotoPosition(0,1);
             /*if (current_herhalingen == 200) 
             {
                 state = RUST;