Revision 4:4dafa4113982, committed 2015-06-25

Comitter:

acodd

Date:

Thu Jun 25 09:41:26 2015 +0000

Parent:

3:f8a5c1cee1fa

Commit message:

Test Harness for a single axis

Changed in this revision

--- a/ServiceKeyboard.cpp	Wed May 27 10:07:39 2015 +0000
+++ b/ServiceKeyboard.cpp	Thu Jun 25 09:41:26 2015 +0000
@@ -15,6 +15,8 @@
 extern float fade_tilt;
 extern float fade_time;
 extern float D;
+extern float T;
+extern float ts;
 extern int DoMove;
 extern float T_Position;
 extern float T_Kp;
@@ -24,6 +26,8 @@
 
 extern float Joy_Zoom;
 
+extern double drive;
+
 // Returns the new set point entered via the keyboard
 int ServiceKeyboard ()
 {
@@ -36,7 +40,7 @@
 // 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");
+            pc.printf("\n\r Zoom Value (1-10) = z  \n\r Drive const speed = d");
             break;
 // Save a shot position:
         case '1':
@@ -61,8 +65,9 @@
         case 's':
             if (!scoping) {
                 Time = 0;
-                pc.printf("\n\r Time, Tilt_JoyStickDem, P_Error, T_Position, P");
-                scoping = true;                
+                pc.printf("\n\r Time, Tilt_JoyStickDem, P_Error, T_Position, P, Motor_Speed, Demand_Speed");
+
+                scoping = true;
             } else {
                 scoping = false;
             }
@@ -71,9 +76,10 @@
 // Calling A Fade:
         case 'f':
             if (DoMove == 0) {
-                fade_time = 15;
+                //fade_time = 15;
                 Profile();
-                pc.printf("\n\r Fade Distance: %f", D);                
+                pc.printf("\n\r Fade Distance: %f", D);
+                pc.printf("\n\r Fade Time = %f,  Ramp segment =  %f",T, ts);
                 DoMove = 1;
             }
             break;
@@ -182,6 +188,22 @@
             Joy_Zoom = (setting);
             pc.printf("\n\r");
             break;
+            // We are going to set Pos Feedforward gain for tilt...
+        case 'd':
+            pc.printf("\n\r Drive const speed (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;
+            drive = setting;
+            pc.printf("\n\r New Speed is = %f", (drive/100));
+            break;
     }
     return(1);
 }
@@ -190,3 +212,4 @@
 
 
 
+

--- 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

--- a/main.cpp	Wed May 27 10:07:39 2015 +0000
+++ b/main.cpp	Thu Jun 25 09:41:26 2015 +0000
@@ -1,4 +1,4 @@
-// Control Test Harness
+// For testing Tilt on an Aluminium unit.
 // AC 19/05/2015.  Based on MS NewMotorVelLoop
 
 
@@ -34,6 +34,8 @@
 // Pan Motor
 PwmOut Pan_Motor_PWM(p21);                      // Purple wire
 DigitalOut Pan_Motor_Direction(p22);            // Yellow wire
+// Button
+DigitalIn Fademe(p14);
 #endif
 
 // Joystick stuff
@@ -41,6 +43,7 @@
 AnalogIn Tilt_Joystick(p17);
 AnalogIn Zoom_Joystick(p18);
 AnalogIn Focus_Pot(p19);            // The top Pot (Pot 1)
+AnalogIn Setspeed(p20);             // The bottom Pot (Pot 2)
 
 // Camera Stuff
 Serial Camera(p9,p10);
@@ -58,7 +61,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(2, 0.000000, 0.0, 1);  //Kp, ki, kd, kvelff
+Classic_PID TiltVelocityPID(4, 0.000000, 0.0, 1);  //Kp, ki, kd, kvelff1
 
 // Globals
 int ZoomPos = 10248;        // Strat off at max Zoom position to avoid jerks on startup
@@ -85,10 +88,10 @@
 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 Joy_DeadBand = 15;
+float Joy_Zoom = 1;  // valid numbers from 1 - 9
 float Time = 0.0;
-extern float P;
+extern double P;
 
 void PanVelocityLoop(void const *args);
 void TiltVelocityLoop(void const *args);
@@ -198,11 +201,11 @@
     Anti_Lock_timer.start(1000);                // Run at 1Hz
 
     pc.baud(921600);
-    
+
     T_Position = 360 - (TiltPos.ReadPOSITION()/T_Encoder_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()) {
@@ -215,53 +218,61 @@
             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.025;
+
+        if(scoping) {
+            P_Error = P - T_Position;
+            pc.printf("\n\r %f, %f, %f, %f, %f, %f, %f, %f", Time, Tilt_JoyStickDem, P_Error, T_Position, P, Actual_Motor_Speed, s_profile, real_time);
+            Time = Time + 0.1;
         }
 
-       // if(DoMove ==1) {
-       //     pc.printf("\n\r %f,   %f,   %f,     %f",s_profile, P_vel, T_Position, real_time);
-       // }
-
-
-
-        Thread::wait(25);
-        // Update the Zoom and Focus Demands
-        //UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
+        Thread::wait(50);
+        
+        
+        if (Fademe){
+            pc.printf("\n\r %i, %f", Setspeed.read(), Setspeed.read());
+            }
+            
+        
+        
+        //Update the Zoom and Focus Demands
+        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;
+        Tilt_JoyStickDem = Setspeed.read() - 0.5; //Tilt_Joystick.read() - 0.5;
+        Tilt_JoyStickDem = Tilt_JoyStickDem;
+  
+        Tilt_JoyStickDem /= (ZoomPos >> 9) + 1;                     // Catch divide by zeros
+
+        Tilt_JoyStickDem *= 10000;                                  // Apply scalefactor
+
+        if ((Tilt_JoyStickDem * Tilt_JoyStickDem) < (Joy_DeadBand * Joy_DeadBand)) {
+            Tilt_JoyStickDem = 0;                                   //Apply Deadband
         }
 
-        Pan_JoyStickDem *= 10000;                                   // Apply scalefactor
-        Tilt_JoyStickDem *= 10000;                                  // Apply scalefactor
+        if(Tilt_JoyStickDem > 0) {
+            // Check the tilt angle to see if it is within softlimits
+            if(T_Position > 310) {                
+                Tilt_JoyStickDem = 0.0;
+            }
+        } else {
+            // Check the tilt angle to see if it is within softlimits
+            if(T_Position < 40) {            
+                Tilt_JoyStickDem = 0.0;
+            }
+        }
         
-       if ((Tilt_JoyStickDem * Tilt_JoyStickDem) < (Joy_DeadBand * Joy_DeadBand)){
-            Tilt_JoyStickDem = 0;                                   //Apply Deadband
-         }
-         
+
         Tilt_JoyStickDem = Tilt_JoyStickDem / Joy_Zoom;
-        
+
         if(joystick) {
-            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;
-
+            T_Joy = Tilt_JoyStickDem / 40;
         }
 
         if(AutofocusFlag) {
@@ -310,3 +321,5 @@
 
 
 
+
+