Revision 4:377ddd65e4a6, committed 2014-10-30

Comitter:

phgbartels

Date:

Thu Oct 30 12:59:47 2014 +0000

Parent:

3:156c3e536ed4

Child:

5:4842219cb77c

Commit message:

first setup of generic speed / position controller;

Changed in this revision

--- a/main.cpp	Thu Oct 30 12:21:43 2014 +0000
+++ b/main.cpp	Thu Oct 30 12:59:47 2014 +0000
@@ -52,7 +52,7 @@
 DigitalOut motordir2(M2_DIR); 
 DigitalOut LEDGREEN(LED_GREEN);
 DigitalOut LEDRED(LED_RED); 
-
+Serial pc(USBTX,USBRX);
 /*
 definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
 */
@@ -134,17 +134,25 @@
     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
     
     //nu gaan we positie regelen i.p.v. snelheid.
-    if (abs(pos_motor1_rad - position_setpoint_rad) <= 0.1) //if position error < ...rad, then stop.
+    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.
     {
         speed_radpersecond = 0; 
+        setpoint = pos_motor1_rad;
         current_herhalingen = previous_herhalingen + 1; 
         previous_herhalingen = current_herhalingen;
+        pc.printf("stop\n\r");
     }
-    else if(pos_motor1_rad - position_setpoint_rad > 0)    
-        setpoint = prev_setpoint + TSAMP * speed_radpersecond;       
+    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); 
     
@@ -157,7 +165,7 @@
         PWM1_percentage =100;
     }
     
-    if(PWM1_percentage > 0)
+    if(PWM1_percentage < 0)
     {
         motordir1 = 1;
     }
@@ -166,7 +174,7 @@
         motordir1 = 0;
     }
         
-    pwm_motor1.write(PWM1_percentage/100.);//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
+    pwm_motor1.write(abs(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();  
@@ -338,6 +346,8 @@
 }
 
 int main() {
-    statemachine.attach(&statemachinefunction, 0.005); // the address of the function to be attached (flip) and the interval (2 seconds)
+    pc.baud(115200);
+    statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
     screen.attach(&screenupdate, 0.2);
+    while(1);
 }
\ No newline at end of file