/* ===========================
 * Mihir Patel 
 * Lab 07 - Inverse Kinematics
 * ===========================
*/
#include "project.h"
#include "math.h"

int ThetaOne(float Angle) // a function to take an angle value and turn it into a tuned compare value
{
    int Compare;
    int min_comp = 1350; // starting point for minimum compare value to achieve zero degrees
    int max_comp = 7200; // starting point for maximum compare value to achieve 180 degrees
    int min_angle = 0; // minimum servo angle in relation to min_comp value
    int max_angle = 180; // maximum servo angle in relation to max_comp value
    Compare = ((max_comp-min_comp)/(max_angle-min_angle)) * (Angle-min_angle) + min_comp; // linear comp/angle transformation equation
    return Compare;
}
int ThetaTwo(float Angle) // a function to take an angle value and turn it into a tuned compare value
{
    int Compare;
    int min_comp = 1400; // starting point for minimum compare value to achieve zero degrees
    int max_comp = 7290; // starting point for maximum compare value to achieve 180 degrees
    int min_angle = -90; // minimum servo angle in relation to min_comp value
    int max_angle = 90; // maximum servo angle in relation to max_comp value
    Compare = ((max_comp-min_comp)/(max_angle-min_angle)) * (Angle-min_angle) + min_comp; // linear comp/angle transformation equation
    return Compare;
}

float X_end = -7.0;
float Y_end = 9.0;

float a2 = 7.0;
float a4 = 8.3;

float Theta1 = 0.0;
float Theta2 = 0.0;
float r1 = 0.0;
float Phi_1 = 0.0;
float Phi_2 = 0.0;
float Phi_3 = 0.0;
int main(void)
{

   
   
    r1 = sqrt((X_end*X_end)+(Y_end*Y_end));
    Phi_1 = atan2(Y_end,X_end);
    Phi_2 = acos(((a4*a4)-(a2*a2)-(r1*r1))/(-2*a2*r1));
    Theta1 = Phi_1-Phi_2;
    Theta1 = (Theta1/M_PI)*180;
   
    Phi_3 = acos(((r1*r1)-(a2*a2)-(a4*a4))/(-2*a2*a4));
    Theta2 = 180 - Phi_3;
    Theta2 = (Theta2/M_PI)*180;
   
    PWM_1_Start();
    CyGlobalIntEnable; /* Enable global interrupts. */

    /* Place your initialization/startup code here (e.g. MyInst_Start()) */

    for(;;)
    {
        PWM_1_WriteCompare1(ThetaOne(Theta1));
        PWM_1_WriteCompare2(ThetaTwo(Theta2));
        Theta2 += 108;
        CyDelay(3000);
       
    }
}