c++ 2D servos robot arm inverse kinematics - c++

Question

I am currently trying to write code to instruct a robot arm (2 servos motors, 3 sticks) to write out words and am having trouble getting it right. At the moment I am just trying to get it to move to a vector. I think that the inverse kinematics part is correct but all I'm getting is a twitch from one of the motors (if I'm lucky). A Nucleo- F411RE board is being used and I'm using the mbed developer.
#include "mbed.h"
#include "Servo.h"
#include "math.h"
Servo servo1(PA_8), servo2(PA_9);
float len1 = 9.0;
float len2 = 7.5;
double pi = 3.1415926535897;
int lawOfCosines (float a, float b, float c )
{
return acos((a*a + b*b - c*c) / (2 * a * b));
}
int distance(float x, float y) {
return sqrt(x*x + y*y);
}
int deg(float rad) {
return rad * 180 / pi;
}
int main() {
//fmt.Println("Lets do some tests. First move to (5,5):");
float x = 5.0;
float y = 5.0;
float dist = distance(x, y);
float D1 = atan2(y, x);
float D2 = lawOfCosines(dist, len1, len2);
float A1 = D1 + D2;
float A2 = lawOfCosines(len1, len2, dist);
float m1 = A1 * (100/90);
float m2 = A2 * (100/90);
for(int i=0; i<m1; i++) {
servo2 = i/100.0;
wait(0.01);
}
for(int i=0; i<m2; i++) {
servo1 = i/100.0;
wait(0.01);
}
}
`}
Any help to where I'm going wrong is greatly appreciated