Diff: TiltVelocityLoop.cpp

Revision:

3:f8a5c1cee1fa

Parent:

2:dc684c402296

Child:

4:4dafa4113982

--- a/TiltVelocityLoop.cpp	Wed May 27 07:13:54 2015 +0000
+++ b/TiltVelocityLoop.cpp	Wed May 27 10:07:39 2015 +0000
@@ -21,25 +21,28 @@
 extern float Actual_Motor_Speed;
 
 extern float T_Position; // True Tilt Position (Degrees)
-extern float T_sf;
+extern float T_Encoder_sf;
 
 
-int LastTiltPosition = 0;
-int Position = 0;
+int Last_M_Position = 0;
+int M_Position = 0;
 int Velocity = 0;
 float Duty_Cycle = 0.0;
 
-
-// S_Ramp Fade values
+// System Configuration:
 float MaxSpeed = 60.00; // 5 Deg/s, 1250 RPM
 float Tilt_motor_max_count_rate = 5461; //encoder counts / ms
-
 float T_Position = 0; // True Tilt Position (Degrees)
-float T_sf = 1456.355;  // counts per degree
+float T_Encoder_sf = 1456.355;  // counts per degree
+float T_Gear_ratio = 125;  // 125:1
+float M_Encoder = 262144; // counts/rev
+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)
 
 // Main servo loop
 int DoMove = 0;
 const float LOOPs = 0.001;
+// S_Ramp Fade values
 float D = 10; // Fade distance
 float T = 15; // Fade time
 float dir = 1; //direction flag
@@ -82,107 +85,99 @@
 
 void TiltVelocityLoop(void const *args)
 {
-    T_Position = 360 - (TiltPos.ReadPOSITION()/T_sf); // the 3D printed unit counts the opposite way to the aluminium unit.
-    
-    if (joystick){
+    T_Position = 360 - (TiltPos.ReadPOSITION()/T_Encoder_sf); // the 3D printed unit counts the opposite way to the aluminium unit.
+
+    if (joystick) {
         P = T_Position;
-        }
-    
-    Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
-    Velocity = Position - LastTiltPosition;             // Calculate change in position (i.e. Velocity)
+    }
+
+    M_Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
+    Velocity = M_Position - Last_M_Position;             // Calculate change in position (i.e. Velocity)
     Actual_Motor_Speed = Velocity;
 
 
     // Check to see if we have gone past the index point
-    if(Position < Lower & LastTiltPosition > Upper) {       // We have gone over the index point in 1 direction
+    if(M_Position < Lower & Last_M_Position > Upper) {       // We have gone over the index point in 1 direction
         Velocity += OneTurn;
-    } else if(Position > Upper & LastTiltPosition < Lower) {// We have gone over the index point in the other direction
+    } else if(M_Position > Upper & Last_M_Position < Lower) {// We have gone over the index point in the other direction
         Velocity -= OneTurn;
     }
-    LastTiltPosition = Position;                            // Update new position from next time
+    Last_M_Position = M_Position;                            // Update new position from next time
 
     TiltVelocityPID.setProcessValue(Velocity);
 
-    if (DoMove == 1) {
-        if ((fadetime < ts) & (s < Vp)) {
-            //led2 = 0;
-            s = (j/2)*fadetime*fadetime;  //bottom parabola
-            fadetime = fadetime + LOOPs; // This provides the base time for the fade sequence
-        } else if ((fadetime >= ts) & (fadetime <(2*ts))) {
-            s = (as*fadetime)+c; //steady accel stage
-            fadetime = fadetime + LOOPs;
-        } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
-            s = (-(j/2)*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vp; // Top parabola
-            fadetime = fadetime + LOOPs;
-        } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
-            s = Vp;  // Steady Speed Stage
-            fadetime = fadetime + LOOPs;
-        } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
-            s = (-(j/2)*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vp; // Top parabola down
-            fadetime = fadetime + LOOPs;
-        } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
-            s = -as*(fadetime - T) + c; //steady decel stage
-            fadetime = fadetime + LOOPs;
-        } else if ((fadetime >= (T-ts)) & (s < Vp) & (fadetime <= T)) {
-            //led2 = 1;
-            s = (j/2)*(T-fadetime)*(T-fadetime);  //bottom parabola to end
-            fadetime = fadetime + LOOPs;
-        } else if (fadetime >= T) {
-            s=0;
-            //led2 = 0;
-            DoMove = 0;
-            TiltVelocityPID.setSetPoint(0);
-        } else {
-            fadetime = fadetime + LOOPs; // for TBC reason this is needed!
-        }
-        if (DoMove==1) {
+    if(!joystick) {
+        if (DoMove == 1) {
+            if ((fadetime < ts) & (s < Vp)) {
+                //led2 = 0;
+                s = (j/2)*fadetime*fadetime;  //bottom parabola
+                fadetime = fadetime + LOOPs; // This provides the base time for the fade sequence
+            } else if ((fadetime >= ts) & (fadetime <(2*ts))) {
+                s = (as*fadetime)+c; //steady accel stage
+                fadetime = fadetime + LOOPs;
+            } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
+                s = (-(j/2)*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vp; // Top parabola
+                fadetime = fadetime + LOOPs;
+            } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
+                s = Vp;  // Steady Speed Stage
+                fadetime = fadetime + LOOPs;
+            } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
+                s = (-(j/2)*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vp; // Top parabola down
+                fadetime = fadetime + LOOPs;
+            } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
+                s = -as*(fadetime - T) + c; //steady decel stage
+                fadetime = fadetime + LOOPs;
+            } else if ((fadetime >= (T-ts)) & (s < Vp) & (fadetime <= T)) {
+                //led2 = 1;
+                s = (j/2)*(T-fadetime)*(T-fadetime);  //bottom parabola to end
+                fadetime = fadetime + LOOPs;
+            } else if (fadetime >= T) {
+                s=0;
+                //led2 = 0;
+                DoMove = 0;
+                TiltVelocityPID.setSetPoint(0);
+            } else {
+                fadetime = fadetime + LOOPs; // for TBC reason this is needed!
+            }
             // compute the new position demand:
             s_profile = s * dir;
             P = P + (s_profile * LOOPs);
             real_time = ((T - fadetime) * 1000);
+        } else {  // If not fading then respect joystick profiler instead
 
-            sout = s_profile * Vff;  //Apply velocity feedforward term
-            Error = (P - T_Position);    // Position Error
-            Prop = T_Kp * Error;      // Calculate proportional gain element
-            demand = sout + Prop;   // Sum the result of Vff and Kp to the demand
-            //This demand represents degrees/s @ the output shaft.
-            // Ratio is 125:1.  5461 couns/ms = 60 Deg/s @ output
-            // scalefactor is approx 91
-            P_vel = demand * 72.8;
-            TiltVelocityPID.setSetPoint(P_vel);
-            //.printf("\n\r %f,   %f,   %f,   %f, %f",Time, s_profile, P_vel, T_Position, Error);
-            //me = Time + LOOPs;
-        }
-    } else {
+            P = P + (T_Joy * LOOPs);  // Sum total of Joystick profiler right now.
+
+        }  // End of profilers
 
