Revision 2:4173f8896073, committed 2015-04-15

Comitter:

acodd

Date:

Wed Apr 15 15:43:16 2015 +0000

Parent:

1:120eeae4d43a

Commit message:

Updated 15/04/2015

Changed in this revision

--- a/main.cpp	Wed Feb 04 10:35:25 2015 +0000
+++ b/main.cpp	Wed Apr 15 15:43:16 2015 +0000
@@ -1,7 +1,8 @@
-/*  Version 3.0, Opened 4/03/2013
+/*  Version 2.7, Opened 4/03/2015
 This Program closes a servo loop on the FHR35 Pan axis. This is using an SSI encoder for
 Position feedback and a Copley Amplifier driven in Velocity mode.*/
 /*
+0. Created around 04/03/2013
 1. Basic Profiler working
 2. Added SSI encoder support
 3. Added Proportional Gain
@@ -12,6 +13,8 @@
 8. Added error traps for max fade speed and keep within 0-360 for fade demand. Reports IP Address on LCD on startup
 9. Added S Ramping Code 8 July 2013
 10. Added proper Vff and control of Vff and Kp from parent control, July 2013
+11. Improved S Ramp according to Diogo's V2 document - previous maths was not working, April 2015
+12. Added Tilt Axis as virtual axis for sync and other testing, 13/04/2015
 */
 #include "mbed.h"
 #include "TextLCD.h"
@@ -60,7 +63,7 @@
 typedef struct _data2 {
     float pan;
     float tilt;
-    int time;
+    float time;
 } data2;
 data2 fade;
 data2 joy;
@@ -101,7 +104,7 @@
 float j; // jerk value for fade
 float aj; // accel value when S ramping
 float tacut = 1; // accel time for a cut
-float Vt;  // Top speed for the move Deg/s @ load (256:1 Ratio to motor)
+float Vp;  // Top speed for the move Deg/s @ load (256:1 Ratio to motor)
 float Vs;  // Speed step increment
 float Da;  // Accel distance
 float Ds; // Distance convered at steady speed
@@ -138,47 +141,7 @@
     }
     led4 = 1;
 }
