File content as of revision 32:b63b5837bcb1:

#pragma once

#include "forward_kinematics.h"
#include "inverse_kinematics.h"

// Moves the end effector by (move_x, move_y) metres.
// Requires current motor angles.
// Last two paramteres will give the new target angles.
void end_effector_control(double move_x, double move_y, double main_current, double sec_current, double &main_target, double &sec_target) {
    
    double end_x, end_y;
    
    forward_kinematics(main_current, sec_current, end_x, end_y);
    
    end_x += move_x;
    end_y += move_y;
    
    double main_potential_target, sec_potential_target;
    
    inverse_kinematics(end_x, end_y, main_potential_target, sec_potential_target);
    
    // Check for NAN values. (they are never equal to themselves)
    if (main_potential_target != main_potential_target) {
        main_potential_target = main_current; // Illogical angle, don't move.
    }
    
       // Check for NAN values. (they are never equal to themselves)
    if (sec_potential_target != sec_potential_target) {
        sec_potential_target = sec_current; // Illogical angle, don't move.
    }
    
    // Return new angles.
    main_target = main_potential_target;
    sec_target = sec_potential_target;
}