Switch rostock arm solution to use floats, fastmath and sqrtf

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index d95c848..c97a74f 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -22,6 +22,11 @@ using std::string;
 #include "arm_solutions/RotatableCartesianSolution.h"
 #include "arm_solutions/RostockSolution.h"
 
+#include "mbed.h"
+static Timer m2st;
+static double m2stime;
+static int m2scnt= 0;
+
 Robot::Robot(){
     this->inch_mode = false;
     this->absolute_mode = true;
@@ -30,7 +35,10 @@ Robot::Robot(){
     clear_vector(this->current_position);
     clear_vector(this->last_milestone);
     this->arm_solution = NULL;
-    seconds_per_minute = 60.0;
+       seconds_per_minute = 60.0;
+
+       m2st.start();
+ 
 }
 
 //Called when the module has just been loaded
@@ -173,10 +181,11 @@ void Robot::execute_gcode(Gcode* gcode){
                 gcode->stream->printf("X:%g Y:%g Z:%g F:%g ", steps[0], steps[1], steps[2], seconds_per_minute);
                 gcode->add_nl = true;
                 return;
-            case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f ",
+            case 114: gcode->stream->printf("C: X:%1.3f Y:%1.3f Z:%1.3f %f",
                                                  this->current_position[0],
                                                  this->current_position[1],
-                                                 this->current_position[2]);
+                                                                                       this->current_position[2], m2scnt > 0 ? m2stime/m2scnt:0);
+                               m2scnt= 0;      m2stime= 0;
                 gcode->add_nl = true;
                 return;
             case 220: // M220 - speed override percentage
@@ -233,8 +242,11 @@ void Robot::execute_gcode(Gcode* gcode){
 void Robot::append_milestone( double target[], double rate ){
     int steps[3]; //Holds the result of the conversion
 
+       m2st.reset();
     this->arm_solution->millimeters_to_steps( target, steps );
-
+       m2stime += m2st.read_us();
+       m2scnt++;
+       
     double deltas[3];
     for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
 

diff --git a/src/modules/robot/arm_solutions/RostockSolution.cpp b/src/modules/robot/arm_solutions/RostockSolution.cpp
index a9e9f4f..4dae77b 100644 (file)
--- a/src/modules/robot/arm_solutions/RostockSolution.cpp
+++ b/src/modules/robot/arm_solutions/RostockSolution.cpp
@@ -1,39 +1,45 @@
 #include "RostockSolution.h"
-#include <math.h>
+#include <fastmath.h>
 
-#define PIOVER180       0.01745329251994329576923690768489
+#define PIOVER180       0.01745329251994329576923690768489F
 
 RostockSolution::RostockSolution(Config* passed_config) : config(passed_config){
     this->alpha_steps_per_mm = this->config->value(alpha_steps_per_mm_checksum)->as_number();
     this->beta_steps_per_mm  = this->config->value( beta_steps_per_mm_checksum)->as_number();
     this->gamma_steps_per_mm = this->config->value(gamma_steps_per_mm_checksum)->as_number();
 
-    double alpha_angle  = PIOVER180 * this->config->value(alpha_angle_checksum)->by_default(30.0)->as_number();
-    this->sin_alpha     = sin(alpha_angle);
-    this->cos_alpha     = cos(alpha_angle);
-    double beta_angle   = PIOVER180 * this->config->value( beta_relative_angle_checksum)->by_default(120.0)->as_number();
-    this->sin_beta      = sin(beta_angle);
-    this->cos_beta      = cos(beta_angle);
-    double gamma_angle  = PIOVER180 * this->config->value(gamma_relative_angle_checksum)->by_default(240.0)->as_number();
-    this->sin_gamma     = sin(gamma_angle);
-    this->cos_gamma     = cos(gamma_angle);
+    float alpha_angle  = PIOVER180 * this->config->value(alpha_angle_checksum)->by_default(30.0)->as_number();
+    this->sin_alpha     = sinf(alpha_angle);
+    this->cos_alpha     = cosf(alpha_angle);
+    float beta_angle   = PIOVER180 * this->config->value( beta_relative_angle_checksum)->by_default(120.0)->as_number();
+    this->sin_beta      = sinf(beta_angle);
+    this->cos_beta      = cosf(beta_angle);
+    float gamma_angle  = PIOVER180 * this->config->value(gamma_relative_angle_checksum)->by_default(240.0)->as_number();
+    this->sin_gamma     = sinf(gamma_angle);
+    this->cos_gamma     = cosf(gamma_angle);
 
     // arm_length is the length of the arm from hinge to hinge
     this->arm_length         = this->config->value(arm_length_checksum)->by_default(250.0)->as_number();
     // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
     this->arm_radius         = this->config->value(arm_radius_checksum)->by_default(124.0)->as_number();
 
-    this->arm_length_squared = pow(this->arm_length, 2);
+    this->arm_length_squared = powf(this->arm_length, 2);
 }
 
 void RostockSolution::millimeters_to_steps( double millimeters[], int steps[] ){
-    double alpha_rotated[3], rotated[3];
+    float mm[3], alpha_rotated[3], rotated[3];
+    
+    // convert input to float
+    mm[0]= millimeters[0];
+    mm[1]= millimeters[1];
+    mm[2]= millimeters[2];
+    
     if( this->sin_alpha == 0 && this->cos_alpha == 1){
-        alpha_rotated[0] = millimeters[0];
-        alpha_rotated[1] = millimeters[1];
-        alpha_rotated[2] = millimeters[2];
+        alpha_rotated[0] = mm[0];
+        alpha_rotated[1] = mm[1];
+        alpha_rotated[2] = mm[2];
     }else{
-        rotate( millimeters, alpha_rotated, sin_alpha, cos_alpha );
+        rotate( mm, alpha_rotated, sin_alpha, cos_alpha );
     }
     steps[ALPHA_STEPPER] = lround( solve_arm( alpha_rotated ) * this->alpha_steps_per_mm );
 
@@ -46,11 +52,11 @@ void RostockSolution::millimeters_to_steps( double millimeters[], int steps[] ){
 
 void RostockSolution::steps_to_millimeters( int steps[], double millimeters[] ){} 
 
-double RostockSolution::solve_arm( double millimeters[]) {
-    return sqrt(arm_length_squared - pow(millimeters[X_AXIS] - this->arm_radius, 2) - pow(millimeters[Y_AXIS], 2)) + millimeters[Z_AXIS];
+float RostockSolution::solve_arm( float millimeters[]) {
+    return sqrtf(arm_length_squared - powf(millimeters[X_AXIS] - this->arm_radius, 2) - powf(millimeters[Y_AXIS], 2)) + millimeters[Z_AXIS];
 }
 
-void RostockSolution::rotate(double in[], double out[], double sin, double cos ){
+void RostockSolution::rotate(float in[], float out[], float sin, float cos ){
     out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS];
     out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS];
     out[Z_AXIS] = in[Z_AXIS];

diff --git a/src/modules/robot/arm_solutions/RostockSolution.h b/src/modules/robot/arm_solutions/RostockSolution.h
index c9e8c5a..c636387 100644 (file)
--- a/src/modules/robot/arm_solutions/RostockSolution.h
+++ b/src/modules/robot/arm_solutions/RostockSolution.h
@@ -27,24 +27,24 @@ class RostockSolution : public BaseSolution {
         void set_steps_per_millimeter( double steps[] );
         void get_steps_per_millimeter( double steps[] );
 
-        double solve_arm( double millimeters[] );
-        void rotate( double in[], double out[], double sin, double cos );
+        float solve_arm( float millimeters[] );
+        void rotate( float in[], float out[], float sin, float cos );
 
         Config* config;
-        double alpha_steps_per_mm;
-        double beta_steps_per_mm;
-        double gamma_steps_per_mm;
-
-        double arm_length;
-        double arm_radius;
-        double arm_length_squared;
-
-        double sin_alpha;
-        double cos_alpha;
-        double sin_beta;
-        double cos_beta;
-        double sin_gamma;
-        double cos_gamma;
+        float alpha_steps_per_mm;
+        float beta_steps_per_mm;
+        float gamma_steps_per_mm;
+
+        float arm_length;
+        float arm_radius;
+        float arm_length_squared;
+
+        float sin_alpha;
+        float cos_alpha;
+        float sin_beta;
+        float cos_beta;
+        float sin_gamma;
+        float cos_gamma;
 };