1 files changed, 495 insertions, 0 deletions
diff --git a/Other_Arduino_Codes/TwoNema17-2DM542-OSP-Plugin-Test/OSP.cpp b/Other_Arduino_Codes/TwoNema17-2DM542-OSP-Plugin-Test/OSP.cpp
new file mode 100644
index 0000000..48abd91
--- /dev/null
+++ b/Other_Arduino_Codes/TwoNema17-2DM542-OSP-Plugin-Test/OSP.cpp
@@ -0,0 +1,495 @@
+// #include "WProgram.h" // Arduino < 1.0
+#include <Arduino.h> //Arduino >= 1.0
+#include "OSP.h"
+
+//int steps;
+int dir1, step1, dir2, step2; 
+float stepx;
+float stepy;
+//int low;
+//int high;
+int x_rev_set = 0;
+int y_rev_set = 0;
+float valx;
+float valy;
+float _pgrad_x;
+float _pgrad_y;
+//bool blind;
+//int diff_x;
+//int diff_y;
+float stepx_new;
+float stepy_new;
+float degsH;
+float degsV;
+float degsH_new;
+float degsV_new;
+//int slow=50;
+//int xcount;
+//int ycount;
+
+
+OSP::OSP(){
+    stopper=0;
+}
+
+void OSP::setMotorsPins(int step1, int dir1, int enable1, int step2, int dir2, int enable2, int ledPin, int ddir1, int ddir2, int motor1, int motor2, int sensor1, int sensor2)
+{
+
+	pinMode(step1, OUTPUT); // pinMode(pulPin, OUTPUT); 
+  pinMode(dir1, OUTPUT); // pinMode(dirPin, OUTPUT);
+  pinMode(enable1, OUTPUT); // pinMode(enblPin, OUTPUT);
+  pinMode(step2, OUTPUT); // pinMode(pulPin1, OUTPUT);
+  pinMode(dir2, OUTPUT);//   pinMode(dirPin1, OUTPUT);
+  pinMode(enable2, OUTPUT); //   pinMode(enblPin1, OUTPUT);
+  pinMode(ledPin, OUTPUT); // pinMode(ledPin, OUTPUT);
+  pinMode(sensor1, INPUT); // pinMode(sensor1, INPUT);
+  pinMode(sensor2, INPUT); // pinMode(sensor2, INPUT);
+  
+  digitalWrite(step1, LOW); // digitalWrite(pulPin, LOW);
+   digitalWrite(step2, LOW); // digitalWrite(pulPin1, LOW);
+  digitalWrite(ledPin, LOW); // digitalWrite(ledPin, LOW);
+  digitalWrite(enable1, LOW); // digitalWrite(enblPin, LOW);
+  digitalWrite(enable2, LOW); // digitalWrite(enblPin1, LOW);
+  digitalWrite(dir1, HIGH); //   digitalWrite(dirPin, LOW);
+digitalWrite(dir2, HIGH); // digitalWrite(dirPin1, LOW);
+
+  
+  //Serial.begin(9600);
+  Serial.println("Setting up Motor pins..done");
+  
+  digitalWrite(step1, HIGH); // digitalWrite(enblPin, HIGH);
+  delay(100);
+  digitalWrite(step1, LOW); // digitalWrite(enblPin, LOW);
+   digitalWrite(step2, HIGH); // digitalWrite(enblPin1, HIGH);
+  delay(100);
+  digitalWrite(step2, LOW); // digitalWrite(enblPin1, LOW);
+
+ 
+  motor1 = 10;//number of steps
+  motor2 = 20;//number of steps
+
+  ddir1=1;//dir 1 for CW 0 for CCW
+  ddir2=1;//dir 1 for CW 0 for CCW
+        delay(1000);
+}
+
+
+//#######################################
+void OSP::init()
+{
+  low = 0;
+  high = 3200;//Motor driver switch setting;
+
+  _pgrad_x=(float)((float)(high-low)/360);
+  _pgrad_y=(float)((float)(high-low)/180);
+
+  stepx=0;
+  stepy=0;
+  low = 0;
+high = 0;
+x_rev_set = 0;
+y_rev_set = 0;
+valx = 0;
+valy = 0;
+_pgrad_x = 0;
+_pgrad_y = 0;
+stepx_new = 0;
+stepy_new = 0;
+degsH = 0;
+degsV = 0;
+degsH_new = 0;
+degsV_new = 0;
+  
+  //Serial.println(_pgrad_x);
+  }
+
+//####################################
+  int OSP::getPX() //Steps per revolution on X axis
+  {
+        return x_rev_set = 3200;//Change this as per the switch setting on the driver
+    }
+//#######################################
+
+    int OSP::getPY() //Steps per revolution on Y axis
+  {
+        return y_rev_set = 3200;//Change this as per the switch setting on the driver
+    }
+    
+  //#####################################  
+	bool OSP::movx(bool dir, int step1, int dir1) //Move on X axis // ovx(bool dir, int pulPin, int dirPin)
+  {
+    int bytes_recv = 0;        
+    char comm[5]="nost";
+ 
+    comm[bytes_recv++] = Serial.read();
+    //Serial.print("value of comm is ");
+    //Serial.println(comm);
+    while(strcmp(comm, "stop")!=0 || stopper)
+  {
+    //Serial.println("movx");
+    //Serial.println(pulPin);
+    if(dir==1)
+    {
+      
+      if(stepx>=3200)
+      {
+     stepx=3200;
+      }
+        /*digitalWrite(dirPin, HIGH);
+        digitalWrite(pulPin, HIGH);
+        digitalWrite(pulPin, LOW); */
+        else
+        {
+        digitalWrite(dir1, HIGH);
+        digitalWrite(step1, HIGH);
+        delay(10); 
+        digitalWrite(step1, LOW);
+        delay(10);
+        
+        stepx++;
+        }
+        
+       //Serial.println(stepx);
+
+    }
+    else
+    {
+      if(stepx<=0)
+      {
+        stepx = 0;
+        }
+        else {
+          /*digitalWrite(dirPin, LOW);
+          digitalWrite(pulPin, HIGH);
+          digitalWrite(pulPin, LOW);*/ 
+          
+           digitalWrite(dir1, LOW);
+          digitalWrite(step1, HIGH);
+           delay(10); 
+           digitalWrite(step1, LOW);
+          delay(10);
+
+          stepx--;
+          
+        }
+             
+        //Serial.println(stepx);
+     }
+     float temp=stepx*360.0/3200.0;
+     Serial.println(temp);
+   //delay(50);
+   while(Serial.available() > 0)
+      comm[bytes_recv++] = Serial.read();
+                //Serial.println(comm);
+                //Serial.println("inside second while");
+    //Safeguard...
+    //Serial.println(stepx);
+    if(bytes_recv>4)
+    {
+      break;
+      
+    }
+   
+}
+
+
+  return true;
+    }
+
+    //#####################################  
+  bool OSP::movy(bool dir,  int step2, int dir2) //Move on Y axis 
+  // bool OSP::movy(bool dir,  int pulPin1, int dirPin1)
+  {
+     // Serial.print("stepx of axes ");    Serial.println(stepx);
+    int bytes_recv = 0;        
+    char comm[5]="nost";
+ 
+    comm[bytes_recv++] = Serial.read();
+    //Serial.print("value of comm is ");
+    //Serial.println(comm);
+    while(strcmp(comm, "stop")!=0)
+  {
+    if(dir==0)
+    {
+      if(stepy>=3200/4)
+      {
+     stepy=3200/4;
+      }
+      else
+      {
+    digitalWrite(dir2, LOW); // digitalWrite(dirPin1, HIGH);
+    digitalWrite(step2, HIGH); // digitalWrite(pulPin1, HIGH); 
+    delay(10);
+    digitalWrite(step2, LOW);  // digitalWrite(pulPin1, LOW);
+    delay(10); 
+    stepy++; 
+      }
+    //Serial.println(stepy);
+
+    }
+    else
+    {
+      if(stepy<=0)
+      {
+        stepy=0;
+      //Serial.println("stop");
+
+        }
+        else
+        {
+          digitalWrite(dir2, HIGH); // digitalWrite(dirPin1, LOW);
+          digitalWrite(step2, HIGH); // digitalWrite(pulPin1, HIGH);
+          delay(10); 
+          digitalWrite(step2, LOW);  // digitalWrite(pulPin1, LOW);
+          delay(10); 
+          stepy--; 
+        }
+      //Serial.println(stepy);
+
+      }
+      float temp=stepy*360.0/3200.0;
+     Serial.println(temp);
+
+ 
+   //delay(50);
+   while(Serial.available() > 0)
+      comm[bytes_recv++] = Serial.read();
+                //Serial.println(comm);
+                //Serial.println("inside second while");
+    //Safeguard...
+    //Serial.println(stepy);
+    if(bytes_recv>4)
+    {
+      break;
+      //
+    }
+   
+}
+
+      
+  return true;
+    }
+
+    //560 to 2420 << 590 2190
+//#################################
+
+float OSP::getX()
+{
+  /**
+       * Returns current position on X axis
+       * 
+       * \return X position as radians to stellarium
+       */
+
+       //Serial.println(stepx);
+  float valx;
+  //valx = map(stepx, 0, 3200, 0, 360);
+  valx = stepx*(float)(360.00/3200.00);
+  valx = (float)(M_PI/180)*valx;
+    //vlax = _deg2rad(valx);
+
+  return valx;
+}
+
+float OSP::getY()
+{
+   /**
+       * Returns current position on Y axis
+       * 
+       * \return Y position as radians to stellarium
+       */
+        //Serial.println(stepy);
+
+  float valy;
+
+  valy = stepy*(float)(360.00/3200.00);
+//  valy = map(stepy, 0, 3200/2, 0, 180);
+  valy = (float)(M_PI/180)*valy;
+//  //vlay=_deg2rad(valy);
+
+  return valy;
+}
+
+int OSP::getPx()
+{
+  /**
+       * Return the number of steps per revolution of the Y axis
+       *
+       * \return Steps per revolution
+       */
+
+    return stepx;
+  
+}
+
+int OSP::getPy()
+{
+    return stepy;
+}
+
+
+float OSP::_rad2deg(float rad){
+//    return lrint( (float) (180.00/M_PI)*rad);
+    return  (float) (180.00/M_PI)*rad;
+
+}
+
+float OSP::_deg2rad(float deg){
+    return (float) (M_PI/180.00)*deg;
+}
+
+/*
+void AxesLib::goToRads(float rx, float ry){
+  float degsH = (360.0 - _rad2deg(rx));
+  if(degsH >= 360.0)
+    degsH = degsH - 360.0;
+  
+  _moveTo((float) degsH*_pgrad_x, (float) _rad2deg(ry)*_pgrad_y);
+}*/
+
+//################################################
+
+//######################################
+void OSP::goToRads(float x_rad, float y_rad)//give this input for debugging> movem_-4.5434_-2.5254
+{
+  
+  float degsH = _rad2deg(x_rad);
+  float degsV = _rad2deg(y_rad);
+
+//if((degsH_new==degsH)
+//if(degsV_new==degsV))
+//if((degsH_new>degsH)
+//{
+//  }
+//if(degsV_new>degsV))
+//if((degsH_new<degsH)
+//if(degsV_new<degsV))
+//{
+//}
+//else if((degsH_new>degsH)||(degsV_new>degsV))
+//
+//{
+//  degsH = degsH_new;
+//  degsV = degsV_new;
+  //Serial.println(degsH);
+  //Serial.println(degsV);
+_moveTo(degsH,degsV);
+//  }
+
+  //degsH = degsH+180;
+  //degsV = 180-degsV;
+
+  
+}
+//##################################################
+void OSP::_moveTo(float x, float y, char* method)
+{
+  //stepx_new = map(abs(x), 0, 360, low, high);
+  //stepy_new = map(abs(y), 0, 180, low, high);
+ 
+
+  stepx_new = (3200*x)/360;//map to counts
+  stepy_new = (3200*y)/360;//map to counts
+
+//  if(((stepx-stepx_new)==0)||((stepy-stepy_new)==0))
+//  {
+//    stepx=0;
+//    stepy=0;
+//  }
+//  else
+//  {
+//    stepx=stepx_new-stepx;
+//    stepy=stepy_new-stepy;
+    //}
+
+//if((stepx_new>stepx)||(stepy_new>stepy))
+//{
+//  Serial.println("Forward");
+//      stepx=stepx_new-stepx;
+//      stepy=stepy_new-stepy;
+//  }
+//  else
+//  {  Serial.println("Reverse");
+//
+//      stepx=stepx-stepx_new;
+//      stepy=stepy-stepy_new;
+//    }
+
+stepx=stepx_new-stepx;
+stepy=stepy_new-stepy;
+
+ 
+  //Serial.println(stepx);
+  //Serial.println(stepy);
+  
+ //_moveXY(stepx, stepy);
+ //Serial.println("done_move");
+
+
+    //Serial.println("done_move");
+
+
+  
+  //Serial.println(low);
+  //Serial.println(high);
+  //Serial.println(stepx_new);
+  //Serial.println(stepy_new);
+
+      //_moveDDA(stepx_new, stepy_new);
+   _moveXY(stepx, stepy,true);
+    stepx=stepx_new;
+stepy=stepy_new;
+//   //_moveEqua(stepx_new, stepy_new);
+//   stepx=stepx_new;
+//   stepy=stepy_new;  
+}
+//##################################################
+void OSP::_moveXY(float x, float y, bool nodelay)
+{
+ 
+
+  if(y>3200.00/4) y = 3200.00/4; // This is to limit the Tilt Motor to 90 degree
+  
+  for(float count=0;count<abs(round(x));count++)
+  {
+    if(x<0)
+    {
+      digitalWrite(11, LOW); //       digitalWrite(11, LOW);
+    }
+    else
+    {
+      digitalWrite(11, HIGH); // digitalWrite(11, HIGH);
+    }
+    
+    digitalWrite(10, HIGH); // digitalWrite(10, HIGH);
+    delay(10); 
+    digitalWrite(10, LOW);  // digitalWrite(10, LOW);
+    //Serial.println(pulPin);
+ 
+    delay(10);
+    }
+
+    for(int count=0;count<abs(y);count++)
+  {
+    if(y<0)
+    {
+      digitalWrite(8, HIGH);// digitalWrite(4, LOW);
+    }
+    else
+    {
+      digitalWrite(8, LOW); // digitalWrite(4, HIGH);
+    }
+    
+    digitalWrite(7, HIGH); //  digitalWrite(3, HIGH);
+    delay(10); 
+    digitalWrite(7, LOW); // digitalWrite(3, LOW);
+    delay(10);
+    }
+
+  }
+
+void OSP::_moveEqua(int x, int y)
+  {
+    
+  }
+