-        if(!joystick) {
-
-            P = P + (T_Joy * LOOPs);
-            sout = T_Joy * Vff;  //Apply velocity feedforward term
-            Error = (P - T_Position);    // Position Error
-            Prop = T_Kp * Error;      // Calculate proportional gain element
-            demand = sout + Prop;   // Sum the result of Vff and Kp to the demand
-            //This demand represents degrees/s @ the output shaft.
-            // Ratio is 125:1.  5461 couns/ms = 60 Deg/s @ output
-            // scalefactor is approx 91
-            P_vel = demand * 72.8;
-            TiltVelocityPID.setSetPoint(P_vel);
-        }
-
-    }
+        sout = s_profile * Vff;  //Apply velocity feedforward term
+        Error = (P - T_Position);    // Position Error
+        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);
+        //.printf("\n\r %f,   %f,   %f,   %f, %f",Time, s_profile, P_vel, T_Position, Error);
+    }  // End of if !Joystick
 
     Duty_Cycle = TiltVelocityPID.compute_ff()/Tilt_motor_max_count_rate;
 
     if(Duty_Cycle < 0) {
-        Tilt_Motor_Direction = 1;
+        if (M_Encoder_front) {
+            Tilt_Motor_Direction = 1;
+        } else {
+            Tilt_Motor_Direction = 0;
+        }
         Tilt_Motor_PWM = 1 - (Duty_Cycle * -1.0);
     } else {
-        Tilt_Motor_Direction = 0;
+        if (M_Encoder_front) {
+            Tilt_Motor_Direction = 0;
+        } else {
+            Tilt_Motor_Direction = 1;
+        }
         Tilt_Motor_PWM = 1 - Duty_Cycle;
     }
 }
 
-void Profile() // For S ramped movement using Servo for S ramping
+void Profile() // For S ramped movement using Servo for S ramping. Calling here as all it's variables are needed in this arena
 {
     if ((fade_tilt >=0) & (fade_tilt <= 359)) {
         D = fade_tilt - T_Position; // Calculate distance to move
@@ -223,7 +218,7 @@
     c = -(Vp / 4);
     s = 0;
     fadetime = 0;
-   // Time = 0;
+    // Time = 0;
     P = T_Position;
 
 }