main.cpp

00001 // For testing Tilt on an Aluminium unit.
00002 // AC 19/05/2015.  Based on MS NewMotorVelLoop
00003 
00004 
00005 #include "mbed.h"
00006 #include "iC_MU.h"
00007 #include "rtos.h"
00008 #include "Classic_PID.h"
00009 
00010 #define _Kp 0.18            // Kp.Freq product
00011 #define _Ki 0.006           // Ki.Freq product
00012 
00013 Serial pc(USBTX,USBRX);
00014 
00015 // iC-MU Encoder Objects
00016 iC_MU tilt_ic_mu(p5,p6,p7,p8);
00017 iC_MU TiltPos(p5,p6,p7,p11);
00018 iC_MU pan_ic_mu(p5,p6,p7,p12);
00019 iC_MU PanPos(p5,p6,p7,p13);
00020 
00021 #ifdef TARGET_LPC4088
00022 // Tilt Motor
00023 PwmOut Tilt_Motor_PWM(p27);                     // Purple wire
00024 DigitalOut Tilt_Motor_Direction(p28);           // Yellow wire
00025 // Pan Motor
00026 PwmOut Pan_Motor_PWM(p25);                      // Purple wire
00027 DigitalOut Pan_Motor_Direction(p26);            // Yellow wire
00028 #endif
00029 
00030 #ifdef TARGET_LPC1768
00031 // Tilt Motor
00032 PwmOut Tilt_Motor_PWM(p23);                     // Purple wire
00033 DigitalOut Tilt_Motor_Direction(p24);           // Yellow wire
00034 // Pan Motor
00035 PwmOut Pan_Motor_PWM(p21);                      // Purple wire
00036 DigitalOut Pan_Motor_Direction(p22);            // Yellow wire
00037 // Button
00038 DigitalIn Fademe(p14);
00039 #endif
00040 
00041 // Joystick stuff
00042 AnalogIn Pan_Joystick(p16);
00043 AnalogIn Tilt_Joystick(p17);
00044 AnalogIn Zoom_Joystick(p18);
00045 AnalogIn Focus_Pot(p19);            // The top Pot (Pot 1)
00046 AnalogIn Setspeed(p20);             // The bottom Pot (Pot 2)
00047 
00048 // Camera Stuff
00049 Serial Camera(p9,p10);
00050 InterruptIn Switch(p14);
00051 #ifdef TARGET_LPC4088
00052 DigitalOut led4(LED1),led2(LED2), led3(LED3), led1(LED4);
00053 #endif
00054 #ifdef TARGET_LPC1768
00055 DigitalOut led1(LED1),led2(LED2), led3(LED3), led4(LED4);
00056 #endif
00057 
00058 
00059 #define AUTO 1
00060 #define MANUAL 0
00061 
00062 /* Kp = 0.00018, Ki = 0.000006, Kd = 0.0, 0.0001 */
00063 Classic_PID PanVelocityPID(0.00018, 0.000006, 0.0, 0.0001);     //Kp, ki, kd, kvelff
00064 Classic_PID TiltVelocityPID(4, 0.000000, 0.0, 1);  //Kp, ki, kd, kvelff1
00065 
00066 // Globals
00067 int ZoomPos = 10248;        // Strat off at max Zoom position to avoid jerks on startup
00068 bool AutofocusFlag = true;
00069 bool ManualfocusFlag = false;
00070 bool Mode = AUTO;
00071 bool joystick = false;
00072 bool scoping = false;
00073 
00074 extern int LastPanPosition;
00075 extern int Last_M_Position;
00076 float Pan_JoyStickDem = 0.8;
00077 float Tilt_JoyStickDem = 0.8;
00078 float Demand_Count_Rate = 0.0;
00079 float Actual_Motor_Speed = 0.0;  //counts/ms
00080 float Velocity_Error = 0.0;   // count/ms
00081 float P_Error = 0.0;  // degrees
00082 extern float Tilt_motor_max_count_rate; //encoder counts / ms
00083 
00084 extern float T_Position; // True Tilt Position (Degrees)
00085 extern float T_Encoder_sf;  // counts per degree
00086 extern int DoMove;
00087 extern float s_profile;
00088 extern float P_vel;
00089 extern float real_time;
00090 extern float T_Joy;
00091 float Joy_DeadBand = 15;
00092 float Joy_Zoom = 1;  // valid numbers from 1 - 9
00093 float Time = 0.0;
00094 extern double P;
00095 
00096 void PanVelocityLoop(void const *args);
00097 void TiltVelocityLoop(void const *args);
00098 void Profile(void const *args);
00099 int ServiceKeyboard();
00100 void UpdateCamera(float, float);
00101 
00102 void isr_Switch(void)
00103 {
00104     wait_ms(1);                 // Wait 1ms and check if the switch is still pressed
00105     if (Switch) {
00106         wait_ms(1);             // Wait another 1ms and double check the switch
00107         if (Switch) {
00108             Mode = !Mode;
00109             if(Mode == AUTO) {
00110                 AutofocusFlag = true;
00111             } else {
00112                 ManualfocusFlag = true;
00113             }
00114         }
00115     }
00116 }
00117 
00118 void Anti_Lock(void const *args)
00119 {
00120     Pan_Motor_Direction = !Pan_Motor_Direction.read();      // Toggle motor direction to overcome lock protection
00121     Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
00122     wait_us(10);
00123     Pan_Motor_Direction = !Pan_Motor_Direction.read();
00124     Tilt_Motor_Direction = !Tilt_Motor_Direction.read();
00125 }
00126 
00127 
00128 // IRQ for Rx camera interrupt
00129 void UART3_rxInterrupt(void)
00130 {
00131     // Create an array to read the whole message
00132     static int index = 0;
00133     static char msg[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
00134 
00135     NVIC_DisableIRQ(UART3_IRQn);            // Disable the interrupt
00136     uint32_t IRR3 = LPC_UART3->IIR;
00137     msg[index] = LPC_UART3->RBR;            // Read the byte and put it into the next available element of the array
00138 
00139     // Check to see if that is the end of the message
00140     if(msg[index] == 0xFF) {
00141         // Check to see if the response is zoom data i.e. 7 bytes long and starts with 0x90, 0x50...
00142         if(index == 6 && msg[0] == 0x90 && msg[1] == 0x50) {
00143             ZoomPos = (msg[2] & 0x0F) << 12;
00144             ZoomPos += (msg[3] & 0x0F) << 8;
00145             ZoomPos += (msg[4] & 0x0F) << 4;
00146             ZoomPos += (msg[5] & 0x0F);
00147         }
00148         index = 0;
00149     } else {
00150         index++;
00151     }
00152 
00153     led3 = !led3;                           // Flash the LED
00154     NVIC_EnableIRQ(UART3_IRQn);             // Enable the interrupt again
00155 }
00156 
00157 int main()
00158 {
00159     // Increase the Camera baud rate
00160     Camera.baud(38400);
00161 
00162     // Set up the Camera Rx Interrupt
00163     Camera.attach(&UART3_rxInterrupt,Serial::RxIrq);
00164 
00165     // Set up the switch to toggle autofocus
00166     Switch.mode(PullDown);                  //Set the internal pull down resistor
00167     Switch.rise(&isr_Switch);               //ISR for the switch
00168     led4 = 1;                               // We start in autofocus mode
00169     led1 = 1;                               // Turn all other LEDs off
00170     led2 = 1;
00171     led3 = 0;
00172 
00173     // Set up the Pan motor
00174     Pan_Motor_PWM.period_us(50);                // Set PWM to 20 kHz
00175     Pan_Motor_PWM = 1;                          // Start with motor static
00176 
00177     // Set up the Tilt motor
00178     Tilt_Motor_PWM.period_us(50);               // Set PWM to 20 kHz
00179     Tilt_Motor_PWM = 1;                         // Start with motor static
00180 
00181     // Initalise Pan Velocity loop RtosTimer thread
00182     RtosTimer PanVelocityLoop_timer(PanVelocityLoop, osTimerPeriodic);
00183     PanVelocityLoop_timer.start(1);             // Run at 1kHz
00184 
00185     // Initalise Tilt Velocity loop RtosTimer thread
00186     RtosTimer TiltVelocityLoop_timer(TiltVelocityLoop, osTimerPeriodic);
00187     TiltVelocityLoop_timer.start(1);            // Run at 1kHz
00188 
00189     // Initalise Pan PID Loop
00190     PanVelocityPID.setProcessLimits(1.0, -1.0);
00191     PanVelocityPID.setSetPoint(0);
00192     LastPanPosition = pan_ic_mu.ReadPOSITION() >> 1;
00193 
00194     // Initalise Tilt PID Loop
00195     TiltVelocityPID.setProcessLimits(Tilt_motor_max_count_rate, (Tilt_motor_max_count_rate*-1));
00196     TiltVelocityPID.setSetPoint(0);
00197     Last_M_Position = tilt_ic_mu.ReadPOSITION() >> 1;
00198 
00199     // Initalise Anti-Lock RtosTimer thread
00200     RtosTimer Anti_Lock_timer(Anti_Lock, osTimerPeriodic);
00201     Anti_Lock_timer.start(1000);                // Run at 1Hz
00202 
00203     pc.baud(921600);
00204 
00205     T_Position = 360 - (TiltPos.ReadPOSITION()/T_Encoder_sf);    // Prime the system on startup, this is not used once running.
00206     P = T_Position;                                      // Priming
00207     pc.printf("\n\r Startup: T_Position = %f, P = %f, \n\r", T_Position, P);
00208 
00209     while(1) {
00210         // Check to see if a key has been pressed
00211         if(pc.readable()) {
00212             ServiceKeyboard();
00213         }
00214         //T_Position = TiltPos.ReadPOSITION()/T_Encoder_sf;
00215         Demand_Count_Rate = TiltVelocityPID.getSetPoint();
00216 
00217         if(1==0) {
00218             Velocity_Error = Demand_Count_Rate - Actual_Motor_Speed;
00219             pc.printf("\n\r Demand Ms = %f, V Error = %f, Pos = %f, Demand P = %f",Demand_Count_Rate, Velocity_Error, T_Position, P);
00220         }
00221 
00222         if(scoping) {
00223             P_Error = P - T_Position;
00224             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);
00225             Time = Time + 0.1;
00226         }
00227 
00228         Thread::wait(50);
00229         
00230         
00231         if (Fademe){
00232             pc.printf("\n\r %i, %f", Setspeed.read(), Setspeed.read());
00233             }
00234             
00235         
00236         
00237         //Update the Zoom and Focus Demands
00238         UpdateCamera(Zoom_Joystick.read(),Focus_Pot.read());
00239 
00240         //pc.printf("\n\r %d ",tiltPosition);
00241         Thread::wait(50);
00242 
00243 
00244         // Apply Offset
00245         Tilt_JoyStickDem = Setspeed.read() - 0.5; //Tilt_Joystick.read() - 0.5;
00246         Tilt_JoyStickDem = Tilt_JoyStickDem;
00247   
00248         Tilt_JoyStickDem /= (ZoomPos >> 9) + 1;                     // Catch divide by zeros
00249 
00250         Tilt_JoyStickDem *= 10000;                                  // Apply scalefactor
00251 
00252         if ((Tilt_JoyStickDem * Tilt_JoyStickDem) < (Joy_DeadBand * Joy_DeadBand)) {
00253             Tilt_JoyStickDem = 0;                                   //Apply Deadband
00254         }
00255 
00256         if(Tilt_JoyStickDem > 0) {
00257             // Check the tilt angle to see if it is within softlimits
00258             if(T_Position > 310) {                
00259                 Tilt_JoyStickDem = 0.0;
00260             }
00261         } else {
00262             // Check the tilt angle to see if it is within softlimits
00263             if(T_Position < 40) {            
00264                 Tilt_JoyStickDem = 0.0;
00265             }
00266         }
00267         
00268 
00269         Tilt_JoyStickDem = Tilt_JoyStickDem / Joy_Zoom;
00270 
00271         if(joystick) {
00272             TiltVelocityPID.setSetPoint((int)Tilt_JoyStickDem);         // Read the joystick and apply the gain
00273         } else {
00274 
00275             T_Joy = Tilt_JoyStickDem / 40;
00276         }
00277 
00278         if(AutofocusFlag) {
00279             // Turn on the autofocus
00280             led1 = 1;                               // Turn the LED on to show we are in auto-focus mode
00281             Camera.putc(0x81);                      // Camera
00282             Camera.putc(0x01);                      // Turn on auto-focus cmd
00283             Camera.putc(0x04);
00284             Camera.putc(0x38);
00285             Camera.putc(0x02);
00286             Camera.putc(0xFF);                      // Terminator
00287             AutofocusFlag = false;
00288             Mode = AUTO;
00289         }
00290         if(ManualfocusFlag) {
00291             // Turn on Manual focus
00292             led1 = 0;                               // Turn the LED off to show we are in manual-focus mode
00293             Camera.putc(0x81);                      // Camera
00294             Camera.putc(0x01);                      // Turn on manual-focus cmd
00295             Camera.putc(0x04);
00296             Camera.putc(0x38);
00297             Camera.putc(0x03);
00298             Camera.putc(0xFF);                      // Terminator
00299             ManualfocusFlag = false;
00300             Mode = MANUAL;
00301         }
00302     }
00303 }
00304 
00305 /*
00306    // Increase the serial baud rate
00307    // pc.baud(921600);
00308 
00309    while(1) {
00310        Thread::wait(100);
00311 
00312       if(pc.readable()) {
00313           ServiceKeyboard();
00314       }
00315 
00316       if(Demand) {
00317           pc.printf("\n\r %0.3f ",PanVelocityPID.getSetPoint());
00318       }
00319   }
00320 }*/
00321 
00322 
00323 
00324 
00325