PanVelocityLoop.cpp

00001 #include "mbed.h"
00002 #include "iC_MU.h"
00003 #include "rtos.h"
00004 #include "Classic_PID.h"
00005 
00006 // Define limits for zero crossing
00007 // These values should allow operation upto 3750 RPM
00008 #define bits 18                 // The number of bits we want to use
00009 #define OneTurn (1<<bits)       // 262144 counts per rev
00010 #define Lower (1<<(bits-5))     // 8192 counts = 11.25 degrees
00011 #define Upper OneTurn - Lower   // 262144 - 8192 = 253952
00012 
00013 extern iC_MU pan_ic_mu;
00014 extern PwmOut Pan_Motor_PWM;
00015 extern DigitalOut Pan_Motor_Direction;
00016 extern Classic_PID PanVelocityPID;
00017 
00018 int LastPanPosition = 0;
00019 
00020 void PanVelocityLoop(void const *args)
00021 {
00022     int Position = pan_ic_mu.ReadPOSITION() >> (19 - bits); // Read the current position from the iC-MU and bitshift to reduce noise
00023     int Velocity = Position - LastPanPosition;              // Calculate change in position (i.e. Velocity)
00024     float Duty_Cycle = 0.0;
00025 
00026     // Check to see if we have gone past the index point
00027     if(Position < Lower & LastPanPosition > Upper) {        // We have gone over the index point in 1 direction
00028         Velocity += OneTurn;
00029     } else if(Position > Upper & LastPanPosition < Lower) { // We have gone over the index point in the other direction
00030         Velocity -= OneTurn;
00031     }  
00032     LastPanPosition = Position;                             // Update new position from next time
00033 
00034     PanVelocityPID.setProcessValue(Velocity);               // Pass the Velocity onto the PID loop
00035     //Duty_Cycle = VelocityPID.compute_ff();                  // Adjust the PWM output with no feed forward
00036     Duty_Cycle = PanVelocityPID.compute();
00037 
00038     if(Duty_Cycle < 0) {
00039         Pan_Motor_Direction = 0;
00040         Pan_Motor_PWM = 1 - (Duty_Cycle * -1.0);
00041     } else {
00042         Pan_Motor_Direction = 1;
00043         Pan_Motor_PWM = 1 - Duty_Cycle;
00044     }
00045 }