diff options
Diffstat (limited to 'CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test')
4 files changed, 758 insertions, 0 deletions
diff --git a/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/OSP.cpp b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/OSP.cpp new file mode 100644 index 0000000..3238c9b --- /dev/null +++ b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/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, HIGH); + digitalWrite(step1, HIGH); + delay(delayValue); + digitalWrite(step1, LOW); + delay(delayValue); + stepx++; + } + } + else + { + if(stepx<=0) + stepx = 0; + else { + digitalWrite(dir1, LOW); + 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, LOW); + digitalWrite(step2, HIGH); + delay(delayValue); + digitalWrite(step2, LOW); + delay(delayValue); + stepy++; + } + } + else + { + if(stepy<=0) + stepy=0; + else { + digitalWrite(dir2, HIGH); + 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, LOW); + else + digitalWrite(11, HIGH); + + digitalWrite(12, HIGH); + delay(10); + digitalWrite(12, LOW); + delay(10); + } + + for(int count=0;count<abs(y);count++) + { + if(y<0) + digitalWrite(8, HIGH); + else + digitalWrite(8, LOW); + + digitalWrite(9, HIGH); + delay(10); + digitalWrite(9, LOW); + delay(10); + } + + } + +void OSP::_moveEqua(int x, int y) + { + + } + diff --git a/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/OSP.h b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/OSP.h new file mode 100644 index 0000000..12dd770 --- /dev/null +++ b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/OSP.h @@ -0,0 +1,234 @@ +#ifndef OSP_h +#define OSP_h +#include <math.h> +#include <string.h> + + /** + * \brief Class that manages movements and the laser of the device + * + * Uses the stepper motors and sensors to positioning the device at a given horizontal coordinates, + * within a range of 360º degrees in horizontal, and 180º on vertical. + * To the movements uses the DDA algorithm (Digital Differential Algorithm). + */ + +class OSP{ + public: + int delayValue; + int step1, dir1, enable1, step2, dir2, enable2; + int stepx, stepy; + int low; + int high; + int stopper; + float degsH; + float degsV; + + /** + * Pins to control the stepper motors + */ + + /** + * Steps per degree on each axis + */ + float _pgrad_x, _pgrad_y; + + /** + * Current position of each axis (in radians) + */ + float _rx, _ry; + + /** + * Auxiliary variables, maximum theoric values, and steps per revolution respectively + */ + int _x, _y, _X, _Y, _pv_x, _pv_y; + + /** + * Theoric central values of each axis + */ + int _revx, _topy; + + /** + * Indicates if X axis has been "reverted" (Y is between 90º and 180º) + */ + bool _x_rev; + + /** + * Sensor pins + */ + int _s0Pin_x, _s360Pin_x, _sbottomPin_y, _stopPin_y; + + /** + * Transforms radian to degrees + * + * \param rad Radians + * \return degrees + */ + float _rad2deg(float rad); + + /** + * Degrees to radians + * + * \param deg Degrees + * \return radians + */ + float _deg2rad(float deg); + + /** + * Moves one of the motors a given number of steps + * + * \param axis Pin of the motor to move + * \param dir Direction: True means clockwise direction on X, and upwards on Y + * \param steps Number of steps (if limit sensor is not reached) + * \param sensor Pin of the sensor that can be reached towards that direction + * \param nodelay Skip the initial delay (useful to DDA algorithm) + * \return Number of steps (distinct of the steps parameter if the sensor has been reached) + */ + //int _step(int axis, bool dir, int steps, int sensor, bool nodelay=false); + + /** + * Enables the motors power supply + */ + void _enableMotors(); + + /** + * Disables the motors power supply + */ + void _disableMotors(); + + /** + * Moves the device to the given position + * + * \param x Number of steps from 0 to the desired position on X axis + * \param y Number of steps from 0 to the desired position on Y axis + * \param method Algorithm selection: DDA or XY (first X axis, then Y), by default is DDA + */ + void _moveTo(float x, float y, char* method = "DDA"); + + /** + * Moves the device to the given position, first X axis and then Y axis + * + * \param x Number of steps from 0 to the desired position on X axis + * \param y Number of steps from 0 to the desired position on Y axis + * \param nodelay Omits the delay on changes of axis or direction + */ + void _moveXY(int x, int y, bool nodelay=false); + + /** + * Moves the device to the given position using DDA algorithm + * + * \param x Number of steps from 0 to the desired position on X axis + * \param y Number of steps from 0 to the desired position on Y axis + */ + void _moveDDA(int x, int y); + //void _moveDDA(int x, int y, float dirx, float diry); + + public: + /** + * Class constructor + */ + OSP(); + + /** + * Initializes the device + * + * Along the process obtains the number of steps on each axis, and calculates the steps + * per degree for positioning + */ + void init(); + + /** + * Returns current position on X axis + * + * \return X position as radians + */ + float getX(); + + /** + * Returns current position on Y axis + * + * \return Y position as radians + */ + float getY(); + + /** + * Return the number of steps per revolution of the X axis + * + * \return Steps per revolution + */ + int getPX(); + + /** + * Number of steps from 0º to current position on X axis + * + * \return Current position on X + */ + int getPx(); + + /** + * Return the number of steps per revolution of the Y axis + * + * \return Steps per revolution + */ + int getPY(); + + /** + * Number of steps from 0º to current position on Y axis + * + * \return Current position on Y + */ + int getPy(); + + /** + * Sets the pins to control the stepper motors + * + * \param stPin_x Pin to move the X axis + * \param stPin_y Pin to move the Y axis + * \param dirPin Direction: True means clockwise direction on X axis, and downwards on Y + * \param enable_x Turn On/Off power supply on X axis motor + * \param enable_y Turn On/Off power supply on Y axis motor + * + */ + +void setMotorsPins(int step1, int dir1, int enable1, int step2, int dir2, int enable2); + + + /** + * Sets the pins connected to the sensors + * + * \param s0Pin_x Pin for the 0º limit sensor on X axis + * \param s360Pin_x Pir for the 360º limit sensor on X axis + * \param sbottomPin_y Pin for the 0º limit sensor on Y axis + * \param stopPin_y Sensor for the 90º limit sensor on Y axis + */ + void setSensorsPins(int s0Pin_x, int s360Pin_x, int sbottomPin_y, int stopPin_y); + + /** + * Points the device towards the given coordinates + * + * \param rx Radians for the X axis, on range of 0 - 2*Pi + * \param ry Radians for the Y axis: on range of 0 - Pi + */ + void goToRads(float rx, float ry); + + /** + * Accelerated movement for X axis + * + * The movement stops when a 'stop' command is received by the serial port + * + * \param dir Direction: True means clockwise direction + * \return Returns true in case of reaches a limit sensor + */ + + bool movx(bool dir, int step1, int dir1); + /** + * Accelerated movement for Y axis + * + * The movement stops when a 'stop' command is received by the serial port + * + * \param dir Direction: True means upwards + * \return Returns true in case of reaches a limit sensor + */ + + bool movy(bool dir, int step2, int dir2); + void OSP::_moveEqua(int x, int y); + }; +#endif diff --git a/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/Readme b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/Readme new file mode 100644 index 0000000..d48f957 --- /dev/null +++ b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/Readme @@ -0,0 +1,15 @@ +This code is used to test OSP plugin with NEMA17 motors and 2DM542 microstep driver. + +The connections for one motor are as under: +1. The pins A+ A- B+ and B- are connected with the four pins of NEMA17. +2. V+ and GND are connected to 24V power supply terminals. GND is shorted with the GND of Arduino. +3. ENA- DIR- and PLS- are connected to GND of Arduino. +4. PLS+ --> D10 of Arduino. +5. DIR+ --> D11 of Arduino. +6. ENA+ --> D12 of Arduino. +7. RS232 pins were completely OFF. +8. The swicthes on the drivers were adjusted for 3200 steps per revolution and 1A current supply. + +Both the drivers were powered by a single 24V DC supply. + + diff --git a/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/TwoNema17-2DM542-OSP-Plugin-Test.ino b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/TwoNema17-2DM542-OSP-Plugin-Test.ino new file mode 100644 index 0000000..17c7a08 --- /dev/null +++ b/CodesArduino-for-plugin/TwoNema17-2DM542-OSP-Plugin-Test/TwoNema17-2DM542-OSP-Plugin-Test.ino @@ -0,0 +1,229 @@ +#include <stdio.h> +#include <string.h> +#include <math.h> +#include "OSP.h" + +/** + * Declarations for stepper motor 1 (at the base) + * All the pins below are connected to digital pins of Arduino Nano. + */ + +#define dir1 11 +#define step1 12 +#define enable1 10 + +#define relay1 2 +#define relay2 4 +#define relay3 5 +#define relay4 6 + +/** + * Declarations for stepper motor 2 (at top holding LASER) + */ +#define dir2 8 +#define step2 9 +#define enable2 7 + +/** + * Digital pin to control the LASER + * This pin is connected to gate (G) of IRF540 + */ +int laserPin = 3; + + +/** + * Library for control the device. + */ +OSP Axes = OSP(); +String m_data, sac_data, salt_data; // to store LASER coorinates + +void setup(){ + Serial.begin(9600); + pinMode(relay1,OUTPUT); + pinMode(relay2,OUTPUT); + pinMode(relay3,OUTPUT); + pinMode(relay4,OUTPUT); + + digitalWrite(relay1, HIGH); + digitalWrite(relay2, HIGH); + digitalWrite(relay3, HIGH); + digitalWrite(relay4, HIGH); + pinMode(laserPin, OUTPUT); + laserOff(); + Axes.setMotorsPins(dir1, step1, enable1, dir2, step2, enable2); + Axes.delayValue = 30; // to adjust motor speed as Coarse and Fine + + } + + +/** + * Turn LASER on at given intensity + */ +void laserOn(int intensity){ + analogWrite(laserPin, intensity); + } + +/** + * Turn LASER Off + */ +void laserOff(){ + digitalWrite(laserPin, LOW); +} + + +void getAcAlt(){ + char bytes[40]; + int nbytes = 0; + bool recv=false; + while(!recv){ + while(nbytes < 17){ + if(Serial.available() > 0){ + bytes[nbytes] = Serial.read(); + nbytes++; + } + recv=true; + } + } + + +/** + * As LASER coordinates, we receive string like movem_-4.5434_-2.5254. + * This string is chunked to extract ac and alt values. + */ + + String data= String(bytes); + data.trim(); + int len=data.length(); + m_data=data; + sac_data=data; + salt_data=data; + m_data.remove(1,len-1); + sac_data.remove(0,2); + len=sac_data.length(); + sac_data.remove(7,len-7); + len=salt_data.length(); + salt_data=salt_data.substring(10,17); +} + + +void loop(){ + + + float ac, alt; + char comm[5]; + int bytes_recv = 0; + bool mov_end; + comm[4]='\0'; + while(bytes_recv < 4){ + //Waiting for a command from serial monitor + if (Serial.available() > 0) + comm[bytes_recv++] = Serial.read(); + //Serial.println("here"); + //Serial.println(analogRead(smps)); + } + + + if(strcmp(comm, "move")==0){ + Serial.println("float"); + getAcAlt(); + Serial.println("done_move"); + Axes.goToRads(sac_data.toFloat(),salt_data.toFloat()); + } + + if(strcmp(comm, "movl")==0){ + mov_end=Axes.movx(false,step1,dir1); + if(mov_end) + Serial.println("done_movl"); + } + + else if(strcmp(comm, "movr")==0){ + mov_end=Axes.movx(true,step1,dir1); + if(mov_end) + Serial.println("done_movr"); + } + + else if(strcmp(comm, "movu")==0){ + mov_end=Axes.movy(false,step2,dir2); + if(mov_end) + Serial.println("done_movu"); + } + + else if(strcmp(comm, "movd")==0){ + mov_end=Axes.movy(true,step2,dir2); + if(mov_end) + Serial.println("done_movd"); + } + + else if(strcmp(comm, "init")==0){ + delay(1000); + digitalWrite(relay1,LOW); + digitalWrite(relay2,LOW); + digitalWrite(relay3,LOW); + digitalWrite(relay4,LOW); + Axes.init(); + Serial.println("done_init"); + } + + else if(strcmp(comm, "laon")==0){ + laserOn(50); + Serial.println("done_laon"); + } + + else if(strcmp(comm, "loff")==0){ + laserOff(); + Serial.println("done_loff"); + } + else if(strcmp(comm, "post")==0){ + ac=Axes.getX(); + alt=Axes.getY(); + Serial.print("t_"); + Serial.print(ac,6); + Serial.print("_"); + Serial.println(alt,6); + } + + else if(strcmp(comm,"stop")==0){ + Axes.stopper=1; + } + + else if(strcmp(comm,"coad")==0){ + Axes.delayValue = 3; + Serial.println("done_high_speed"); + } + + else if(strcmp(comm,"fiad")==0){ + Axes.delayValue = 30; + Serial.println("done_low_speed"); + } + else if(strcmp(comm,"clos")==0){ + digitalWrite(relay1, HIGH); + digitalWrite(relay2, HIGH); + digitalWrite(relay3, HIGH); + digitalWrite(relay4, HIGH); + laserOff(); + Serial.println("done_close"); + + } + else if(strcmp(comm,"lase")==0){ + Serial.println("done_lase"); + char bytes[40]; + int nbytes = 0; + bool recv=false; + while(!recv){ + while(nbytes < 4){ + if(Serial.available() > 0){ + bytes[nbytes] = Serial.read(); + nbytes++; + } + recv=true; + } + } + String data= String(bytes); + int intensity=data.toInt(); + laserOn(intensity); + } + + else{ + } +} + |