-void Profile_Trap()
-{
-    if ((fade.pan >=0) & (fade.pan <= 359)) {
-        D = fade.pan - Angle; // Calculate distance to move
-    } else {
-        D = 0;
-        abort();  // leave this function
-        // add an error event handler here
-    }
 
-    if (D <= 0) {
-        dir = -1;
-        D = abs(D);
-    } else {
-        dir = 1;
-    }
-
-    if (fade.time <= (2*tafade + 0.2)) {
-        ta = tacut;
-        T = fade.time;
-    } else {
-        ta = tafade;
-        T = fade.time;
-    }
-    if (fade.time <= (2*tacut+0.2)) {
-        T = 2*tacut + 0.2;  //min fade fime is 2xcut acceleration for now
-    }
-
-    Vt = D / (T-ta);
-    if (Vt > MaxSpeed) {         //Check for maximum speed condition
-        Vt = MaxSpeed;           //Do the fade as fast as possible
-        T = (D + (Vt * ta))/Vt;
-    }
-    Da = (Vt/2)*ta;
-    Ds = Vt * (T - (2*ta));
-    Steps = (ta*1000) / LOOPms;
-    Vs = Vt / Steps;
-    s = 0;
-    fadetime = 0;
-    //P = 0; //Angle;
-}
 void Profile() // For S ramped movement using Servo for S ramping
 {
     if ((fade.pan >=0) & (fade.pan <= 359)) {
@@ -203,30 +166,21 @@
         ts = tsfade;
         T = fade.time;
     }
-    if (fade.time <= (2*tscut+0.2)) {
+    if (fade.time <= (6*tscut+0.2)) {
         T = 6*tscut + 0.2;  //min fade fime
     }
 
-    Vt = D / (T-(3*ts));  // Equation 1
-    if (Vt > MaxSpeed) {         //Check for maximum speed condition
-        Vt = MaxSpeed;           //Do the fade as fast as possible
-        T = (D + (Vt * (3*ts)))/Vt;
+    Vp = D / (T-(3*ts));  // Equation 1
+    if (Vp > MaxSpeed) {         //Check for maximum speed condition
+        Vp = MaxSpeed;           //Do the fade as fast as possible
+        T = (D + (Vp * (3*ts)))/Vp;
     }
-    // change more here
-    Va =(Vt / 2);  // Equation 2
-
-    j = ( ((2*Va)-(1.5*ts))/(3*ts*ts));  // Eqation 3
-
-    Vj = (j * ts * ts);  // Equation 5
-
-    Vjp = Vt - Vj; // Equation 6
-
-    as = (Vjp-Vj)/ts; // Equation 4
-
-    c = Vj - (as*ts); // Equation 7
-
-    b = Vj - (-as*(T-ts)); // Equation 8
-
+    
+    // New version based on S-Ramping Doc - V2
+    
+    j = Vp / (2*ts*ts);
+    as = j * ts;
+    c = -(Vp / 4);
     s = 0;
     fadetime = 0;
     //P = 0; //Angle;
@@ -284,6 +238,7 @@
                 fade.tilt = tilt;
                 fade.tilt = fade.tilt/1000;
                 fade.time = duration;
+                fade.time = fade.time/1000;
                 sprintf(header, "%s", "Fade");
                 Profile();
                 DoMove = 1;
@@ -404,85 +359,34 @@
     wait(0.1);
 }
 
-
-void Servo_Trap(void const *args)  //This is threaded with real-time priority.
-{
-    while(true) {
-        ReadEncoder();
-        if ((DoMove == 1) & (Enable != 0) & (Stopped !=1)) {
-            if ((fadetime < ta) & (s < Vt)) {
-                led2 = 1;
-                s = s + Vs;
-                fadetime = fadetime + 0.005; // This provides the base time for the fade sequence
-            } else if ((fadetime >= ta) & (fadetime <(T-ta))) {
-                s = Vt;
-                fadetime = fadetime + 0.005;
-            } else if ((fadetime >= (T-ta)) & (fadetime < T)) {
-                s = s-Vs;
-                fadetime = fadetime + 0.005;
-            } else if (fadetime >= T) {
-                s=0;
-                led2 = 0;
-                DoMove = 0;
-            } else {
-                fadetime = fadetime + 0.005; // for TBC reason this is needed!
-            }
-            // compute the new position demand:
-            s_profile = s * dir;
-            P = P + (s_profile * 0.005);
-            real.time = ((T - fadetime) * 1000);
-        } else {
-            DoMove = 0;
-            real.time = 0;
-        }   // end of DoMove (Fade or Cut
-
-        if ((Enable !=0) & (Stopped !=1)) {  //This executes the core postion loop.
-            if (DoMove == 0) {               //Adding Joystick Demand as appropriate
-                s_profile = MaxSpeed * (joy.pan/100);
-                P = P + (s_profile * 0.005);
-            } else {
-                joy.pan = 0;
-                joy.tilt = 0;
-            }
-            Error = (P - Angle);
-            Prop = Kp * Error;
-            demand = s_profile+Prop;
-            DriveMotor(demand);
-        } else {
-            DriveMotor(0);
-        }  //End of core position loop
-        Thread::wait(5);
-    }
-}
-
 void Servo(void const *args)  //This is threaded with real-time priority.
 // This is S ramped servo control based on the S ramp Profile calculations
 {
     while(true) {
         ReadEncoder();
         if ((DoMove == 1) & (Enable != 0) & (Stopped !=1)) {
-            if ((fadetime < ts) & (s < Vt)) {
+            if ((fadetime < ts) & (s < Vp)) {
                 led2 = 0;
-                s = j*fadetime*fadetime;  //bottom parabola
+                s = (j/2)*fadetime*fadetime;  //bottom parabola
                 fadetime = fadetime + 0.005; // 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 + 0.005;
             } else if ((fadetime >= (2*ts)) & (fadetime <(3*ts))) {
-                s = (-j*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vt; // Top parabola
+                s = (-(j/2)*(fadetime-(3*ts))*(fadetime-(3*ts))) + Vp; // Top parabola
                 fadetime = fadetime + 0.005;
             } else if ((fadetime >= (3*ts)) & (fadetime <(T-(3*ts)))) {
-                s = Vt;  // Steady Speed Stage
+                s = Vp;  // Steady Speed Stage
                 fadetime = fadetime + 0.005;
             } else if ((fadetime >= (T-(3*ts))) & (fadetime <(T-(2*ts)))) {
-                s = (-j*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vt; // Top parabola down
+                s = (-(j/2)*(fadetime-(T-(3*ts)))*(fadetime-(T-(3*ts)))) + Vp; // Top parabola down
                 fadetime = fadetime + 0.005;
             } else if ((fadetime >= (T-ts-ts)) & (fadetime < (T-ts))) {
-                s = (-as*fadetime)+b; //steady decel stage
+                s = -as*(fadetime - T) + c; //steady decel stage
                 fadetime = fadetime + 0.005;
-            } else if ((fadetime >= (T-ts)) & (s < Vt) & (fadetime <= T)) {
+            } else if ((fadetime >= (T-ts)) & (s < Vp) & (fadetime <= T)) {
                 led2 = 1;
-                s = j*(T-fadetime)*(T-fadetime);  //bottom parabola to end
+                s = (j/2)*(T-fadetime)*(T-fadetime);  //bottom parabola to end
                 fadetime = fadetime + 0.005;
             } else if (fadetime >= T) {
                 s=0;
@@ -527,7 +431,7 @@
     Enable = 0;    // Try to prevent movement on startup
     DriveMotor(0); // Drive demand to Zero
     lcd.cls();
-    lcd.printf("2.5: Connecting");
+    lcd.printf("2.7: Connecting");
 
 
     pc.baud(115200);            //Pimp the baud rate
@@ -554,35 +458,8 @@
     wait(2);
 
     while(1) {
-        //  if (Run == 1) {
-        //     wait(0.01); //de-bounce
-        //    if (Run == 1) {
-
-        //        DoMove = 1;
-        //   }
-        // }
+  
     }
 }
-/*
-FILE *fp = fopen("/local/log.txt", "a");  // Open "log.txt" on the local file system for append
-    fprintf(fp,"Angle %f\r\n",Angle); //
-    fclose(fp);
-        wait(2);
-    ReadEncoder();
-      fopen("/local/log.txt", "a");  // Open "log.txt" on the local file system for append
-    fprintf(fp,"Angle %f\r\n",Angle); //
-    fclose(fp);
-void ReadFile (void) {
-    FILE *set = fopen("/local/setup.txt", "r");  // Open "setup.txt" on the local file system for read
-    fscanf(set,"%s %d",A,&offset); // read offset
-    fscanf(set,"%s %f",B,&gain);   // read gain
-    fclose(set);
-}
-void WriteFile(void) { // write to USB memory
-    FILE *fp = fopen("/local/setup.txt", "a");  // Open "setup.txt" on the local file system for append
-    fprintf(fp,"offset %d\r\n",Angle); //
-    fprintf(fp,"gain %f\r\n",s);   //
-    fclose(fp);
-}
-*/
 
+