Updated OSP-plugin

1 files changed, 280 insertions, 0 deletions

diff --git a/CodesArduino-for-plugin/TwoRelay/OSP.cpp b/CodesArduino-for-plugin/TwoRelay/OSP.cpp
new file mode 100644
index 0000000..21c5f28
--- /dev/null
+++ b/CodesArduino-for-plugin/TwoRelay/OSP.cpp
@@ -0,0 +1,280 @@
+// #include "WProgram.h" // Arduino < 1.0
+#include <Arduino.h> //Arduino >= 1.0
+#include "OSP.h"
+
+
+int dir1, step1, dir2, step2; 
+int stepx;
+int 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;
+
+int stepx_new;
+int stepy_new;
+float degsH;
+float degsV;
+float degsH_new;
+float degsV_new;
+
+
+OSP::OSP(){
+    stopper=0;
+}
+
+void OSP::setMotorsPins(int step1, int dir1, int enable1, int step2, int dir2, int enable2)
+{
+	pinMode(step1, OUTPUT);  
+  pinMode(dir1, OUTPUT); 
+  pinMode(enable1, OUTPUT); 
+  pinMode(step2, OUTPUT); 
+  pinMode(dir2, OUTPUT);
+  pinMode(enable2, OUTPUT); 
+  
+  digitalWrite(step1, LOW); 
+  digitalWrite(step2, LOW); 
+  digitalWrite(enable1, LOW); 
+  digitalWrite(enable2, LOW); 
+  delayMicroseconds(50); 
+  digitalWrite(dir1, HIGH); 
+  digitalWrite(dir2, HIGH); 
+
+  
+  Serial.println("Setting up Motor pins..done");
+  
+  digitalWrite(enable1, HIGH); 
+  delay(100);
+  digitalWrite(enable1, LOW); 
+  digitalWrite(enable2, HIGH); 
+  delay(100);
+  digitalWrite(enable2, LOW); 
+  delay(1000);
+}
+
+
+void OSP::init()
+{
+  low = 0;
+  high = 25600; //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;
+}
+
+int OSP::getPX() //Steps per revolution on X axis
+{
+  return x_rev_set = 25600; //Change this as per the switch setting on the driver
+}
+
+
+int OSP::getPY() //Steps per revolution on Y axis
+{
+  return y_rev_set = 25600; //Change this as per the switch setting on the driver
+}
+     
+bool OSP::movx(bool dir, int step1, int dir1) 
+  {
+    int bytes_recv = 0;        
+    char comm[5]="nost";
+    comm[bytes_recv++] = Serial.read();
+    while(strcmp(comm, "stop")!=0 || stopper)
+  {
+    if(dir==1)
+    {      
+      if(stepx>=25600)
+        stepx=25600;
+        
+      else {
+        digitalWrite(dir1, LOW);
+        digitalWrite(step1, HIGH);
+        delay(delayValue); 
+        digitalWrite(step1, LOW);
+        delay(delayValue);
+        stepx++;
+        }
+    }
+    else
+    {
+      if(stepx<=0)
+        stepx = 0;
+        else {
+         digitalWrite(dir1, HIGH);
+         digitalWrite(step1, HIGH);
+         delay(delayValue); 
+         digitalWrite(step1, LOW);
+         delay(delayValue);
+         stepx--;  
+        }
+     }
+   while(Serial.available() > 0)
+      comm[bytes_recv++] = Serial.read();
+    if(bytes_recv>4)
+      break;   
+}
+    return true;
+}
+  
+bool OSP::movy(bool dir,  int step2, int dir2) 
+{
+    int bytes_recv = 0;        
+    char comm[5]="nost";
+ 
+    comm[bytes_recv++] = Serial.read();
+    while(strcmp(comm, "stop")!=0)
+    {
+    if(dir==0)
+    {
+      if(stepy>=25600/4)
+        stepy=25600/4;
+      else {
+        digitalWrite(dir2, HIGH); 
+        digitalWrite(step2, HIGH); 
+        delay(delayValue);
+        digitalWrite(step2, LOW);  
+        delay(delayValue); 
+        stepy++; 
+      }
+    }
+    else
+    {
+      if(stepy<=0)
+        stepy=0;
+        else {
+          digitalWrite(dir2, LOW); 
+          digitalWrite(step2, HIGH); 
+          delay(delayValue); 
+          digitalWrite(step2, LOW);  
+          delay(delayValue); 
+          stepy--; 
+        }
+      }
+
+   while(Serial.available() > 0)
+      comm[bytes_recv++] = Serial.read();
+    if(bytes_recv>4)
+      break;  
+}   
+  return true;
+}
+
+float OSP::getX()
+{
+  float valx;
+  valx = stepx*(float)(360.00/25600.00);
+  valx = (float)(M_PI/180)*valx;
+  return valx;
+}
+
+
+float OSP::getY()
+{
+  float valy;
+  valy = stepy*(float)(360.00/25600.00);
+  valy = (float)(M_PI/180)*valy;
+  return valy;
+}
+
+
+int OSP::getPx()
+{
+  return stepx; 
+}
+
+
+int OSP::getPy()
+{
+  return stepy;
+}
+
+
+float OSP::_rad2deg(float rad){
+  return  (float) (180.00/M_PI)*rad;
+}
+
+float OSP::_deg2rad(float deg){
+  return (float) (M_PI/180.00)*deg;
+}
+
+void OSP::goToRads(float x_rad, float y_rad) 
+{
+  
+  float degsH = _rad2deg(x_rad);
+  float degsV = _rad2deg(y_rad);
+
+_moveTo(degsH,degsV);
+  
+}
+
+void OSP::_moveTo(float x, float y, char* method)
+{
+  stepx_new = (25600*x)/360; //map to counts
+  stepy_new = (25600*y)/360; //map to counts
+  
+  stepx=stepx_new-stepx;
+  stepy=stepy_new-stepy;
+  
+  _moveXY(stepx,stepy,true);
+  stepx=stepx_new;
+  stepy=stepy_new;  
+}
+
+void OSP::_moveXY(int x, int y, bool nodelay)
+{
+   if(y>25600/4) 
+      y = 25600/4; // This is to limit the tilt Motor to 90 degrees. 
+  
+  for(int count=0;count<abs(x);count++)
+  {
+    if(x<0)
+      digitalWrite(11, HIGH);      
+    else
+      digitalWrite(11, LOW); 
+        
+    digitalWrite(12, HIGH); 
+    delay(10); 
+    digitalWrite(12, LOW);  
+    delay(10);
+  }
+
+  for(int count=0;count<abs(y);count++)
+  {
+    if(y<0)
+      digitalWrite(8, LOW);
+    else
+      digitalWrite(8, HIGH); 
+    
+    digitalWrite(9, HIGH); 
+    delay(10); 
+    digitalWrite(9, LOW); 
+    delay(10);
+  }
+
+  }
+
+void OSP::_moveEqua(int x, int y)
+  {
+    
+  }
+