Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Classic_PID iC_MU mbed-rtos mbed
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 @@ + +