I am building a submarine with two thrusters each of which is connected to two servos allowing 360 degrees of motion. I have used the fsolve function in octave to solve the nonlinear system of equations to get the servo angles and thrust needed to create my desired motion. I want the Arduino to take a string input <(x,y,z,roll,pitch,yaw> and run those inputs through fsolve and output the angles and thrusts to be sent to the respective servos and motor controllers. Unfortunately fsolve does not exist in Arduino so I will be trying to program it in based on the source code provided by octave. I am very new to programming in Arduino so any help will be very appreciated. Is this goal achievable?
fsolve source code - fsolve.m | searchcode