Diff: main.cpp

Revision:

4:4dafa4113982

Parent:

3:f8a5c1cee1fa

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