Diff: TiltVelocityLoop.cpp

Revision:

4:4dafa4113982

Parent:

3:f8a5c1cee1fa

--- a/TiltVelocityLoop.cpp	Wed May 27 10:07:39 2015 +0000
+++ b/TiltVelocityLoop.cpp	Thu Jun 25 09:41:26 2015 +0000
@@ -31,7 +31,7 @@
 
 // System Configuration:
 float MaxSpeed = 60.00; // 5 Deg/s, 1250 RPM
-float Tilt_motor_max_count_rate = 5461; //encoder counts / ms
+float Tilt_motor_max_count_rate = 6000; //encoder counts / ms
 float T_Position = 0; // True Tilt Position (Degrees)
 float T_Encoder_sf = 1456.355;  // counts per degree
 float T_Gear_ratio = 125;  // 125:1
@@ -39,6 +39,8 @@
 float Motor_sf = ((1 * T_Gear_ratio) * (M_Encoder / 360))/1000 ;  // This is the scale factor from deg/s at payload to motor counts/ms
 bool M_Encoder_front = true; // or 0 depending on the encoder configuration on the motor (facing to motor, or facing away from motor)
 
+double drive = 0;  // this is a const speed demand straight into the loop (divided by 100)
+
 // Main servo loop
 int DoMove = 0;
 const float LOOPs = 0.001;
@@ -49,7 +51,7 @@
 float ta; // The actual value used after sanity checks
 float ts; // The actual value used after sanity checks(S ramping)
 float tsfade = 0.5; // segment time for S ramped fade
-float tscut = 0.2; // segment time for S ramped cut
+float tscut = 0.3; // segment time for S ramped cut
 float j; // jerk value for fade
 float aj; // accel value when S ramping
 float Vp;  // Top speed for the move Deg/s @ load (125:1 Ratio to motor)
@@ -59,11 +61,11 @@
 float s;  // Profiler internal demand speed (always positive)
 float sout;  // Demand as applied by the Vff term
 float s_profile; // output demand speed to postion loop + or -)
-float P;  // Profiler Demand postion
+double P;  // Profiler Demand postion
 float fadetime; // this will retain the current fade time
 float Error;    // Current position vs the profiler position
 float Vff = 1;  // Velocity feedforward term - a value of 1 sends 100% profiler speed demand to motor
-float T_Kp = 8;  //  This is is multiplied by the position error and added to the motor demand
+float T_Kp = 6;  //  This is is multiplied by the position error and added to the motor demand
 float Prop;   // The demand created by the Kp and error calculation
 float demand = 0;  // The value sento to the motor to make it move
 float P_vel;
@@ -141,11 +143,12 @@
             }
             // compute the new position demand:
             s_profile = s * dir;
-            P = P + (s_profile * LOOPs);
+            P = P + ((s_profile * LOOPs));
             real_time = ((T - fadetime) * 1000);
         } else {  // If not fading then respect joystick profiler instead
 
             P = P + (T_Joy * LOOPs);  // Sum total of Joystick profiler right now.
+            P = P + (drive / 100000);  // Apply const speed drive
 
         }  // End of profilers
 
@@ -154,7 +157,7 @@
         Prop = T_Kp * Error;      // Calculate proportional gain element
         demand = sout + Prop;   // Sum the result of Vff and Kp to the demand
         P_vel = demand * Motor_sf; // Convert from load Deg/s into motor encoder counts/ms
-        TiltVelocityPID.setSetPoint(P_vel);
+        TiltVelocityPID.setSetPoint(-P_vel);
         //.printf("\n\r %f,   %f,   %f,   %f, %f",Time, s_profile, P_vel, T_Position, Error);
     }  // End of if !Joystick
 
@@ -196,15 +199,15 @@
 
     if (fade_time <= (6*tsfade + 0.2)) {
         ts = tscut;
-        T = fade_time;
+        if (fade_time <= (6*tscut+0.2)) {
+            T = 6*tscut + 0.2;  //min fade fime
+        }
+
     } else {
         ts = tsfade;
         T = fade_time;
     }
-    if (fade_time <= (6*tscut+0.2)) {
-        T = 6*tscut + 0.2;  //min fade fime
-    }
-
+   
     Vp = D / (T-(3*ts));  // Equation 1
     if (Vp > MaxSpeed) {         //Check for maximum speed condition
         Vp = MaxSpeed;           //Do the fade as fast as possible