IK implementation

MicroRobotArm/Constants.h

@@ -8,10 +8,11 @@
 #define Constants_h
 #include "Arduino.h"
 
-#define EXECUTING_ISR_CODE 5
+#define EXECUTING_ISR_CODE 34
 // NOTE: SineStepper and MoveBatch ids must be lower then MAX_NUM_OF_STEPPERS
 #define MAX_NUM_OF_STEPPERS 10
 #define MAX_NUM_OF_BATCHED_MOVES 20
 #define M_PI 3.14159265358979323846
 #define M_PI_2 1.57079632679489661923
+
 #endif

MicroRobotArm/MicroRobotArm.ino

@@ -9,10 +9,14 @@
 #include "SineStepperController.h"
 #include "Queue.h"
 #include "MoveBatch.h"
+#include "RobotArmIK.h"
 
 SineStepper sineStepper1(/*pinStep:*/ 2, /*pinDir:*/ 4, /*id:*/ 0);
-SineStepper sineStepper2(/*pinStep:*/ 19, /*pinDir:*/ 21, /*id:*/ 1);
+SineStepper sineStepper2(/*pinStep:*/ 16, /*pinDir:*/ 17, /*id:*/ 1);
+SineStepper sineStepper3(/*pinStep:*/ 5, /*pinDir:*/ 18, /*id:*/ 2);
+SineStepper sineStepper4(/*pinStep:*/ 19, /*pinDir:*/ 21, /*id:*/ 3);
 SineStepperController sineStepperController(/*endlessRepeat:*/ true);
+RobotArmIK robotArmIK(43.0, 60.0, 70.0, 54.0);
 
 hw_timer_t *timer = NULL;
 portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
@@ -36,25 +40,50 @@ void setup()
   timerSemaphore = xSemaphoreCreateBinary();
   sineStepperController.attach(&sineStepper1);
   sineStepperController.attach(&sineStepper2);
+  sineStepperController.attach(&sineStepper3);
+  sineStepperController.attach(&sineStepper4);
 
   // initialize MoveBatches
   MoveBatch mb;
-  mb.addMove(/*id:*/ 0, /*pos:*/ 70);
-  mb.addMove(/*id:*/ 1, /*pos:*/ 9);
-  mb.moveDuration = 1.0;
+  //mb.addMove(/*id:*/ 0, /*pos:*/ 0);
+  //mb.addMove(/*id:*/ 1, /*pos:*/ 0);
+  //mb.addMove(/*id:*/ 2, /*pos:*/ 0);
+  //mb.addMove(/*id:*/ 3, /*pos:*/ 0);
+  //mb.moveDuration = 1;
+  //sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(114.0, 20.0, 0.0, mb);
+  mb.moveDuration = 2;
   sineStepperController.addMoveBatch(mb);
 
-  mb.addMove(/*id:*/ 0, /*pos:*/ -130);
-  mb.moveDuration = 1.0;
+  mb = robotArmIK.RunIK(84.0, 20.0, 0.0, mb);
   sineStepperController.addMoveBatch(mb);
 
-  mb.addMove(/*id:*/ 0, /*pos:*/ 256);
-  mb.moveDuration = 1.0;
+  mb = robotArmIK.RunIK(114.0, 20.0, 0.0, mb);
   sineStepperController.addMoveBatch(mb);
 
-  mb.addMove(/*id:*/ 0, /*pos:*/ 0);
-  mb.addMove(/*id:*/ 1, /*pos:*/ 0);
-  mb.moveDuration = 1.0;
+  mb = robotArmIK.RunIK(94.0, 20.0, 0.5, mb);
+  sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(94.0, 20.0, -0.5, mb);
+  sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(94.0, 20.0, 0.0, mb);
+  sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(94.0, 50.0, 0.0, mb);
+  sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(134.0, 50.0, M_PI_2, mb);
+  sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(134.0, 80.0, M_PI_2, mb);
+  sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(134.0, 50.0, M_PI_2, mb);
+  sineStepperController.addMoveBatch(mb);
+
+  mb = robotArmIK.RunIK(94.0, 50.0, 0.0, mb);
   sineStepperController.addMoveBatch(mb);
 
   pinMode(EXECUTING_ISR_CODE, OUTPUT);
@@ -77,15 +106,16 @@ void loop()
     pos = sineStepper1.currentPos;
     portEXIT_CRITICAL(&timerMux);
 
-    Serial.print("pos: ");
-    Serial.println(pos);
+    //Serial.print("pos: ");
+    //Serial.println(pos);
     delay(10);
   }
 }
 
 // MEMO:
 //
 // TODO:
+// - calculate max possible stepsToTake without loosing steps.
 //
 // DOING:
 //

MicroRobotArm/MoveBatch.h

@@ -12,7 +12,7 @@
 struct MoveCommand
 {
   bool isActive;
-  int32_t positon;
+  int32_t position;
 };
 
 class MoveBatch

MicroRobotArm/RobotArmIK.cpp

