/*   
}  

  Feedback 360 Angle Control [Low Level Example].c
  
  This is a simplified example of how the low-level position control is done.
  These features would normally be tucked away in a library like abdrive360
  or servo360 with functions to request measured angle, set target angle, 
  and etc.  
*/

// Library inlcudes  
#include "simpletools.h"                      // For pulse_in, print, scan etc...
#include "servo.h"                            // For servo pulse control

int pinFeedback = 14;                         // P14 connected to feedback line
int pinControl = 12;                          // P12 connected to control line

volatile int angle, targetAngle;              // Global shared variables
volatile int Kp = 1;                          // Proportional constant   

void feedback360();                           // Position monitoring
void control360();                            // Position control

int main()                                    // Main function
{
  cog_run(feedback360, 128);                  // Run feedback360 in a cog
  cog_run(control360, 128);                   // Run control360 in a cog
  pause(100);                                 // Wait 1/10 s, might not need

  while(1)                                    // Main loop
  {
    print("Enter angle: ");                   // Prompt user for angle
    scan("%d", &targetAngle);                 // Get entered angle
    print("\r");                              // Next line
    while(abs(targetAngle - angle) > 4)       // Display until close to finish
    {
      print("targetAngle = %d, angle = %d\r", // Display target & measured
             targetAngle, angle);             
      pause(50);                              // ...every 50 ms
    }      
  }    
}


void feedback360()                            // Cog keeps angle variable updated
{
  int unitsFC = 360;                          // Units in a full circle
  int dutyScale = 1000;                       // Scale duty cycle to 1/1000ths
  int dcMin = 29;                             // Minimum duty cycle
  int dcMax = 971;                            // Maximum duty cycle
  int q2min = unitsFC/4;                      // For checking if in 1st quadrant
  int q3max = q2min * 3;                      // For checking if in 4th quadrant
  int turns = 0;                              // For tracking turns
  // dc is duty cycle, theta is 0 to 359 angle, thetaP is theta from previous
  // loop repetition, tHigh and tLow are the high and low signal times for 
  // duty cycle calculations.
  int dc, theta, thetaP, tHigh, tLow;         

  // Measure feedback signal high/low times.
  tLow = pulse_in(pinFeedback, 0);            // Measure low time 
  tHigh = pulse_in(pinFeedback, 1);           // Measure high time

  // Calcualte initial duty cycle and angle.
  dc = (dutyScale * tHigh) / (tHigh + tLow);
  theta = (unitsFC - 1) - ((dc - dcMin) * unitsFC) / (dcMax - dcMin + 1);
  thetaP = theta;

  while(1)                                    // Main loop for this cog
  {
    // Measure high and low times, making sure to only take valid cycle
    // times (a high and a low on opposite sides of the 0/359 boundary
    // will not be valid.
    int tCycle = 0;                           // Clear cycle time
    while(1)                                  // Keep checking
    {
      tHigh = pulse_in(pinFeedback, 1);       // Measure time high
      tLow = pulse_in(pinFeedback, 0);        // Measure time low
      tCycle = tHigh + tLow;
      if((tCycle > 1000) && (tCycle < 1200))  // If cycle time valid 
        break;                                // break from loop
    }      
    dc = (dutyScale * tHigh) / tCycle;        // Calculate duty cycle
    
    // This gives a theta increasing int the
    // counterclockwise direction.
    theta = (unitsFC - 1) -                   // Calculate angle
            ((dc - dcMin) * unitsFC) 
            / (dcMax - dcMin + 1);

    if(theta < 0)                             // Keep theta valid
      theta = 0; 
    else if(theta > (unitsFC - 1)) 
      theta = unitsFC - 1;

    // If transition from quadrant 4 to  
    // quadrant 1, increase turns count. 
    if((theta < q2min) && (thetaP > q3max))
      turns++;
    // If transition from quadrant 1 to  
    // quadrant 4, decrease turns count. 
    else if((thetaP < q2min) && (theta > q3max))
      turns --;

    // Construct the angle measurement from the turns count and
    // current theta value.
    if(turns >= 0)
      angle = (turns * unitsFC) + theta;
    else if(turns <  0)
      angle = ((turns + 1) * unitsFC) - (unitsFC - theta);

    thetaP = theta;                           // Theta previous for next rep
  }
}    


// Most rudimentary control system example, 
// just proportional.  This could be done
// in the same cog as the angle mesurement.                                            
void control360()                             // Cog for control system
{
  servo_speed(pinControl, 0);                 // Start servo control cog
  
  int errorAngle, output, offset;             // Control system variables
  
  while(1)                                    // Main loop for this cog
  {
    errorAngle = targetAngle - angle;         // Calculate error
    output = errorAngle * Kp;                 // Calculate proportional 
    if(output > 200) output = 200;            // Clamp output
    if(output < -200) output = -200;
    if(errorAngle > 0)                        // Add offset
      offset = 30;
    else if(errorAngle < 0)
      offset = -30;
    else
      offset = 0;     
    servo_speed(pinControl, output + offset); // Set output
    pause(20);                                // Repeat after 20 ms
  } 
}