Revision 2:dc684c402296, committed 2015-05-27

Comitter:

acodd

Date:

Wed May 27 07:13:54 2015 +0000

Parent:

1:1ad84260ff59

Child:

3:f8a5c1cee1fa

Commit message:

First version of a full control loop for the T3 head
;

Changed in this revision

--- a/Classic_PID.lib	Mon May 18 09:06:07 2015 +0000
+++ b/Classic_PID.lib	Wed May 27 07:13:54 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Vitec-Group/code/Classic_PID/#7b42de70b65f
+http://developer.mbed.org/teams/Vitec-Group/code/Classic_PID/#b9ae2b80dfcd

--- a/ServiceKeyboard.cpp	Mon May 18 09:06:07 2015 +0000
+++ b/ServiceKeyboard.cpp	Wed May 27 07:13:54 2015 +0000
@@ -6,8 +6,23 @@
 extern Serial pc;
 extern Classic_PID PanVelocityPID;
 extern Classic_PID TiltVelocityPID;
+void Profile(void);
+extern float Demand_Count_Rate;
 
 extern bool joystick;
+extern bool scoping;
+
+extern float fade_tilt;
+extern float fade_time;
+extern float D;
+extern int DoMove;
+extern float T_Position;
+extern float T_Kp;
+extern float Vff;
+float setting;
+extern float  Time;
+
+extern float Joy_Zoom;
 
 // Returns the new set point entered via the keyboard
 int ServiceKeyboard ()
@@ -17,40 +32,55 @@
 
     key = pc.getc();                                    // Read the intial keypress
 
