/cnc.ino
C++ | 941 lines | 535 code | 368 blank | 38 comment | 76 complexity | fd823a44c28c90c41589c381469aed81 MD5 | raw file
- //#include <LiquidCrystal.h>
- //#include <Servo.h>
- #include <TimerOne.h>
- //Servo myservo;
- int pos = 0; // variable to store the servo position
- int axisspeed = 10;
- int zaxisspeed = 10;
- int extrudspeed= 500;
- int xcoila1 = 2;
- int xcoila2 = 3;
- int xcoilb1 = 4;
- int xcoilb2 = 5;
- int ycoila1 = 6;
- int ycoila2 = 7;
- int ycoilb1 = 8;
- int ycoilb2 = 9;
- int zcoila1 = 10;
- int zcoila2 = 11;
- int zcoilb1 = 12;
- int zcoilb2 = 13;
- int ecoila1 = 46;
- int ecoila2 = 48;
- int ecoilb1 = 50;
- int ecoilb2 = 52;
- int x=0;
- int y=0;
- int z=0;
- int extrudOn = 0;
- //int xdir=1;
- //int ydir=0;
- //int zdir=0;
- //Servo myservo;
- String inputString = ""; // a string to hold incoming data
- String xstr = "";
- String ystr = "";
- String zstr = "";
- String tempstr = "";
- String extrudestr = "";
- int Sxlimit1 = HIGH;
- int Sylimit1 = HIGH;
- int Sxlimit2 = HIGH;
- int Sylimit2 = HIGH;
- int homed = 0;
- int limiterror = 0;
- int curTemp = 0;
- int tempSet = 29;
- int heatPin = 40;
- double tempC;
- boolean stringComplete = false; // whether the string is complete
- int turn = 0;
-
- void setup()
- {
-
-
- Timer1.initialize(100000); // set a timer of length 100000 microseconds (or 0.1 sec - or 10Hz => the led will blink 5 times, 5 cycles of on-and-off, per second)
- Timer1.attachInterrupt( extrudturn ); // attach the service routine here
-
- // myservo.attach(52);
- //myservo.write(90);
-
-
- pinMode(21, INPUT);
- pinMode(20, INPUT);
- pinMode(19, INPUT);
- pinMode(18, INPUT);
-
- pinMode(heatPin, OUTPUT);
-
- Sylimit1 = digitalRead(21);
- Sxlimit1 = digitalRead(20);
- Sylimit2 = digitalRead(19);
- Sxlimit2 = digitalRead(18);
-
-
- //myservo.attach(22);
-
- attachInterrupt(2, ylimit1On, LOW);
- attachInterrupt(4, ylimit2On, LOW);
-
- attachInterrupt(3, xlimit1On, LOW);
- attachInterrupt(5, xlimit2On, LOW);
-
- pinMode(xcoila1, OUTPUT);
- pinMode(xcoila2, OUTPUT);
- pinMode(xcoilb1, OUTPUT);
- pinMode(xcoilb2, OUTPUT);
-
- pinMode(ycoila1, OUTPUT);
- pinMode(ycoila2, OUTPUT);
- pinMode(ycoilb1, OUTPUT);
- pinMode(ycoilb2, OUTPUT);
-
- pinMode(zcoila1, OUTPUT);
- pinMode(zcoila2, OUTPUT);
- pinMode(zcoilb1, OUTPUT);
- pinMode(zcoilb2, OUTPUT);
-
- pinMode(ecoila1, OUTPUT);
- pinMode(ecoila2, OUTPUT);
- pinMode(ecoilb1, OUTPUT);
- pinMode(ecoilb2, OUTPUT);
-
-
- pinMode(13, OUTPUT);
- digitalWrite(13, LOW);
-
- xIdle();
- yIdle();
- zIdle();
- eIdle();
-
-
- //axisTest();
- homeing();
-
- Serial.begin(9600);
- inputString.reserve(200);
- xstr.reserve(5);
- ystr.reserve(5);
- zstr.reserve(5);
- tempstr.reserve(5);
- extrudestr.reserve(5);
-
-
-
- prepareHeater();
-
- //myservo.write(50);
-
- Serial.flush();
-
- Serial.println("Begin");
-
-
-
- }
- double Thermister(int RawADC) {
- double Temp;
- Temp = log(((10240000/RawADC) - 10000));
- Temp = 1 / (0.001129148 + (0.000234125 * Temp) + (0.0000000876741 * Temp * Temp * Temp));
- Temp = Temp - 273.15; // Convert Kelvin to Celcius
- return Temp;
- }
- void axisTest(){
-
- for (int i=0;i<50;i++){
-
- zbackward();
-
- }
-
-
- for (int i=0;i<100;i++){
-
- xforward();
-
- }
-
-
- for (int i=0;i<100;i++){
-
- xbackward();
-
- }
-
-
- for (int i=0;i<100;i++){
-
- yforward();
-
- }
-
-
- for (int i=0;i<100;i++){
-
- ybackward();
-
- }
-
- for (int i=0;i<50;i++){
-
- zforward();
-
- }
-
-
- }
- void xIdle(){
-
- digitalWrite(xcoila1, LOW);
- digitalWrite(xcoila2, LOW);
- digitalWrite(xcoilb1, LOW);
- digitalWrite(xcoilb2, LOW);
- }
- void yIdle(){
- digitalWrite(ycoila1, LOW);
- digitalWrite(ycoila2, LOW);
- digitalWrite(ycoilb1, LOW);
- digitalWrite(ycoilb2, LOW);
- }
- void zIdle(){
- digitalWrite(zcoila1, LOW);
- digitalWrite(zcoila2, LOW);
- digitalWrite(zcoilb1, LOW);
- digitalWrite(zcoilb2, LOW);
-
- }
- void eIdle(){
- digitalWrite(ecoila1, LOW);
- digitalWrite(ecoila2, LOW);
- digitalWrite(ecoilb1, LOW);
- digitalWrite(ecoilb2, LOW);
-
- }
- void homeing(){
-
-
- while ( Sxlimit1 == HIGH ){
-
- xbackward();
- }
- while ( Sylimit1 == HIGH ){
-
- ybackward();
- }
-
-
- for (int i=0;i<350;i++){
-
- xforward();
-
- }
-
- for (int i=0;i<350;i++){
-
- yforward();
-
- }
-
- Sylimit1 = HIGH;
- Sxlimit1 = HIGH;
-
- delay(100);
-
- }
- void prepareHeater(){
-
- heatOn();
-
- tempC = Thermister(analogRead(0)); // Read sensor
-
- while((int)tempC < tempSet){
-
- tempC = Thermister(analogRead(0)); // Read sensor
- Serial.println(" TEMP :" + String(tempSet) + " Cur TEMP:" + String ((int)tempC));
- delay(100);
-
- }
-
-
- }
- void eforward(){
-
- digitalWrite(ecoila1, LOW);
- digitalWrite(ecoila2, LOW);
- digitalWrite(ecoilb1, LOW);
- digitalWrite(ecoilb2, LOW);
-
- if (extrudOn==0){
- return ;
- }
-
- if (turn > 3){
- turn = 0;
- }
-
- if(turn == 0){
- digitalWrite(ecoila1, HIGH);
- }
-
- if(turn == 1){
- digitalWrite(ecoila2, HIGH);
- }
- if(turn == 2){
- digitalWrite(ecoilb1, HIGH);
- }
- if(turn == 3){
- digitalWrite(ecoilb2, HIGH);
- }
-
- turn++;
-
- }
- void ebackward(){
-
- digitalWrite(ecoila1, LOW);
- digitalWrite(ecoila2, LOW);
- digitalWrite(ecoilb1, LOW);
- digitalWrite(ecoilb2, LOW);
-
-
- if (extrudOn==0){
- return ;
- }
-
- if (turn > 3){
- turn = 0;
- }
-
- if(turn == 3){
- digitalWrite(ecoila1, HIGH);
- }
-
- if(turn == 2){
- digitalWrite(ecoila2, HIGH);
- }
- if(turn == 1){
- digitalWrite(ecoilb1, HIGH);
- }
- if(turn == 0){
- digitalWrite(ecoilb2, HIGH);
- }
-
- turn++;
-
- }
- void extrudturn(){
-
- ebackward();
-
- }
-
-
- void ybackward(){
-
- digitalWrite(xcoila1, HIGH);
- digitalWrite(xcoila2, LOW);
- digitalWrite(xcoilb1, LOW);
- digitalWrite(xcoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(xcoila1, LOW);
- digitalWrite(xcoila2, HIGH);
- digitalWrite(xcoilb1, LOW);
- digitalWrite(xcoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(xcoila1, LOW);
- digitalWrite(xcoila2, LOW);
- digitalWrite(xcoilb1, HIGH);
- digitalWrite(xcoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(xcoila1, LOW);
- digitalWrite(xcoila2, LOW);
- digitalWrite(xcoilb1, LOW);
- digitalWrite(xcoilb2, HIGH);
- delay(axisspeed);
-
- }
-
-
- void yforward(){
-
-
-
- digitalWrite(xcoila1, LOW);
- digitalWrite(xcoila2, LOW);
- digitalWrite(xcoilb1, LOW);
- digitalWrite(xcoilb2, HIGH);
- delay(axisspeed);
-
-
- digitalWrite(xcoila1, LOW);
- digitalWrite(xcoila2, LOW);
- digitalWrite(xcoilb1, HIGH);
- digitalWrite(xcoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(xcoila1, LOW);
- digitalWrite(xcoila2, HIGH);
- digitalWrite(xcoilb1, LOW);
- digitalWrite(xcoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(xcoila1, HIGH);
- digitalWrite(xcoila2, LOW);
- digitalWrite(xcoilb1, LOW);
- digitalWrite(xcoilb2, LOW);
- delay(axisspeed);
-
- }
- void xbackward(){
-
- digitalWrite(ycoila1, HIGH);
- digitalWrite(ycoila2, LOW);
- digitalWrite(ycoilb1, LOW);
- digitalWrite(ycoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(ycoila1, LOW);
- digitalWrite(ycoila2, HIGH);
- digitalWrite(ycoilb1, LOW);
- digitalWrite(ycoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(ycoila1, LOW);
- digitalWrite(ycoila2, LOW);
- digitalWrite(ycoilb1, HIGH);
- digitalWrite(ycoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(ycoila1, LOW);
- digitalWrite(ycoila2, LOW);
- digitalWrite(ycoilb1, LOW);
- digitalWrite(ycoilb2, HIGH);
- delay(axisspeed);
-
- }
-
-
- void xforward(){
-
-
-
- digitalWrite(ycoila1, LOW);
- digitalWrite(ycoila2, LOW);
- digitalWrite(ycoilb1, LOW);
- digitalWrite(ycoilb2, HIGH);
- delay(axisspeed);
-
-
- digitalWrite(ycoila1, LOW);
- digitalWrite(ycoila2, LOW);
- digitalWrite(ycoilb1, HIGH);
- digitalWrite(ycoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(ycoila1, LOW);
- digitalWrite(ycoila2, HIGH);
- digitalWrite(ycoilb1, LOW);
- digitalWrite(ycoilb2, LOW);
- delay(axisspeed);
-
-
- digitalWrite(ycoila1, HIGH);
- digitalWrite(ycoila2, LOW);
- digitalWrite(ycoilb1, LOW);
- digitalWrite(ycoilb2, LOW);
- delay(axisspeed);
-
- }
- /*void zforward(){
-
- myservo.write(19);
-
- }
- void zbackward(){
-
- myservo.write(30);
-
- }*/
- void zforward(){
-
- digitalWrite(zcoila1, HIGH);
- digitalWrite(zcoila2, LOW);
- digitalWrite(zcoilb1, LOW);
- digitalWrite(zcoilb2, LOW);
- delay(zaxisspeed);
-
-
- digitalWrite(zcoila1, LOW);
- digitalWrite(zcoila2, HIGH);
- digitalWrite(zcoilb1, LOW);
- digitalWrite(zcoilb2, LOW);
- delay(zaxisspeed);
-
-
- digitalWrite(zcoila1, LOW);
- digitalWrite(zcoila2, LOW);
- digitalWrite(zcoilb1, HIGH);
- digitalWrite(zcoilb2, LOW);
- delay(zaxisspeed);
-
-
- digitalWrite(zcoila1, LOW);
- digitalWrite(zcoila2, LOW);
- digitalWrite(zcoilb1, LOW);
- digitalWrite(zcoilb2, HIGH);
- delay(zaxisspeed);
-
- }
-
- void zbackward(){
-
- digitalWrite(zcoila1, LOW);
- digitalWrite(zcoila2, LOW);
- digitalWrite(zcoilb1, LOW);
- digitalWrite(zcoilb2, HIGH);
- delay(zaxisspeed);
-
-
- digitalWrite(zcoila1, LOW);
- digitalWrite(zcoila2, LOW);
- digitalWrite(zcoilb1, HIGH);
- digitalWrite(zcoilb2, LOW);
- delay(zaxisspeed);
-
-
- digitalWrite(zcoila1, LOW);
- digitalWrite(zcoila2, HIGH);
- digitalWrite(zcoilb1, LOW);
- digitalWrite(zcoilb2, LOW);
- delay(zaxisspeed);
-
-
- digitalWrite(zcoila1, HIGH);
- digitalWrite(zcoila2, LOW);
- digitalWrite(zcoilb1, LOW);
- digitalWrite(zcoilb2, LOW);
- delay(zaxisspeed);
-
- }
-
- void strgcode(){
-
- int i=0;
- int axis=-1;
- int op = -1;
-
-
-
- while(inputString[i] != '\0'){
-
-
- if(inputString[i]==' '){
- i++;
- continue;
-
- }
-
- if(inputString[i]=='X'){
- axis=0;
- i++;
- continue;
- }
-
- if(inputString[i]=='Y'){
- axis=1;
- i++;
- continue;
- }
-
- if(inputString[i]=='Z'){
- axis=2;
- i++;
- continue;
- }
-
- if(inputString[i]=='S'){
- op=0;
- i++;
- continue;
- }
-
- if(inputString[i]=='M'){
- op=1;
- i++;
- continue;
- }
-
- if(axis==0){
-
- xstr+=inputString[i];
-
- }
-
- if(axis==1){
-
- ystr+=inputString[i];
-
- }
- if(axis==2){
-
- zstr+=inputString[i];
-
- }
- if(op==0){
-
- tempstr+=inputString[i];
-
- }
-
- if(op==1){
-
- extrudestr+=inputString[i];
-
- }
-
- i++;
-
- }
-
-
-
-
- }
- void heatOn(){
-
-
- digitalWrite(heatPin, HIGH);
-
-
- }
- void heatOff(){
-
- digitalWrite(heatPin, LOW);
-
- }
-
- void loop()
- {
- //digitalWrite(13, HIGH);
- //delay(2000);
- //digitalWrite(13, LOW);
-
- tempC = Thermister(analogRead(0)); // Read sensor
-
- if((int)tempC < tempSet+3){
-
- heatOn();
-
- }else{
-
- if((int)tempC > tempSet+10){
-
- heatOff();
- }
-
- }
- //Serial.println("Idle "+ String(x) +" "+ String(y)+" "+ String(z) + " TEMP :" + String(tempSet) + " Cur TEMP:" + String ((int)tempC));
-
- if (stringComplete) {
-
-
- /*Serial.println(inputString);
- // clear the string:
- inputString = "";
- stringComplete = false; */
-
- strgcode();
- Serial.flush();
-
- int xdone=0;
- int ydone=0;
- int zdone=0;
-
- if(tempstr.compareTo("")!= 0){
- tempSet = tempstr.toInt();
- }
-
- if(extrudestr.compareTo("")!= 0){
- if(extrudestr.toInt() == 101){
- extrudOn = 1;
-
- }else{
-
- if(extrudestr.toInt()==103){
-
- extrudOn = 0;
- }
- }
- }
-
-
- //digitalWrite(13, Sxlimit1 );
-
- //if(tempstr.toInt())
-
-
- while(xdone==0 || ydone==0 || zdone==0){
-
-
-
-
- if (Sxlimit1 == LOW || Sylimit1 == LOW || Sxlimit2 == LOW || Sylimit2 == LOW ){
-
- xIdle();
- yIdle();
- zIdle();
- heatOff();
-
- Serial.println("Error "+ String(Sxlimit1)+" "+String(Sylimit1)+" "+ String(Sxlimit2)+" "+String(Sylimit2));
-
- while(1){
-
- }
- }
-
-
-
-
- if(xstr.compareTo("")!= 0){
- if(x<xstr.toInt()){
-
- xforward();
- x++;
- }else{
-
- if(x>xstr.toInt()){
- xbackward();
- x--;
- }else{
- xdone=1;
- }
-
- }
-
- }else{
- xdone=1;
- }
-
-
- if(ystr.compareTo("")!= 0){
- if(y<ystr.toInt()){
- yforward();
- y++;
- }else{
-
- if(y>ystr.toInt()){
- ybackward();
- y--;
- }else{
- ydone=1;
- }
-
- }
- }else{
-
- ydone=1;
-
- }
- if(zstr.compareTo("")!= 0){
- if(z<zstr.toInt()){
-
- for (int i=0;i<10;i++){
-
- zforward();
-
- }
-
-
-
- z++;
- }else{
-
- if(z>zstr.toInt()){
-
- for (int i=0;i<10;i++){
-
- zbackward();
-
- }
-
- z--;
- }else{
- zdone=1;
- }
-
- }
-
- }else{
-
- zdone=1;
- }
-
-
- }
-
-
- Serial.println("Done "+ String(xstr) +" "+ String(ystr)+" "+ String(zstr) + " TEMP :" + String(tempSet) + "Cur TEMP:" + String ((int)tempC));
-
- // clear the string:
- inputString = "";
- xstr="";
- ystr="";
- zstr="";
- tempstr="";
- extrudestr="";
- stringComplete = false;
-
- }
-
-
-
-
- }
- void xlimit1On(){
-
- Sxlimit1 = !Sxlimit1;
-
- }
- void ylimit1On(){
-
- Sylimit1 = !Sylimit1;
- }
- void xlimit2On(){
-
- Sxlimit2 = !Sxlimit2;
-
- }
- void ylimit2On(){
-
- Sylimit2 = !Sylimit2;
- }
- void serialEvent() {
- while (Serial.available()) {
- // get the new byte:
- char inChar = (char)Serial.read();
- // add it to the inputString:
- if (inChar == '\n') {
-
- inputString+='\0';
- stringComplete = true;
-
- return;
- }
-
- inputString += inChar;
- // if the incoming character is a newline, set a flag
- // so the main loop can do something about it:
-
- }
- }