Diff: main.cpp

Revision:

2:dc684c402296

Parent:

1:1ad84260ff59

Child:

3:f8a5c1cee1fa

--- 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 @@
       }
   }
 }*/
+
+
+