-    if (key == 'j') {
-        joystick = !joystick;
-        if(joystick) {
-            pc.printf("\n\r Under Joystick Control");
-        } else {
-            // Stop both axes
-            PanVelocityPID.setSetPoint(0);
-            TiltVelocityPID.setSetPoint(0);
-            pc.printf("\n\r Under PC Control");
-        }
-    } 
-    // Set Velocity for Pan and Tilt here...
-    else if(key == 'p') {
-        // We are going to set the speed of pan...
-        pc.printf("\n\r New Pan Vel: ");
-        key = pc.getc();
-        if (key == 0x1B) {
-            if(pc.getc() == 0x4F) {
-                if(pc.getc() == 0x53) {
-                    pc.printf("-");
-                    do {
-                        key = pc.getc();                            // Wait for a keypress
-                        if(key >= '0' && key <= '9') {              // Check it is a number
-                            value *= 10;                            // Index the old number
-                            value += (key - '0');                   // Add on the new number
-                            pc.printf("%c",key);                    // Display the new number
-                        }
-                    } while(key != 0x0D);
-                    PanVelocityPID.setSetPoint(value * -1);
-                }
+    switch (key)   {
+// List keypresses used:
+        case 'q':
+            pc.printf("\n\r Toggle joystick = j \n\r Scope = s \n\r Fade = f \n\r Stop = c \n\r Set POS gains k, v, \n\r Set Vel Vff, kp: l, m \n\r Set fade time: t \n\r Save Shot = 1");
+            pc.printf("\n\r Zoom Value (1-10) = z");
+            break;
+// Save a shot position:
+        case '1':
+
+            fade_tilt = T_Position;
+            pc.printf("\n\r Saving this postion as %f", fade_tilt);
+            break;
+// Switch Joystick Mode on and off
+        case 'j':
+            joystick = !joystick;
+            if(joystick) {
+                pc.printf("\n\r Under Joystick Control");
+            } else {
+                // Stop both axes
+                //PanVelocityPID.setSetPoint(0);
+                //TiltVelocityPID.setSetPoint(0);
+                pc.printf("\n\r Under PC Control");
+                pc.printf("\n\r");
             }
-        } else if(key >= '0' && key <= '9') {                      // Check it is a number
-            pc.printf("%c",key);
-            value = (key - '0');
+            break;
+// Turn on Scope Tool:
+        case 's':
+            if (!scoping) {
+                Time = 0;
+                pc.printf("\n\r Time, Tilt_JoyStickDem, P_Error, T_Position, P");
+                scoping = true;                
+            } else {
+                scoping = false;
+            }
+
+            break;
+// Calling A Fade:
+        case 'f':
+            if (DoMove == 0) {
+                fade_time = 15;
+                Profile();
+                pc.printf("\n\r Fade Distance: %f", D);                
+                DoMove = 1;
+            }
+            break;
+            // We are going to set the fade time...
+        case 't':
+            pc.printf("\n\r New Fade Time: ");
+            key = pc.getc();
             do {
                 key = pc.getc();                            // Wait for a keypress
                 if(key >= '0' && key <= '9') {              // Check it is a number
@@ -59,30 +89,39 @@
                     pc.printf("%c",key);                    // Display the new number
                 }
             } while(key != 0x0D);
-            PanVelocityPID.setSetPoint(value);
-        }
-    } else if(key == 't') {
-        // We are going to set the speed of tilt...
-        pc.printf("\n\r New Tilt Vel: ");
-        key = pc.getc();
-        if (key == 0x1B) {
-            if(pc.getc() == 0x4F) {
-                if(pc.getc() == 0x53) {
-                    pc.printf("-");
-                    do {
-                        key = pc.getc();                            // Wait for a keypress
-                        if(key >= '0' && key <= '9') {              // Check it is a number
-                            value *= 10;                            // Index the old number
-                            value += (key - '0');                   // Add on the new number
-                            pc.printf("%c",key);                    // Display the new number
-                        }
-                    } while(key != 0x0D);
-                    TiltVelocityPID.setSetPoint(value * -1);
+            setting = value;
+            fade_time = (setting);
+            pc.printf("\n\r");
+            break;
+// Stop a fade
+        case 'c':
+            if (DoMove == 1) {
+                pc.printf("\n\r Fade Stop");
+                DoMove = 0;
+                //TiltVelocityPID.setSetPoint(0);
+            }
+            break;
+// Set Tilt Kp
+        case 'k':
+            pc.printf("\n\r New Tilt Pos Kp (Multiply by 100): ");
+            key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
                 }
-            }
-        } else if(key >= '0' && key <= '9') {                      // Check it is a number
-            pc.printf("%c",key);
-            value = (key - '0');
+            } while(key != 0x0D);
+            setting = value;
+            T_Kp = (setting/100);
+            //TiltVelocityPID.setKp(value);
+            pc.printf("\n\r");
+            break;
+// We are going to set Pos Feedforward gain for tilt...
+        case 'v':
+            pc.printf("\n\r New Tilt Pos Vff (Multiply by 100): ");
+            key = pc.getc();
             do {
                 key = pc.getc();                            // Wait for a keypress
                 if(key >= '0' && key <= '9') {              // Check it is a number
@@ -91,8 +130,62 @@
                     pc.printf("%c",key);                    // Display the new number
                 }
             } while(key != 0x0D);
-            TiltVelocityPID.setSetPoint(value);
-        }
+            setting = value;
+            Vff = (setting/100);
+            pc.printf("\n\r");
+            break;
+// We are going to set Kvelff gain for tilt...
+        case 'l':
+            pc.printf("\n\r New Tilt Velocity Vff (Multiply by 100): ");
+            key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            setting = value;
+            TiltVelocityPID.setKvelff(setting/100);
+            pc.printf("\n\r");
+            break;
+// Set Tilt Velocity Kp
+        case 'm':
+            pc.printf("\n\r New Tilt Velocity Kp (Multiply by 100): ");
+            key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            setting = value;
+            TiltVelocityPID.setKp(setting/100);
+            pc.printf("\n\r");
+            break;
+// Set Zoom level for Zoom proportional
+        case 'z':
+            pc.printf("\n\r Set Zoom Scalar Low 1-9 High): ");
+            key = pc.getc();
+            do {
+                key = pc.getc();                            // Wait for a keypress
+                if(key >= '0' && key <= '9') {              // Check it is a number
+                    value *= 10;                            // Index the old number
+                    value += (key - '0');                   // Add on the new number
+                    pc.printf("%c",key);                    // Display the new number
+                }
+            } while(key != 0x0D);
+            setting = value;
+            Joy_Zoom = (setting);
+            pc.printf("\n\r");
+            break;
     }
     return(1);
-}
\ No newline at end of file
+}
+
+
+
+