@@ -0,0 +1,83 @@
+/*
+  RobotArmIK library
+  Author: T-Kuhn.
+  Sapporo, September, 2018. Released into the public domain.
+  */
+
+#include "Arduino.h"
+#include "RobotArmIK.h"
+#include "Constants.h"
+#include "MoveBatch.h"
+
+// - - - - - - - - - - - - - - -
+// - - - - CONSTRUCTOR - - - - -
+// - - - - - - - - - - - - - - -
+RobotArmIK::RobotArmIK(double link1, double link2, double link3, double link4)
+{
+    // Setup link length.
+    _link1 = link1;
+    _link2 = link2;
+    _link3 = link3;
+    _link4 = link4;
+}
+
+// - - - - - - - - - - - - - - -
+// - - - - - RUN IK  - - - - - -
+// - - - - - - - - - - - - - - -
+MoveBatch RobotArmIK::RunIK(double x, double y, double ohm, MoveBatch mb)
+{
+    Point2D _P_A, _P_B, _P_C;
+    double _g, _f;
+    double _phi, _c, _d, _e;
+    double _lambda1, _lambda2, _lambda3;
+    double _gamma, _alpha, _beta;
+    // Note how there's no z-coordinate.
+
+    // 1. Set up P_endeffector and the angle "ohm"
+    Point2D _P_endeffector = {x, y};
+    double _ohm = ohm;
+
+    // 2. Calculate point A
+    Point2D P_origin = {0.0, 0.0};
+
+    _P_A = {P_origin.x,
+            P_origin.y + _link1};
+
+    // 3. Calculate point B
+    _g = cos(_ohm) * _link4;
+    _f = sin(_ohm) * _link4;
+    _P_B = {_P_endeffector.x - _f,
+            _P_endeffector.y + _g};
+
+    // 4. Calculate angle phi
+    _d = _P_B.x - _P_A.x;
+    _e = _P_B.y - _P_A.y;
+    _phi = atan2(_e, _d);
+
+    // 5. Calculate the length of side c
+    _c = sqrt(_d * _d + _e * _e);
+
+    // 6. Calculate angles gamma, alpha and beta
+    _gamma = acos((_link2 * _link2 + _link3 * _link3 - _c * _c) / (2 * _link2 * _link3));
+    _alpha = acos((_link2 * _link2 + _c * _c - _link3 * _link3) / (2 * _link2 * _c));
+    _beta = M_PI - _gamma - _alpha;
+
+    // 7. Calculate angles lambda1, lambda2 and lambda3
+    _lambda1 = M_PI_2 - (_phi + _alpha);
+    _lambda2 = M_PI - _gamma;
+    _lambda3 = M_PI - ((M_PI_2 - _phi) + _beta) - _ohm; // TODO: This looks fishy. Make sure this is correct.
+
+    mb.addMove(/*id:*/ 0, /*pos:*/ (int32_t)(2048 * _lambda1 / M_PI));
+    mb.addMove(/*id:*/ 1, /*pos:*/ (int32_t)(2048 * _lambda2 / M_PI));
+    mb.addMove(/*id:*/ 2, /*pos:*/ (int32_t)(2048 * _lambda3 / M_PI));
+    /*
+    Serial.print("lambda1: ");
+    Serial.println(_lambda1);
+    Serial.print("lambda2: ");
+    Serial.println(_lambda2);
+    Serial.print("lambda3: ");
+    Serial.println(_lambda3);
+    */
+
+    return mb;
+}

MicroRobotArm/RobotArmIK.h

@@ -0,0 +1,29 @@
+/*
+  RobotArmIK library
+  Author: T-Kuhn.
+  Sapporo, September, 2018. Released into the public domain.
+  */
+
+#ifndef RobotArmIK_h
+#define RobotArmIK_h
+#include "Constants.h"
+#include "Arduino.h"
+#include "MoveBatch.h"
+
+struct Point2D
+{
+  double x;
+  double y;
+};
+
+class RobotArmIK
+{
+public:
+  RobotArmIK(double link1, double link2, double link3, double link4);
+  MoveBatch RunIK(double x, double y, double ohm, MoveBatch mb);
+
+private:
+  double _link1, _link2, _link3, _link4;
+};
+
+#endif

MicroRobotArm/SineStepper.cpp

@@ -47,12 +47,12 @@ void SineStepper::setGoalPos(int32_t goalPos)
 
     if (_currentStepsToTake > 0)
     {
-        digitalWrite(_pinDir, HIGH);
+        digitalWrite(_pinDir, LOW);
         _isMovingCW = true;
     }
     else
     {
-        digitalWrite(_pinDir, LOW);
+        digitalWrite(_pinDir, HIGH);
         _isMovingCW = false;
     }
 }

MicroRobotArm/SineStepperController.cpp

@@ -89,7 +89,7 @@ void SineStepperController::update()
             {
                 if (mb.batch[i].isActive)
                 {
-                    _sineSteppers[i]->setGoalPos(mb.batch[i].positon);
+                    _sineSteppers[i]->setGoalPos(mb.batch[i].position);
                 }
                 else
                 {