// // /* ---------------------------------
// // * Controls 2 steppers with ESP32 and CNC shield plus one servo for pen up/down
// // * for 4xiDraw machine
// // *
// // * code by misan, April 2018
// // * known to work on ESP32 Arducam UC406 board
// // *
// // * serial 115200 and UDP & TCP 9999 accept g-code
// // * board broadcasts itself on UDP till first connection
// // */
// // #define MAXI 30
// // #define VERSION (1) // firmware version
// // #define BAUD (115200) // How fast is the Arduino talking?
// // #define MAX_BUF (64) // What is the longest message Arduino can store?
// // #define STEPS_PER_TURN (400) // depends on your stepper motor. most are 200.
// // #define MAX_FEEDRATE (250000)
// // #define MIN_FEEDRATE (0.01)
// // #define STEPX 12
// // #define DIRX 14
// // #define ENABLE 33
// // #define STEPY 27
// // #define DIRY 26
// // #define STEPZ 18
// // #define DIRZ 19
// // #define SERVO 21
// // #define STEPS_MM 80
// // hw_timer_t * timer = NULL; // stepper
// // hw_timer_t * timer1 = NULL; // servo
// // portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
// // int ACCEL = 1500 ; // mm/s^2
// // boolean flip = false;
// // // for arc directions
// // #define ARC_CW (1)
// // #define ARC_CCW (-1)
// // // Arcs are split into many line segments. How long are the segments?
// // #define MM_PER_SEGMENT (1)
// // int i = 1;
// // long last=0;
// // int servo=1500;
// // float factor, v2 = 0;
// // long coast_steps=40000; // how many steps during coasting
// // float t0=0.1; // determines initial speed
// // boolean diry,dirx;
// // long over;
// // double dx,dy;
// // double px,py;
// // long int px_step, py_step;
// // // interrupt triggered by timer expiring
// // // motion will happen here based on a few variables
// // volatile double cn=t0;
// // volatile boolean busy = false;
// // volatile int cs=0; // current step
// // volatile long accel_steps=1000; // how many steps during acceleration
// // volatile long total_steps=0; // total number of steps for the current movement
// // boolean motorStopped = false;
// // boolean motorPaused = false;
// // void IRAM_ATTR itr1(void) { // servo interrupt
// // if(flip) {
// // digitalWrite(SERVO,LOW);
// // flip = false;
// // timerAlarmWrite(timer1, 20000, true); //timer1_write(20000 * 5); // 20 ms between pulses
// // }
// // else {
// // timerAlarmWrite(timer1, servo, true); //timer1_write(servo*5);
// // flip = true;
// // digitalWrite(SERVO,HIGH);
// // }
// // }
// // int k=0;
// // void IRAM_ATTR itr(void) { // stepper interrupt
// // if (cs >= total_steps) {
// // // no more interrupts scheduled or motor stopped permanently
// // busy = false;
// // timerAlarmWrite(timer, 1000000, true);
// // } else if (!motorPaused) { // I have to move the motors
// // cs++;
// // if (cs < accel_steps) {
// // // acceleration
// // cn = cn - (cn * 2) / (4 * cs + 1 );
// // } else if (cs < accel_steps + coast_steps) {
// // // coast at max speed
// // } else if (cs < total_steps) {
// // // deceleration
// // cn = cn - (cn * 2) / (4 * (cs - total_steps) + 1);
// // }
// // timerAlarmWrite(timer, cn * 1000000L, true);
// // if (dx > dy) {
// // //over=dx/2;
// // digitalWrite(STEPX, HIGH);
// // px_step = (dirx ? px_step + 1 : px_step - 1);
// // over += dy;
// // if (over >= dx) {
// // over -= dx;
// // py = (diry ? py + 1 : py - 1);
// // digitalWrite(STEPY, HIGH);
// // }
// // delayMicroseconds(1);
// // } else {
// // //over=dy/2;
// // digitalWrite(STEPY, HIGH);
// // py_step = (diry ? py_step + 1 : py_step - 1);
// // over += dx;
// // if (over >= dy) {
// // over -= dy;
// // px = (dirx ? px + 1 : px - 1);
// // digitalWrite(STEPX, HIGH);
// // }
// // delayMicroseconds(1);
// // }
// // px = px_step/STEPS_MM;
// // py = py_step/STEPS_MM;
// // digitalWrite(STEPY, LOW);
// // digitalWrite(STEPX, LOW);
// // }
// // }
// // //------------------------------------------------------------------------------
// // // 2 Axis CNC Demo
// // // [email protected] 2013-08-30
// // //------------------------------------------------------------------------------
// // // Copyright at end of file.
// // // please see http://www.github.com/MarginallyClever/GcodeCNCDemo for more information.
// // //------------------------------------------------------------------------------
// // // GLOBALS
// // //------------------------------------------------------------------------------
// // char buffer[MAX_BUF]; // where we store the message until we get a newline
// // int sofar; // how much is in the buffer
// // //float px, py; // location
// // // speeds
// // float fr=1000; // human version
// // long step_delay; // machine version
// // // settings
// // char mode_abs=1; // absolute mode?
// // //------------------------------------------------------------------------------
// // // METHODS
// // //------------------------------------------------------------------------------
// // /**
// // * delay for the appropriate number of microseconds
// // * @input ms how many milliseconds to wait
// // */
// // void pause(long ms) {
// // delay(ms/1000);
// // delayMicroseconds(ms%1000); // delayMicroseconds doesn't work for values > ~16k.
// // }
// // /**
// // * Set the feedrate (speed motors will move)
// // * @input nfr the new speed in steps/second
// // */
// // void feedrate(float nfr) {
// // if(fr==nfr) return; // same as last time? quit now.
// // if(nfr>MAX_FEEDRATE || nfr<MIN_FEEDRATE) { // don't allow crazy feed rates
// // Serial.print(F("New feedrate must be greater than "));
// // Serial.print(MIN_FEEDRATE);
// // Serial.print(F("steps/s and less than "));
// // Serial.print(MAX_FEEDRATE);
// // Serial.println(F("steps/s."));
// // return;
// // }
// // step_delay = 1000000.0/nfr;
// // fr=nfr;
// // cn = t0 = sqrt( 2.0 /** STEPS_MM*/ / ACCEL );
// // }
// // /**
// // * Set the logical position
// // * @input npx new position x
// // * @input npy new position y
// // */
// // void position(float npx,float npy) {
// // // here is a good place to add sanity tests
// // px=npx;
// // py=npy;
// // }
// // //#define max(a,b) ((a)>(b)?(a):(b))
// // /**
// // * Uses bresenham's line algorithm to move both motors
// // * @input newx the destination x position
// // * @input newy the destination y position
// // **/
// // void line(float newx,float newy) {
// // dx=newx-px;
// // dy=newy-py;
// // dirx=dx>0;
// // diry=dy>0; // because the motors are mounted in opposite directions
// // dx=abs(dx);
// // dy=abs(dy);
// // Serial.print("line: ");Serial.print(dx); Serial.print(","); Serial.println(dy);
// // // move(dx,dirx,dy,diry);
// // // long i;
// // over=0;
// // total_steps = max( dx, dy ) * STEPS_MM;
// // float ta = fr / ACCEL ;
// // float ea = ( fr ) * ta / 2 ;
// // accel_steps = _min(ea * STEPS_MM , total_steps/2); // just in case feedrate cannot be reached
// // coast_steps = total_steps - accel_steps * 2; // acceleration
// // digitalWrite(DIRX, dirx); // direction of X motor
// // digitalWrite(DIRY, diry);
// // cs = 0; // let the motion start :-)
// // cn = t0;
// // if(dx>dy)
// // over=dx/2;
// // else
// // over=dy/2;
// // busy=true;
// // // wait till the command ends
// // while(busy) { /*doServo();*/ yield(); }
// // }
// // // returns angle of dy/dx as a value from 0...2PI
// // float atan3(float dy,float dx) {
// // float a=atan2(dy,dx);
// // if(a<0) a=(PI*2.0)+a;
// // return a;
// // }
// // // This method assumes the limits have already been checked.
// // // This method assumes the start and end radius match.
// // // This method assumes arcs are not >180 degrees (PI radians)
// // // cx/cy - center of circle
// // // x/y - end position
// // // dir - ARC_CW or ARC_CCW to control direction of arc
// // void arc(float cx,float cy,float x,float y,float dir) {
// // // get radius
// // float dx = px - cx;
// // float dy = py - cy;
// // float radius=sqrt(dx*dx+dy*dy);
// // // find angle of arc (sweep)
// // float angle1=atan3(dy,dx);
// // float angle2=atan3(y-cy,x-cx);
// // float theta=angle2-angle1;
// // if(dir>0 && theta<0) angle2+=2*PI;
// // else if(dir<0 && theta>0) angle1+=2*PI;
// // theta=angle2-angle1;
// // // get length of arc
// // // float circ=PI*2.0*radius;
// // // float len=theta*circ/(PI*2.0);
// // // simplifies to
// // float len = abs(theta) * radius;
// // int i, segments = ceil( len * MM_PER_SEGMENT );
// // float nx, ny, angle3, scale;
// // for(i=0;i<segments;++i) {
// // // interpolate around the arc
// // scale = ((float)i)/((float)segments);
// // angle3 = ( theta * scale ) + angle1;
// // nx = cx + cos(angle3) * radius;
// // ny = cy + sin(angle3) * radius;
// // // send it to the planner
// // line(nx,ny);
// // }
// // line(x,y);
// // }
// // /**
// // * Look for character /code/ in the buffer and read the float that immediately follows it.
// // * @return the value found. If nothing is found, /val/ is returned.
// // * @input code the character to look for.
// // * @input val the return value if /code/ is not found.
// // **/
// // float parsenumber(char code,float val) {
// // char *ptr=buffer;
// // while(ptr && *ptr && ptr<buffer+sofar) {
// // if(*ptr==code) {
// // return atof(ptr+1);
// // }
// // ptr++; //ptr=strchr(ptr,' ')+1;
// // }
// // return val;
// // }
// // /**
// // * write a string followed by a float to the serial line. Convenient for debugging.
// // * @input code the string.
// // * @input val the float.
// // */
// // void output(const char *code,float val) {
// // Serial.print(code);
// // Serial.println(val);
// // }
// // /**
// // * print the current position, feedrate, and absolute mode.
// // */
// // void where() {
// // output("X",px);
// // output("Y",py);
// // output("F",fr);
// // Serial.println(mode_abs?"ABS":"REL");
// // }
// // /**
// // * display helpful information
// // */
// // void help() {
// // Serial.print(F("GcodeCNCDemo2AxisV1 "));
// // Serial.println(VERSION);
// // Serial.println(F("Commands:"));
// // Serial.println(F("G00 [X(steps)] [Y(steps)] [F(feedrate)]; - line"));
// // Serial.println(F("G01 [X(steps)] [Y(steps)] [F(feedrate)]; - line"));
// // Serial.println(F("G02 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - clockwise arc"));
// // Serial.println(F("G03 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - counter-clockwise arc"));
// // Serial.println(F("G04 P[seconds]; - delay"));
// // Serial.println(F("G90; - absolute mode"));
// // Serial.println(F("G91; - relative mode"));
// // Serial.println(F("G92 [X(steps)] [Y(steps)]; - change logical position"));
// // Serial.println(F("M18; - disable motors"));
// // Serial.println(F("M100; - this help message"));
// // Serial.println(F("M114; - report position and feedrate"));
// // Serial.println(F("All commands must end with a newline."));
// // }
// // /**
// // * Read the input buffer and find any recognized commands. One G or M command per line.
// // */
// // void processCommand() {
// // int cmd = parsenumber('G',-1);
// // switch(cmd) {
// // case 0:
// // case 1: { // line
// // feedrate(parsenumber('F',fr));
// // line( parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
// // parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py) );
// // break;
// // }
// // case 2:
// // case 3: { // arc
// // feedrate(parsenumber('F',fr));
// // arc(parsenumber('I',(mode_abs?px:0)) + (mode_abs?0:px),
// // parsenumber('J',(mode_abs?py:0)) + (mode_abs?0:py),
// // parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
// // parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py),
// // (cmd==2) ? -1 : 1);
// // break;
// // }
// // case 4: pause(parsenumber('P',0)*1000); break; // dwell
// // case 90: mode_abs=1; break; // absolute mode
// // case 91: mode_abs=0; break; // relative mode
// // case 92: // set logical position
// // position( parsenumber('X',0),
// // parsenumber('Y',0) );
// // break;
// // default: break;
// // }
// // cmd = parsenumber('M',-1);
// // switch(cmd) {
// // case 3:servo = ( parsenumber('S',1500) ); break; // sets the servo value in microseconds, it only works while inside loop :-(
// // case 18: // disable motors
// // // disable();
// // break;
// // case 100: help(); break;
// // case 114: where(); break;
// // case 201: ACCEL = parsenumber('A',0); feedrate(fr); Serial.print(ACCEL); break; // M201 A<accel> change acceleration
// // default: break;
// // }
// // }
// // /**
// // * prepares the input buffer to receive a new message and tells the serial connected device it is ready for more.
// // */
// // void ready() {
// // sofar=0; // clear input buffer
// // Serial.print(F(">")); // signal ready to receive input
// // }
// // void setup() {
// // pinMode(STEPX, OUTPUT); // stepX
// // pinMode(DIRX, OUTPUT); //dirX
// // pinMode(ENABLE, OUTPUT); // enable
// // pinMode(STEPY, OUTPUT); // stepY
// // pinMode(DIRY, OUTPUT); // dirY
// // pinMode(SERVO, OUTPUT); // servo
// // digitalWrite(ENABLE, LOW);
// // digitalWrite(SERVO, LOW);
// // Serial.begin(BAUD); // open coms
// // // setup_controller();
// // position(0,0); // set staring position
// // feedrate((MAX_FEEDRATE + MIN_FEEDRATE)/2); // set default speed
// // help(); // say hello
// // ready();
// // timer = timerBegin(0, 80, true);
// // timerAttachInterrupt(timer, &itr, true);
// // timerAlarmWrite(timer, 1000000, true);
// // timerAlarmEnable(timer);
// // timer1 = timerBegin(1, 80, true);
// // timerAttachInterrupt(timer1, &itr1, true);
// // timerAlarmWrite(timer1, 20000, true);
// // timerAlarmEnable(timer1);
// // }
// // char c;
// // long lastping;
// // void loop() {
// // while(Serial.available() > 0 ) { // if something is available
// // c=Serial.read(); // get it
// // Serial.print(c); // repeat it back so I know you got the message
// // if(sofar<MAX_BUF-1) buffer[sofar++]=c; // store it
// // if((c=='\n') || (c == '\r')) {
// // // entire message received
// // buffer[sofar]=0; // end the buffer so string functions work right
// // Serial.print(F("\r\n")); // echo a return character for humans
// // if(sofar>3) {
// // busy=true;
// // processCommand(); // do something with the command
// // Serial.println("ok");
// // ready();
// // }
// // }
// // }
// // }
// /* ---------------------------------
// * Controls 2 steppers with ESP32 and CNC shield plus one servo for pen up/down
// * for 4xiDraw machine
// *
// * code by misan, April 2018
// * known to work on ESP32 Arducam UC406 board
// *
// * serial 115200 and UDP & TCP 9999 accept g-code
// * board broadcasts itself on UDP till first connection
// */
// #define MAXI 30
// #define VERSION (1) // firmware version
// #define BAUD (115200) // How fast is the Arduino talking?
// #define MAX_BUF (64) // What is the longest message Arduino can store?
// #define STEPS_PER_TURN (400) // depends on your stepper motor. most are 200.
// #define MAX_FEEDRATE (250000)
// #define MIN_FEEDRATE (0.01)
// #define STEPX 12
// #define DIRX 14
// #define ENABLE 33
// #define STEPY 27
// #define DIRY 26
// #define STEPZ 18
// #define DIRZ 19
// #define SERVO T7
// #define STEPS_MM 1
// hw_timer_t * timer = NULL; // stepper
// portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
// int ACCEL = 1500 ; // mm/s^2
// boolean flip = false;
// // for arc directions
// #define ARC_CW (1)
// #define ARC_CCW (-1)
// // Arcs are split into many line segments. How long are the segments?
// #define MM_PER_SEGMENT (1)
// int i = 1;
// long last=0;
// float factor, v2 = 0;
// long coast_steps=40000; // how many steps during coasting
// float t0=0.1; // determines initial speed
// boolean diry,dirx;
// long over;
// double dx,dy;
// // interrupt triggered by timer expiring
// // motion will happen here based on a few variables
// volatile double cn=t0;
// volatile boolean busy = false;
// volatile int cs=0; // current step
// volatile long accel_steps=1000; // how many steps during acceleration
// volatile long total_steps=0; // total number of steps for the current movement
// int k=0;
// void IRAM_ATTR itr (void) { // stepper interrupt
// if( cs >= total_steps ) {busy = false; timerAlarmWrite(timer, 1000000, true); } // no more interrupts scheduled
// else { // I have to move the motors
// cs++;
// if( cs < accel_steps ) {
// // acceleration
// #ifdef SCURVE
// if(cs<15) {k=15-cs; k*=k; }
// else{
// k=accel_steps-cs;
// if (k<150) { k=150-k; k*=k; } else k=0;
// }
// #endif
// cn = cn - (cn*2)/(4*cs+1+k);
// }
// else if ( cs < accel_steps + coast_steps ) {
// // coast at max speed
// }
// else if(cs<total_steps) {
// // deceleration
// cn = cn - (cn*2)/(4*(cs-total_steps)+1);
// }
// timerAlarmWrite(timer, cn*1000000L , true);
// if(dx>dy) {
// //over=dx/2;
// digitalWrite(STEPX, HIGH);
// over+=dy;
// if(over>=dx) {
// over-=dx;
// digitalWrite(STEPY, HIGH);
// }
// delayMicroseconds(1);
// } else {
// //over=dy/2;
// digitalWrite(STEPY, HIGH);
// over+=dx;
// if(over>=dy) {
// over-=dy;
// digitalWrite(STEPX, HIGH);
// }
// delayMicroseconds(1);
// }
// digitalWrite(STEPY, LOW);
// digitalWrite(STEPX , LOW);
// }
// }
// //------------------------------------------------------------------------------
// // 2 Axis CNC Demo
// // [email protected] 2013-08-30
// //------------------------------------------------------------------------------
// // Copyright at end of file.
// // please see http://www.github.com/MarginallyClever/GcodeCNCDemo for more information.
// //------------------------------------------------------------------------------
// // GLOBALS
// //------------------------------------------------------------------------------
// char buffer[MAX_BUF]; // where we store the message until we get a newline
// int sofar; // how much is in the buffer
// float px, py; // location
// // speeds
// float fr=1000; // human version
// long step_delay; // machine version
// // settings
// char mode_abs=1; // absolute mode?
// //------------------------------------------------------------------------------
// // METHODS
// //------------------------------------------------------------------------------
// /**
// * delay for the appropriate number of microseconds
// * @input ms how many milliseconds to wait
// */
// void pause(long ms) {
// delay(ms/1000);
// delayMicroseconds(ms%1000); // delayMicroseconds doesn't work for values > ~16k.
// }
// /**
// * Set the feedrate (speed motors will move)
// * @input nfr the new speed in steps/second
// */
// void feedrate(float nfr) {
// if(fr==nfr) return; // same as last time? quit now.
// if(nfr>MAX_FEEDRATE || nfr<MIN_FEEDRATE) { // don't allow crazy feed rates
// Serial.print(F("New feedrate must be greater than "));
// Serial.print(MIN_FEEDRATE);
// Serial.print(F("steps/s and less than "));
// Serial.print(MAX_FEEDRATE);
// Serial.println(F("steps/s."));
// return;
// }
// step_delay = 1000000.0/nfr;
// fr=nfr;
// cn = t0 = sqrt( 2.0 /** STEPS_MM*/ / ACCEL );
// }
// /**
// * Set the logical position
// * @input npx new position x
// * @input npy new position y
// */
// void position(float npx,float npy) {
// // here is a good place to add sanity tests
// px=npx;
// py=npy;
// }
// //#define max(a,b) ((a)>(b)?(a):(b))
// /**
// * Uses bresenham's line algorithm to move both motors
// * @input newx the destination x position
// * @input newy the destination y position
// **/
// void line(float newx,float newy) {
// dx=newx-px;
// dy=newy-py;
// dirx=dx>0;
// diry=dy>0; // because the motors are mounted in opposite directions
// dx=abs(dx);
// dy=abs(dy);
// Serial.print("line: ");Serial.print(dx); Serial.print(","); Serial.println(dy);
// // move(dx,dirx,dy,diry);
// long i;
// over=0;
// total_steps = _max ( dx, dy ) * STEPS_MM;
// float ta = fr / 60.0 / ACCEL;
// float ea = ( fr / 60.0 ) * ta / 2 ;
// accel_steps = _min(ea * STEPS_MM , total_steps/2); // just in case feedrate cannot be reached
// coast_steps = total_steps - accel_steps * 2; // acceleration
// digitalWrite(DIRX, dirx); // direction of X motor
// digitalWrite(DIRY, diry);
// cs = 0; // let the motion start :-)
// cn = t0;
// if(dx>dy) over=dx/2; else over=dy/2;
// busy=true;
// // wait till the command ends
// px=newx;
// py=newy;
// }
// // returns angle of dy/dx as a value from 0...2PI
// float atan3(float dy,float dx) {
// float a=atan2(dy,dx);
// if(a<0) a=(PI*2.0)+a;
// return a;
// }
// // This method assumes the limits have already been checked.
// // This method assumes the start and end radius match.
// // This method assumes arcs are not >180 degrees (PI radians)
// // cx/cy - center of circle
// // x/y - end position
// // dir - ARC_CW or ARC_CCW to control direction of arc
// void arc(float cx,float cy,float x,float y,float dir) {
// // get radius
// float dx = px - cx;
// float dy = py - cy;
// float radius=sqrt(dx*dx+dy*dy);
// // find angle of arc (sweep)
// float angle1=atan3(dy,dx);
// float angle2=atan3(y-cy,x-cx);
// float theta=angle2-angle1;
// if(dir>0 && theta<0) angle2+=2*PI;
// else if(dir<0 && theta>0) angle1+=2*PI;
// theta=angle2-angle1;
// // get length of arc
// // float circ=PI*2.0*radius;
// // float len=theta*circ/(PI*2.0);
// // simplifies to
// float len = abs(theta) * radius;
// int i, segments = ceil( len * MM_PER_SEGMENT );
// float nx, ny, angle3, scale;
// for(i=0;i<segments;++i) {
// // interpolate around the arc
// scale = ((float)i)/((float)segments);
// angle3 = ( theta * scale ) + angle1;
// nx = cx + cos(angle3) * radius;
// ny = cy + sin(angle3) * radius;
// // send it to the planner
// line(nx,ny);
// }
// line(x,y);
// }
// /**
// * Look for character /code/ in the buffer and read the float that immediately follows it.
// * @return the value found. If nothing is found, /val/ is returned.
// * @input code the character to look for.
// * @input val the return value if /code/ is not found.
// **/
// float parsenumber(char code,float val) {
// char *ptr=buffer;
// while(ptr && *ptr && ptr<buffer+sofar) {
// if(*ptr==code) {
// return atof(ptr+1);
// }
// ptr++; //ptr=strchr(ptr,' ')+1;
// }
// return val;
// }
// /**
// * write a string followed by a float to the serial line. Convenient for debugging.
// * @input code the string.
// * @input val the float.
// */
// void output(const char *code,float val) {
// Serial.print(code);
// Serial.println(val);
// }
// /**
// * print the current position, feedrate, and absolute mode.
// */
// void where() {
// output("X",px);
// output("Y",py);
// output("F",fr);
// Serial.println(mode_abs?"ABS":"REL");
// }
// /**
// * display helpful information
// */
// void help() {
// Serial.print(F("GcodeCNCDemo2AxisV1 "));
// Serial.println(VERSION);
// Serial.println(F("Commands:"));
// Serial.println(F("G00 [X(steps)] [Y(steps)] [F(feedrate)]; - line"));
// Serial.println(F("G01 [X(steps)] [Y(steps)] [F(feedrate)]; - line"));
// Serial.println(F("G02 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - clockwise arc"));
// Serial.println(F("G03 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - counter-clockwise arc"));
// Serial.println(F("G04 P[seconds]; - delay"));
// Serial.println(F("G90; - absolute mode"));
// Serial.println(F("G91; - relative mode"));
// Serial.println(F("G92 [X(steps)] [Y(steps)]; - change logical position"));
// Serial.println(F("M18; - disable motors"));
// Serial.println(F("M100; - this help message"));
// Serial.println(F("M114; - report position and feedrate"));
// Serial.println(F("All commands must end with a newline."));
// }
// /**
// * Read the input buffer and find any recognized commands. One G or M command per line.
// */
// void processCommand() {
// int cmd = parsenumber('G',-1);
// switch(cmd) {
// case 0:
// case 1: { // line
// feedrate(parsenumber('F',fr));
// line( parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
// parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py) );
// break;
// }
// case 2:
// case 3: { // arc
// feedrate(parsenumber('F',fr));
// arc(parsenumber('I',(mode_abs?px:0)) + (mode_abs?0:px),
// parsenumber('J',(mode_abs?py:0)) + (mode_abs?0:py),
// parsenumber('X',(mode_abs?px:0)) + (mode_abs?0:px),
// parsenumber('Y',(mode_abs?py:0)) + (mode_abs?0:py),
// (cmd==2) ? -1 : 1);
// break;
// }
// case 4: pause(parsenumber('P',0)*1000); break; // dwell
// case 90: mode_abs=1; break; // absolute mode
// case 91: mode_abs=0; break; // relative mode
// case 92: // set logical position
// position( parsenumber('X',0),
// parsenumber('Y',0) );
// break;
// default: break;
// }
// cmd = parsenumber('M',-1);
// switch(cmd) {
// case 18: // disable motors
// // disable();
// break;
// case 100: help(); break;
// case 114: where(); break;
// case 201: ACCEL = parsenumber('A',0); feedrate(fr); Serial.print(ACCEL); break; // M201 A<accel> change acceleration
// default: break;
// }
// }
// /**
// * prepares the input buffer to receive a new message and tells the serial connected device it is ready for more.
// */
// void ready() {
// sofar=0; // clear input buffer
// Serial.print(F(">")); // signal ready to receive input
// }
// void setup() {
// pinMode(STEPX, OUTPUT); // stepX
// pinMode(DIRX, OUTPUT); //dirX
// pinMode(ENABLE, OUTPUT); // enable
// pinMode(STEPY, OUTPUT); // stepY
// pinMode(DIRY, OUTPUT); // dirY
// digitalWrite(ENABLE, LOW);
// Serial.begin(BAUD); // open coms
// // setup_controller();
// position(0,0); // set staring position
// feedrate((MAX_FEEDRATE + MIN_FEEDRATE)/2); // set default speed
// help(); // say hello
// ready();
// timer = timerBegin(0, 80, true);
// timerAttachInterrupt(timer, &itr, true);
// timerAlarmWrite(timer, 1000000, true);
// timerAlarmEnable(timer);
// }
// char c;
// long lastping;
// void loop() {
// while(Serial.available() > 0 ) { // if something is available
// c=Serial.read(); // get it
// Serial.print(c); // repeat it back so I know you got the message
// if(sofar<MAX_BUF-1) buffer[sofar++]=c; // store it
// if((c=='\n') || (c == '\r')) {
// // entire message received
// buffer[sofar]=0; // end the buffer so string functions work right
// Serial.print(F("\r\n")); // echo a return character for humans
// if(sofar>3) {
// busy=true;
// processCommand(); // do something with the command
// Serial.println("ok");
// ready();
// }
// }
// }
// }
//------------------------------------------------------------------------------
// GLOBALS
//------------------------------------------------------------------------------
#include <ESP32Servo.h>
byte directionPinX = 14;
byte stepPinX = 12;
byte enableX = 33;
byte directionPinY = 26;
byte stepPinY = 27;
byte enableY = 33;
byte directionPinZ = 19;
byte stepPinZ = 18;
byte enableZ = 33;
byte directionPinE = 16;
byte stepPinE = 17;
byte enableE = 33;
byte SERVO_PIN = 21;
Servo servo;
hw_timer_t *timer = NULL; // stepper
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
float t0 = 0.1; // determines initial speed
volatile double cn = t0;
volatile boolean busy = false;
volatile int cs = 0; // current step
volatile long accel_steps = 1000; // how many steps during acceleration
volatile long total_steps = 0; // total number of steps for the current movement
long coast_steps = 40000; // how many steps during coasting
long over;
double dx, dy, dz, de;
boolean diry, dirx, dirz, dire;
#define STEPS_MM 1
int ACCEL = 1500; // mm/s^2
unsigned long t, t1;
#define MAX_BUF (64)
#define MIN_STEP_DELAY (50.0)
#define MAX_FEEDRATE (1000000.0 / MIN_STEP_DELAY)
#define MIN_FEEDRATE (0.01)
#define ARC_CW (1)
#define ARC_CCW (-1)
#define MM_PER_SEGMENT (10)
char serialBuffer[MAX_BUF]; // tempat menyimpan pesan sampai mendapatkan baris baru
int sofar; // berapa banyak yang ada di serialBuffer
double px, py, pz, pe; // location - posisi saat ini //DOUBEL AJA KLO INGET
double px_step, py_step, pz_step, pe_step;
// speeds
float fr = 0; // human version
long step_delay; // machine version
// settings
char mode_abs = 1; // absolute mode?
unsigned long lastReportTime = 0;
const long reportInterval = 500; // 1000 ms = 1 detik
QueueHandle_t queueX;
QueueHandle_t queueY;
QueueHandle_t queueZ;
struct MotorCommand
{
float x;
float y;
float z;
float e;
};
// Membuat queue
QueueHandle_t queueXYZE = xQueueCreate(10, sizeof(MotorCommand));
boolean motorPaused = false;
boolean motorStopped = false;
boolean motorRunning = true;
boolean noACCEL = false;
double angle;
void IRAM_ATTR itr(void) { // stepper interrupt
if (cs >= total_steps) {
// no more interrupts scheduled or motor stopped permanently
busy = false;
timerAlarmWrite(timer, 1000000, true);
}
else if (!motorPaused) { // I have to move the motors
cs++;
if (cs < accel_steps) { // acceleration
cn = cn - (cn * 2) / (4 * cs + 1);
} else if (cs < accel_steps + coast_steps || busy) { // coast at max speed
}
else if (cs < total_steps) { // deceleration
cn = cn - (cn * 2) / (4 * (cs - total_steps) + 1);
}
timerAlarmWrite(timer, cn * 1000000L, true);
t1 = micros();
servo.write(angle);
if (dx >= dy && dx >= dz && dx >= de) {
digitalWrite(stepPinX, HIGH);
px_step = (dirx ? px_step + 1 : px_step - 1);
over += dy;
if (over >= dx) {
over -= dx;
py_step = (diry ? py_step + 1 : py_step - 1);
digitalWrite(stepPinY, HIGH);
}
over += dz;
if (over >= dx) {
over -= dx;
pz_step = (dirz ? pz_step + 1 : pz_step - 1);
digitalWrite(stepPinZ, HIGH);
}
over += de;
if (over >= dx) {
over -= dx;
pe_step = (dire ? pe_step + 1 : pe_step - 1);
digitalWrite(stepPinE, HIGH);
}
delayMicroseconds(1);
while (motorPaused == true && !motorStopped) { // pause trus stop
delay(500);
}
if (motorStopped) {
motorPaused = false;
}
}
else if (dy >= dx && dy >= dz && dy >= de) {
digitalWrite(stepPinY, HIGH);
py_step = (diry ? py_step + 1 : py_step - 1);
over += dx;
if (over >= dy) {
over -= dy;
px_step = (dirx ? px_step + 1 : px_step - 1);
digitalWrite(stepPinX, HIGH);
}
over += dz;
if (over >= dy) {
over -= dy;
pz_step = (dirz ? pz_step + 1 : pz_step - 1);
digitalWrite(stepPinZ, HIGH);
}
over += de;
if (over >= dy) {
over -= dy;
pe_step = (dire ? pe_step + 1 : pe_step - 1);
digitalWrite(stepPinE, HIGH);
}
delayMicroseconds(1);
while (motorPaused == true && !motorStopped) { // pause trus stop
delay(500);
}
if (motorStopped) {
motorPaused = false;
}
}
else if (dz >= dx && dz >= dy && dz >= de) {
digitalWrite(stepPinZ, HIGH);
pz_step = (dirz ? pz_step + 1 : pz_step - 1);
over += dx;
if (over >= dz) {
over -= dz;
px_step = (dirx ? px_step + 1 : px_step - 1);
digitalWrite(stepPinX, HIGH);
}
over += dy;
if (over >= dz) {
over -= dz;
pz_step = (dirz ? pz_step + 1 : pz_step - 1);
digitalWrite(stepPinY, HIGH);
}
over += de;
if (over >= dz) {
over -= dz;
pe_step = (dire ? pe_step + 1 : pe_step - 1);
digitalWrite(stepPinE, HIGH);
}
delayMicroseconds(1);
}
else if (de >= dx && de >= dy && de >= dz) {
digitalWrite(stepPinE, HIGH);
pe_step = (dire ? pe_step + 1 : pe_step - 1);
over += dx;
if (over >= de) {
over -= de;
px_step = (dirx ? px_step + 1 : px_step - 1);
digitalWrite(stepPinX, HIGH);
}
over += dy;
if (over >= de) {
over -= de;
py_step = (diry ? py_step + 1 : py_step - 1);
digitalWrite(stepPinY, HIGH);
}
over += dz;
if (over >= de) {
over -= de;
pz_step = (dirz ? pz_step + 1 : pz_step - 1);
digitalWrite(stepPinZ, HIGH);
}
delayMicroseconds(1);
while (motorPaused == true && !motorStopped) { // pause trus stop
delay(500);
}
if (motorStopped) {
motorPaused = false;
}
}
px = px_step / STEPS_MM;
py = py_step / STEPS_MM;
pz = pz_step / STEPS_MM;
pe = pe_step / STEPS_MM;
t = micros() - t1;
// if(!noACCEL){
digitalWrite(stepPinY, LOW);
digitalWrite(stepPinX, LOW);
digitalWrite(stepPinZ, LOW);
digitalWrite(stepPinE, LOW);
// }
}
}
//------------------------------------------------------------------------------
// METHODS
//------------------------------------------------------------------------------
void pause(long ms) {
delay(ms / 1000);
delayMicroseconds(ms % 1000); // delayMicroseconds tidak berfungsi untuk nilai > ~16k.
}
/**
Set feedrate (kecepatan motor akan bergerak)
@input nfr kecepatan baru dalam steps/second
*/
void feedrate(float nfr)
{
if (fr == nfr)
return; // same as last time? quit now.
if (nfr > MAX_FEEDRATE || nfr < MIN_FEEDRATE)
{ // don't allow crazy feed rates
Serial.print(F("New feedrate must be greater than "));
Serial.print(MIN_FEEDRATE);
Serial.print(F("steps/s and less than "));
Serial.print(MAX_FEEDRATE);
Serial.println(F("steps/s."));
return;
}
step_delay = 1000000.0 / nfr;
fr = nfr;
}
/**
Set the logical position
@input npx new position x
@input npy new position y
*/
void position(float npx, float npy, float npz, float npe)
{
px = npx;
py = npy;
pz = npz;
pe = npe;
}
/**
Menggunakan algoritma garis bresenham untuk menggerakkan motor pada sumbu X, Y, dan Z
@input newx tujuan x position
@input newy tujuan y position
@input newz tujuan z position
**/
void line(float newx, float newy, float newz, float newe)
{
dx = newx - px;
dy = newy - py;
dz = newz - pz;
de = newe - pe;
angle = atan2(dy, dx) * 180 / PI;
dirx = dx > 0;
diry = dy > 0;
dirz = dz > 0;
dire = de > 0;
dx = abs(dx);
dy = abs(dy);
dz = abs(dz);
de = abs(de);
Serial.print("line: ");
Serial.print(dx);
Serial.print(",");
Serial.print(dy);
Serial.print(",");
Serial.print(dz);
Serial.print(",");
Serial.println(de);
long i;
over = 0;
total_steps = max(max(dx, dy), max(dz, de)) * STEPS_MM;
float ta = fr / ACCEL; // ta : waktu percepatan, klo mau steps per menit perlu dibagi 60
float da = (fr) * ta / 2; // da : jarak percepatan, d = a * ta * 1/2, Rumus asli : da = 1/2 * ta * Vmax
accel_steps = _min(da * STEPS_MM, total_steps / 2); // just in case feedrate cannot be reached
coast_steps = total_steps - accel_steps * 2; // kecepatan konstan
digitalWrite(directionPinX, dirx); // direction of X motor
digitalWrite(directionPinY, diry);
digitalWrite(directionPinZ, dirz); // direction of X motor
digitalWrite(directionPinE, dire);
cs = 0; // let the motion start :-)
cn = t0;
if (dx >= dy)
{
over = dx / 2;
}
else
{
over = dy / 2;
}
busy = true;
}
// mengembalikan sudut dy/dx sebagai nilai dari 0...2PI
static float atan3(float dy, float dx)
{
float a = atan2(dy, dx);
if (a < 0)
a = (PI * 2.0) + a;
return a;
}
// Metode ini mengasumsikan batasan telah diperiksa.
// Metode ini mengasumsikan kecocokan radius awal dan akhir.
// Metode ini mengasumsikan busur tidak >180 derajat (radian PI)
// cx/cy - pusat lingkaran
// x/y - posisi akhir
// dir - ARC_CW atau ARC_CCW untuk mengontrol arah busur
// void arc(float i, float j, float x, float y, int direction) {
// float radius = hypot(i, j);
// float start_angle = atan2(j, i);
// float end_angle = atan2(y - py, x - px);
// if (direction == ARC_CW && end_angle > start_angle) {
// end_angle -= 2 * PI;
// } else if (direction == ARC_CCW && start_angle > end_angle) {
// end_angle += 2 * PI;
// }
// float total_angle = end_angle - start_angle;
// int num_segments = abs(total_angle * radius / MM_PER_SEGMENT);
// float angle_step = total_angle / num_segments;
// for (int s = 0; s < num_segments; s++) {
// while (busy) {
// delay(1);
// }
// float angle = start_angle + s * angle_step;
// float nx = px + radius * cos(angle);
// float ny = py + radius * sin(angle);
// line(nx, ny, pz, pe);
// busy = true;
// }
// // Pastikan kita sampai di titik akhir karena kesalahan pembulatan
// line(x, y, pz, pe);
// }
void arc(float i, float j, float x, float y, int8_t direction) {
float centerX = px + i;
float centerY = py + j;
float radius = sqrt(i*i + j*j);
float startAngle = atan2(-j, -i) * 180 / PI;
float endAngle = atan2(y-centerY, x-centerX) * 180 / PI;
// Adjust angles for clockwise direction
if (direction == ARC_CW && endAngle >= startAngle) {
endAngle -= 360;
} else if (direction == ARC_CCW && endAngle <= startAngle) {
endAngle += 360;
}
// Calculate the number of steps, this will depend on your motor and controller
int steps = abs(endAngle - startAngle) * STEPS_MM;
for (int step = 0; step < steps; step++) {
// Wait for the previous movement to complete
while (busy) {
delay(1); // You might need to adjust this delay
}
float angle = startAngle + (endAngle - startAngle) * step / steps;
int x = centerX + radius * cos(angle * PI / 180);
int y = centerY + radius * sin(angle * PI / 180);
// Issue the next movement
line(x, y, pz, pe);
}
}
// static void arc(float cx, float cy, float x, float y, float dir) {
// Serial.println("jalanin G02");
// // get radius
// float dx = cx - px;
// float dy = cy - py;
// float radius = sqrt(dx * dx + dy * dy);
// // find angle of arc (sweep)
// float angle1 = atan3(dy, dx);
// float angle2 = atan3(y - cy, x - cx);
// // Ensure angles are within 0 to 2*PI range
// if (angle1 < 0)
// angle1 += 2 * PI;
// if (angle2 < 0)
// angle2 += 2 * PI;
// // Calculate sweep angle (theta)
// float theta = angle2 - angle1;
// if (dir > 0 && theta < 0)
// theta += 2 * PI;
// else if (dir < 0 && theta > 0)
// theta -= 2 * PI;
// // Ensure theta is within 0 to 2*PI range
// if (theta < 0)
// theta += 2 * PI;
// // get length of arc
// float len = abs(theta) * radius;
// int segments = ceil(len / MM_PER_SEGMENT); // use floating point division
// // Interpolate along the circle
// for (int i = 1; i <= segments; ++i)
// {
// while (busy) {
// delay(1);
// }
// float angle3 = angle1 + (theta * i) / segments;
// float nx = cx + cos(angle3) * radius;
// float ny = cy + sin(angle3) * radius;
// Serial.print("nx:");
// Serial.println(nx);
// Serial.print("ny:");
// Serial.println(ny);
// // send to planner
// line(nx, ny, pz, pe);
// busy = true;
// }
// line(x, y, pz, pe);
// }
// static void arc(float cx, float cy, float x, float y, float dir){
// // get radius
// float dx = cx - px;
// float dy = cy - py;
// float radius = sqrt(dx * dx + dy * dy);
// // find angle of arc (sweep)
// float angle1 = atan2(dy, dx);
// float angle2 = atan2(y - cy, x - cx);
// if (angle1 < 0)
// angle1 += 2 * PI;
// if (angle2 < 0)
// angle2 += 2 * PI;
// float theta = angle2 - angle1;
// if (dir > 0 && theta < 0)
// theta += 2 * PI;
// else if (dir < 0 && theta > 0)
// theta -= 2 * PI;
// // get length of arc
// float len = abs(theta) * radius;
// int segments = ceil(len / MM_PER_SEGMENT); // use floating point division
// // Interpolasi sepanjang lingkaran
// for (int i = 1; i <= segments; ++i)
// {
// while (busy) {
// delay(1);
// }
// float angle3 = angle1 + (theta * i) / segments;
// float nx = cx + cos(angle3) * radius;
// float ny = cy + sin(angle3) * radius;
// Serial.print("nx:");
// Serial.println(nx);
// Serial.print("ny:");
// Serial.println(ny);
// // kirim ke planner
// line(nx, ny, pz, pe);
// busy = true;
// }
// line(x, y, pz, pe);
// }
/**
Cari karakter /code/ di buffer dan baca float setelahnya.
@return nilai yang ditemukan. Jika tidak ada yang ditemukan, /val/ dikembalikan.
@input code karakter yang ingin dicari.
@input val nilai kembalian jika /code/ tidak ditemukan.
**/
float parseNumber(char code, float val)
{
char *ptr = serialBuffer; // start at the beginning of buffer
while ((long)ptr > 1 && (*ptr) && (long)ptr < (long)serialBuffer + sofar)
{ // walk to the end
if (*ptr == code)
{ // ketika ketemu code
return atof(ptr + 1); // ubah digit berikutnya menjadi float dan kembalikan
}
ptr = strchr(ptr, ' ') + 1; // ambil satu langkah dari sini ke huruf setelah spasi berikutnya
}
return val; // selesai, return default val
}
/**
tulis string diikuti dengan float ke serial line.
@input code the string.
@input val the float.
*/
void output(const char *code, float val)
{
Serial.print(code);
Serial.println(val);
}
/**
print the current position, feedrate, and absolute mode.
*/
void where()
{
output("X", px);
output("Y", py);
output("Z", pz);
output("E", pe);
output("F", fr);
Serial.println(mode_abs ? "ABS" : "REL");
}
/**
display helpful information
*/
void help()
{
Serial.print(F("GcodeCNCDemo2AxisV1 "));
Serial.println(F("Commands:"));
Serial.println(F("G00 [X(steps)] [Y(steps)] [F(feedrate)]; - line"));
Serial.println(F("G01 [X(steps)] [Y(steps)] [F(feedrate)]; - line"));
Serial.println(F("G02 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - clockwise arc"));
Serial.println(F("G03 [X(steps)] [Y(steps)] [I(steps)] [J(steps)] [F(feedrate)]; - counter-clockwise arc"));
Serial.println(F("G04 P[seconds]; - delay"));
Serial.println(F("G28 - home"));
Serial.println(F("G90; - absolute mode"));
Serial.println(F("G91; - relative mode"));
Serial.println(F("G92 [X(steps)] [Y(steps)]; - change logical position"));
Serial.println(F("M18; - disable motors"));
Serial.println(F("M100; - this help message"));
Serial.println(F("M114; - report position and feedrate"));
Serial.println(F("All commands must end with a newline."));
}
/**
Baca buffer input dan temukan perintah yang dikenali. Satu perintah G atau M per baris.
*/
void processCommand()
{
int cmd = parseNumber('G', -1);
switch (cmd)
{
case 0:
case 1:
{ // line
feedrate(parseNumber('F', fr));
MotorCommand cmd;
cmd.x = parseNumber('X', (mode_abs ? px : 0)) + (mode_abs ? 0 : px);
cmd.y = parseNumber('Y', (mode_abs ? py : 0)) + (mode_abs ? 0 : py);
cmd.z = parseNumber('Z', (mode_abs ? pz : 0)) + (mode_abs ? 0 : pz);
cmd.e = parseNumber('E', (mode_abs ? pe : 0)) + (mode_abs ? 0 : pe);
xQueueSend(queueXYZE, &cmd, portMAX_DELAY);
break;
}
case 2:
case 3:
{
feedrate(parseNumber('F', fr));
arc(parseNumber('I', (mode_abs ? px : 0)) + (mode_abs ? 0 : px),
parseNumber('J', (mode_abs ? py : 0)) + (mode_abs ? 0 : py),
parseNumber('X', (mode_abs ? px : 0)) + (mode_abs ? 0 : px),
parseNumber('Y', (mode_abs ? py : 0)) + (mode_abs ? 0 : py),
(cmd == 2) ? -1 : 1);
break;
}
case 4:
break;
case 28: //go home
MotorCommand cmd;
cmd.x = 0;
cmd.y = 0;
cmd.z = 0;
xQueueSend(queueXYZE, &cmd, portMAX_DELAY);
break;
case 90:
mode_abs = 1; // absolute mode
break;
case 91:
mode_abs = 0; // relative mode
break;
default:
{
Serial.println("Command tidak dikenali");
break;
}
}
cmd = parseNumber('M', -1);
switch (cmd)
{
case 100:
help();
break;
case 114:
where();
break;
default:
break;
}
}
/**
menyiapkan input buffer untuk menerima pesan baru dan memberi tahu perangkat yang terhubung secara serial bahwa ia siap menerima pesan lebih lanjut.
*/
void ready()
{
sofar = 0; // clear input buffer
Serial.print(F(">")); // signal ready untuk menerima input
}
void lineTaskX(void *parameter)
{
float newx;
for (;;)
{
if (xQueueReceive(queueX, &newx, portMAX_DELAY))
{
line(newx, py, pz, pe);
}
}
}
void lineTaskY(void *parameter)
{
float newy;
for (;;)
{
if (xQueueReceive(queueY, &newy, portMAX_DELAY))
{
line(px, newy, pz, pe);
}
}
}
void lineTaskZ(void *parameter)
{
float newz;
for (;;)
{
if (xQueueReceive(queueZ, &newz, portMAX_DELAY))
{
line(px, py, newz, pe);
}
}
}
void lineTask(void *parameter)
{
MotorCommand receivedCmd;
for (;;)
{
if (xQueueReceive(queueXYZE, &receivedCmd, portMAX_DELAY))
{
line(receivedCmd.x, receivedCmd.y, receivedCmd.z, receivedCmd.e);
}
}
}
/**
Hal pertama yang dilakukan mesin ini saat startup. Hanya berjalan sekali.
*/
void setup()
{
Serial.begin(115200); // open coms
// x
pinMode(directionPinX, OUTPUT);
pinMode(stepPinX, OUTPUT);
pinMode(directionPinY, OUTPUT);
// y
pinMode(stepPinY, OUTPUT);
pinMode(enableX, OUTPUT);
pinMode(enableY, OUTPUT);
// z
pinMode(enableZ, OUTPUT);
pinMode(directionPinZ, OUTPUT);
pinMode(stepPinZ, OUTPUT);
// e
pinMode(enableE, OUTPUT);
pinMode(directionPinE, OUTPUT);
pinMode(stepPinE, OUTPUT);
servo.attach(SERVO_PIN);
position(0, 0, 0, 0); // set starting position for X, Y, Z
feedrate((MAX_FEEDRATE + MIN_FEEDRATE) / 2); // set default speed
queueX = xQueueCreate(10, sizeof(float));
queueY = xQueueCreate(10, sizeof(float));
queueZ = xQueueCreate(10, sizeof(float));
xTaskCreate(lineTaskX, "Line Task X", 10000, NULL, 1, NULL);
xTaskCreate(lineTaskY, "Line Task Y", 10000, NULL, 1, NULL);
xTaskCreate(lineTaskZ, "Line Task Z", 10000, NULL, 1, NULL);
xTaskCreate(lineTask, "Line Task", 10000, NULL, 1, NULL);
timer = timerBegin(0, 80, true);
timerAttachInterrupt(timer, &itr, true);
timerAlarmWrite(timer, 1000000, true);
timerAlarmEnable(timer);
help();
ready();
}
void loop()
{
// listen for serial commands
while (Serial.available() > 0)
{ // if something is available
char c = Serial.read(); // get it
Serial.print(c); // repeat it back so I know you got the message
if (sofar < MAX_BUF - 1)
serialBuffer[sofar++] = c; // store it
if ((c == '\n') || (c == '\r'))
{
// entire message received
serialBuffer[sofar] = 0; // end the buffer so string functions work right
Serial.print(F("\r\n")); // echo a return character for humans
processCommand(); // do something with the command
ready();
}
}
// Serial.println(t);
// delay(1000);
}