--- a/TiltVelocityLoop.cpp	Mon May 18 09:06:07 2015 +0000
+++ b/TiltVelocityLoop.cpp	Wed May 27 07:13:54 2015 +0000
@@ -10,35 +10,222 @@
 #define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
 #define Upper OneTurn - Lower   // 262144 - 8192 = 253952
 
+extern Serial pc;
 extern iC_MU tilt_ic_mu;
+extern iC_MU TiltPos;
 extern PwmOut Tilt_Motor_PWM;
 extern DigitalOut Tilt_Motor_Direction;
 extern Classic_PID TiltVelocityPID;
+extern float Demand_Count_Rate;
+extern float Tilt_motor_max_count_rate;
+extern float Actual_Motor_Speed;
+
+extern float T_Position; // True Tilt Position (Degrees)
+extern float T_sf;
+
 
 int LastTiltPosition = 0;
+int Position = 0;
+int Velocity = 0;
+float Duty_Cycle = 0.0;
+
+
+// S_Ramp Fade values
+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
+
+// Main servo loop
+int DoMove = 0;
+const float LOOPs = 0.001;
+float D = 10; // Fade distance
+float T = 15; // Fade time
+float dir = 1; //direction flag
+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 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)
+float Vs;  // Speed step increment
+float Da;  // Accel distance
+float Ds; // Distance convered at steady speed
+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
+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 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;
+float Va; // mid speed point
+float as; // acceleration value during linear accel stage
+float Vj; // Speed at bottom intersection
+float Vjp; // Speed at top intersection
+float c; // constant for up ramp y=mx+c
+float b; // constant for down ramp y = mx+b
+float real_time;
+float fade_tilt;
+float joy_tilt;
+float fade_time=10;
+extern bool joystick;
+extern float Time;
+
+float T_Joy = 0.0;
+
 
 void TiltVelocityLoop(void const *args)
 {
-    int Position = tilt_ic_mu.ReadPOSITION() >> (19 - bits);// Read the current position from the iC-MU and bitshift to reduce noise
-    int Velocity = Position - LastTiltPosition;             // Calculate change in position (i.e. Velocity)
-    float Duty_Cycle = 0.0;
+    T_Position = 360 - (TiltPos.ReadPOSITION()/T_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)
+    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
         Velocity += OneTurn;
     } else if(Position > Upper & LastTiltPosition < Lower) {// We have gone over the index point in the other direction
         Velocity -= OneTurn;
-    }  
+    }
     LastTiltPosition = Position;                            // Update new position from next time
 
-    TiltVelocityPID.setProcessValue(Velocity);              // Pass the Velocity onto the PID loop
-    Duty_Cycle = TiltVelocityPID.compute();
+    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) {
+            // compute the new position demand:
+            s_profile = s * dir;
+            P = P + (s_profile * LOOPs);
+            real_time = ((T - fadetime) * 1000);
+
+            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 {
+
+        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);
+        }
+
+    }
+
+    Duty_Cycle = TiltVelocityPID.compute_ff()/Tilt_motor_max_count_rate;
 
     if(Duty_Cycle < 0) {
-        Tilt_Motor_Direction = 0;
+        Tilt_Motor_Direction = 1;
         Tilt_Motor_PWM = 1 - (Duty_Cycle * -1.0);
     } else {
-        Tilt_Motor_Direction = 1;
+        Tilt_Motor_Direction = 0;
         Tilt_Motor_PWM = 1 - Duty_Cycle;
     }
