From a04e2a950bace2c486753f25727093fbe9ef25e5 Mon Sep 17 00:00:00 2001
From: isaac879 <isaac879@live.com>
Date: Fri, 5 Jun 2020 03:02:35 +0100
Subject: [PATCH] Add files via upload

---
 pan_tilt_mount_nano_code_pcb/panTiltMount.cpp | 19 +++++++++++--------
 pan_tilt_mount_nano_code_pcb/panTiltMount.h   |  2 +-
 .../pan_tilt_mount_nano_code_pcb.ino          |  9 +--------
 3 files changed, 13 insertions(+), 17 deletions(-)

diff --git a/pan_tilt_mount_nano_code_pcb/panTiltMount.cpp b/pan_tilt_mount_nano_code_pcb/panTiltMount.cpp
index 37dab54..295476c 100644
--- a/pan_tilt_mount_nano_code_pcb/panTiltMount.cpp
+++ b/pan_tilt_mount_nano_code_pcb/panTiltMount.cpp
@@ -814,9 +814,10 @@ bool calculateTargetCoordinate(void){
         intercept.x = (c2 - c1) / (m1 - m2);
         intercept.y = m1 * intercept.x + c1;
     }
-    intercept.z = tan(degToRads(tiltStepsToDegrees(keyframe_array[0].tiltStepCount))) * sqrt(pow(intercept.x, 2) + pow(intercept.y, 2));
+    intercept.z = tan(degToRads(tiltStepsToDegrees(keyframe_array[0].tiltStepCount))) * sqrt(pow(intercept.x - sliderStepsToMillimetres(keyframe_array[0].sliderStepCount), 2) + pow(intercept.y, 2));
     if(((panStepsToDegrees(keyframe_array[0].panStepCount) > 0 && panStepsToDegrees(keyframe_array[1].panStepCount) > 0) && intercept.y < 0)
-    || ((panStepsToDegrees(keyframe_array[0].panStepCount) < 0 && panStepsToDegrees(keyframe_array[1].panStepCount) < 0) && intercept.y > 0)){
+    || ((panStepsToDegrees(keyframe_array[0].panStepCount) < 0 && panStepsToDegrees(keyframe_array[1].panStepCount) < 0) && intercept.y > 0) || intercept.y == 0){
+        printi(F("Invalid intercept.\n"));
         return false;
     }
     return true;
@@ -830,7 +831,6 @@ void interpolateTargetPoint(FloatCoordinate targetPoint, int repeat){
         return; //check there are posions to move to
     }
     
-    float increment = 1;
     float sliderStartPos = sliderStepsToMillimetres(keyframe_array[0].sliderStepCount); //slider start position
     float sliderEndPos = sliderStepsToMillimetres(keyframe_array[1].sliderStepCount);
     float panAngle = 0;
@@ -838,9 +838,10 @@ void interpolateTargetPoint(FloatCoordinate targetPoint, int repeat){
     float x = targetPoint.x - sliderStepsToMillimetres(keyframe_array[0].sliderStepCount);
     float ySqared = pow(targetPoint.y, 2);    
     float sliderTravel = sliderStepsToMillimetres(keyframe_array[1].sliderStepCount) - sliderStepsToMillimetres(keyframe_array[0].sliderStepCount);
-    unsigned int numberOfIncrements = sliderTravel / increment;
-
-    for(int j = 0; j < repeat; j++){
+    int numberOfIncrements = abs(sliderTravel);
+    float increment = sliderTravel / numberOfIncrements;//size of interpolation increments in mm
+    
+    for(int j = 0; (j < repeat || (repeat == 0 && j == 0)); j++){
         for(int i = 0; i <= numberOfIncrements; i++){
             x = targetPoint.x - (sliderStartPos + increment * i);
             panAngle = radsToDeg(atan2(targetPoint.y, x));
@@ -861,8 +862,10 @@ void interpolateTargetPoint(FloatCoordinate targetPoint, int repeat){
             setTargetPositions(panAngle, tiltAngle, sliderStartPos + increment * i);
             multi_stepper.runSpeedToPosition();//blocking move to the next position
         }
-        setTargetPositions(panAngle, tiltAngle, sliderStartPos);
-        multi_stepper.runSpeedToPosition();//blocking move to the next position   
+        if(repeat > 0){
+            setTargetPositions(panAngle, tiltAngle, sliderStartPos);
+            multi_stepper.runSpeedToPosition();//blocking move to the next position 
+        }     
     }
 }
 
diff --git a/pan_tilt_mount_nano_code_pcb/panTiltMount.h b/pan_tilt_mount_nano_code_pcb/panTiltMount.h
index 7d0249d..4a63e61 100644
--- a/pan_tilt_mount_nano_code_pcb/panTiltMount.h
+++ b/pan_tilt_mount_nano_code_pcb/panTiltMount.h
@@ -98,7 +98,7 @@
 #define NUM_LEDS 1
 #define BRIGHTNESS 255
 
-#define VERSION_NUMBER "3.2.0"
+#define VERSION_NUMBER "3.2.5"
 
 /*------------------------------------------------------------------------------------------------------------------------------------------------------*/
 
diff --git a/pan_tilt_mount_nano_code_pcb/pan_tilt_mount_nano_code_pcb.ino b/pan_tilt_mount_nano_code_pcb/pan_tilt_mount_nano_code_pcb.ino
index 5aa02f4..03dd0b5 100644
--- a/pan_tilt_mount_nano_code_pcb/pan_tilt_mount_nano_code_pcb.ino
+++ b/pan_tilt_mount_nano_code_pcb/pan_tilt_mount_nano_code_pcb.ino
@@ -18,19 +18,12 @@
  * 
  * Code written by isaac879
  * 
- * Last modified: 04/05/2020
+ * Last modified: 05/06/2020
  *
  *--------------------------------------------------------------------------------------------------------------------------------------------------------*/
 //TODOs:
 //EEPROM values will need to be set after uploading the code and then a reset is required.
 
-/*----------Future development----------*/
-
-//program array in deg
-//Make an AT+ command set
-//report commands
-//Refactor code
-
 /*--------------------------------------------------------------------------------------------------------------------------------------------------------*/
 
 #include "panTiltMount.h"