-}
\ No newline at end of file
+}
+
+void Profile() // For S ramped movement using Servo for S ramping
+{
+    if ((fade_tilt >=0) & (fade_tilt <= 359)) {
+        D = fade_tilt - T_Position; // 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 <= (6*tsfade + 0.2)) {
+        ts = tscut;
+        T = fade_time;
+    } 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
+        T = (D + (Vp * (3*ts)))/Vp;
+    }
+
+    // New version based on S-Ramping Doc - V2
+
+    j = Vp / (2*ts*ts);
+    as = j * ts;
+    c = -(Vp / 4);
+    s = 0;
+    fadetime = 0;
+   // Time = 0;
+    P = T_Position;
+
+}
+
+

--- a/main.cpp	Mon May 18 09:06:07 2015 +0000
+++ b/main.cpp	Wed May 27 07:13:54 2015 +0000
@@ -1,3 +1,7 @@
+// Control Test Harness
+// AC 19/05/2015.  Based on MS NewMotorVelLoop
+
+
 #include "mbed.h"
 #include "iC_MU.h"
 #include "rtos.h"
@@ -54,7 +58,7 @@
 
 /* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
 Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001);     //Kp, ki, kd, kvelff
-Classic_PID TiltVelocityPID(0.00018, 0.000006, 0.0, 0.0001);    //Kp, ki, kd, kvelff
+Classic_PID TiltVelocityPID(2, 0.000000, 0.0, 1);  //Kp, ki, kd, kvelff
 
 // Globals
 int ZoomPos = 10248;        // Strat off at max Zoom position to avoid jerks on startup
@@ -62,14 +66,33 @@
 bool ManualfocusFlag = false;
 bool Mode = AUTO;
 bool joystick = true;
+bool scoping = false;
 
 extern int LastPanPosition;
 extern int LastTiltPosition;
-float Pan_JoyStickDem = 0.5;
-float Tilt_JoyStickDem = 0.5;
+float Pan_JoyStickDem = 0.8;
+float Tilt_JoyStickDem = 0.8;
+float Demand_Count_Rate = 0.0;
+float Actual_Motor_Speed = 0.0;  //counts/ms
+float Velocity_Error = 0.0;   // count/ms
+float P_Error = 0.0;  // degrees
+extern float Tilt_motor_max_count_rate; //encoder counts / ms
+
+extern float T_Position; // True Tilt Position (Degrees)
+extern float T_sf;  // counts per degree
+extern int DoMove;
+extern float s_profile;
+extern float P_vel;
+extern float real_time;
+extern float T_Joy;
+float Joy_DeadBand = 55;
+float Joy_Zoom = 5;  // valid numbers from 1 - 9
+float Time = 0.0;
+extern float P;
 
 void PanVelocityLoop(void const *args);
 void TiltVelocityLoop(void const *args);
+void Profile(void const *args);
 int ServiceKeyboard();
 void UpdateCamera(float, float);
 
@@ -166,65 +189,79 @@
     LastPanPosition = pan_ic_mu.ReadPOSITION() >> 1;
 
     // Initalise Tilt PID Loop
-    TiltVelocityPID.setProcessLimits(1.0, -1.0);
+    TiltVelocityPID.setProcessLimits(Tilt_motor_max_count_rate, (Tilt_motor_max_count_rate*-1));
     TiltVelocityPID.setSetPoint(0);
     LastTiltPosition = tilt_ic_mu.ReadPOSITION() >> 1;
 
     // Initalise Anti-Lock RtosTimer thread
     RtosTimer Anti_Lock_timer(Anti_Lock, osTimerPeriodic);
     Anti_Lock_timer.start(1000);                // Run at 1Hz
+
+    pc.baud(921600);
     
-    int tiltPosition;
+    T_Position = 360 - (TiltPos.ReadPOSITION()/T_sf);    // Prime the system on startup, this is not used once running.
+    P = T_Position;                                      // Priming
+    pc.printf("\n\r Startup: T_Position = %f, P = %f, \n\r", T_Position, P);
     
     while(1) {
         // Check to see if a key has been pressed
         if(pc.readable()) {
             ServiceKeyboard();
         }
+        //T_Position = TiltPos.ReadPOSITION()/T_sf;
+        Demand_Count_Rate = TiltVelocityPID.getSetPoint();
+
+        if(1==0) {
+            Velocity_Error = Demand_Count_Rate - Actual_Motor_Speed;
+            pc.printf("\n\r Demand Ms = %f, V Error = %f, Pos = %f, Demand P = %f",Demand_Count_Rate, Velocity_Error, T_Position, P);
+        }
+        
+         if(scoping) {
+            P_Error = T_Position - P;
+            pc.printf("\n\r %f, %f, %f, %f, %f", Time, Tilt_JoyStickDem, P_Error, T_Position, P);
+            Time = Time + 0.05;
+        }
+
+       // if(DoMove ==1) {
+       //     pc.printf("\n\r %f,   %f,   %f,     %f",s_profile, P_vel, T_Position, real_time);
+       // }
+
+
 
         Thread::wait(50);
         // Update the Zoom and Focus Demands
-        UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
-        
+        //UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
+
         //pc.printf("\n\r %d ",tiltPosition);
-        Thread::wait(50);
+        //Thread::wait(50);
+
 
+        // Apply Offset
+        Pan_JoyStickDem = Pan_Joystick.read() - 0.5;
+        Tilt_JoyStickDem = Tilt_Joystick.read() - 0.5;
+        // Square the demand for pan joystick profile & check if neg
+        if(Pan_JoyStickDem > 0) {
+            Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem;
+        } else {
+            Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem * -1;
+        }
+
+        Pan_JoyStickDem *= 10000;                                   // Apply scalefactor
+        Tilt_JoyStickDem *= 10000;                                  // Apply scalefactor
+        
+       if ((Tilt_JoyStickDem * Tilt_JoyStickDem) < (Joy_DeadBand * Joy_DeadBand)){
+            Tilt_JoyStickDem = 0;                                   //Apply Deadband
+         }
+         
+        Tilt_JoyStickDem = Tilt_JoyStickDem / Joy_Zoom;
+        
         if(joystick) {
-            // Apply Offset
-            Pan_JoyStickDem = Pan_Joystick.read() - 0.5;
-            Tilt_JoyStickDem = Tilt_Joystick.read() - 0.5;
-            // Square the demand for pan joystick profile & check if neg
-            if(Pan_JoyStickDem > 0) {
-                Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem;
-            } else {
-                Pan_JoyStickDem = Pan_JoyStickDem * Pan_JoyStickDem * -1;
-            }
-            // Square the demand for tilt joystick profile & check if neg
-            tiltPosition = TiltPos.ReadPOSITION();
-            if(Tilt_JoyStickDem < 0) {
-                // Check the tilt angle to see if it is within softlimits
-                if(tiltPosition < 170000) {
-                    Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem;
-                } else {
-                    Tilt_JoyStickDem = 0.0;
-                }
-            } else {
-                // Check the tilt angle to see if it is within softlimits
-                if(tiltPosition > 25000) {
-                    Tilt_JoyStickDem = Tilt_JoyStickDem * Tilt_JoyStickDem * -1;
-                } else {
-                    Tilt_JoyStickDem = 0.0;
-                }
-            }
-            // Apply scalefactor
-            Pan_JoyStickDem *= 20000;                                   // Apply scalefactor
-            Tilt_JoyStickDem *= 20000;                                  // Apply scalefactor
-            // Apply Zoom %
-            Pan_JoyStickDem /= (ZoomPos >> 9) + 1;                      // Catch divide by zeros
-            Tilt_JoyStickDem /= (ZoomPos >> 9) + 1;                     // Catch divide by zeros
-
             PanVelocityPID.setSetPoint((int)Pan_JoyStickDem);           // Read the joystick and apply the gain
             TiltVelocityPID.setSetPoint((int)Tilt_JoyStickDem);         // Read the joystick and apply the gain
+        } else {
+
+            T_Joy = -Tilt_JoyStickDem / 100;
+
         }
 
         if(AutofocusFlag) {
@@ -270,3 +307,6 @@
       }
   }
 }*/
+
+
+