|
42 | 42 | // If enabled, check for soft limit violations. Placed here all line motions are picked up |
43 | 43 | // from everywhere in Grbl. |
44 | 44 | if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); } |
45 | | - |
46 | 45 | // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. |
47 | 46 | if (sys.state == STATE_CHECK_MODE) { return; } |
48 | 47 |
|
|
67 | 66 | float final_angle[3]; |
68 | 67 |
|
69 | 68 | float x= target[X_AXIS], y= target[Y_AXIS], z = target[Z_AXIS]; |
70 | | - |
| 69 | + if ((uarm.motor_state_bits&0x07)!=0x07) return UARM_ENABLE_ERROR; |
71 | 70 | coord_effect2arm( &x, &y, &z ); // calculate the arm current coord |
72 | 71 | coord_to_angle( x, y, z, |
73 | 72 | &final_angle[X_AXIS], &final_angle[Y_AXIS], &final_angle[Z_AXIS] ); // calculate final angle |
|
84 | 83 | if( is_angle_legal( final_angle[X_AXIS], final_angle[Y_AXIS], final_angle[Z_AXIS] ) == false ){ // check the angle |
85 | 84 | return UARM_COORD_ERROR; |
86 | 85 | } |
87 | | - |
| 86 | + |
88 | 87 | // angle_to_coord( final_angle[X_AXIS], final_angle[Y_AXIS], final_angle[Z_AXIS], &x, &y, &z ); |
89 | 88 | // coord_arm2effect( &x, &y, &z ); |
90 | 89 |
|
91 | 90 | // target[X_AXIS] = x; |
92 | 91 | // target[Y_AXIS] = y; |
93 | 92 | // target[Z_AXIS] = z; |
94 | | - |
| 93 | + if(feed_rate!=0) |
| 94 | + { |
95 | 95 | step_to_coord( uarm.target_step[X_AXIS], uarm.target_step[Y_AXIS], uarm.target_step[Z_AXIS], |
96 | 96 | ¤t_coord[X_AXIS], ¤t_coord[Y_AXIS], ¤t_coord[Z_AXIS]); // calculate the arm current coord |
97 | 97 | coord_arm2effect( ¤t_coord[X_AXIS], ¤t_coord[Y_AXIS], ¤t_coord[Z_AXIS] ); // calculate the effect current coord |
|
138 | 138 | coord_effect2arm( &final_coord[X_AXIS], &final_coord[Y_AXIS], &final_coord[Z_AXIS] ); // <! convert the position |
139 | 139 |
|
140 | 140 | coord_to_angle( final_coord[X_AXIS], final_coord[Y_AXIS], final_coord[Z_AXIS], |
141 | | - &angle_x, &angle_y, &angle_z ); // <! calculate final angle |
| 141 | + &angle_x, &angle_y |
| 142 | +, &angle_z ); // <! calculate final angle |
142 | 143 |
|
143 | 144 | if( is_angle_legal( angle_x, angle_y, angle_z ) == false ){ // check the angle |
144 | 145 | continue; |
|
160 | 161 | else { break; } |
161 | 162 | } while (1); |
162 | 163 |
|
163 | | - uarm.target_step[X_AXIS] = final_step[X_AXIS]; |
| 164 | + uarm.target_step[X_AXIS] = final_step[X_AXIS]; |
164 | 165 | uarm.target_step[Y_AXIS] = final_step[Y_AXIS]; |
165 | 166 | uarm.target_step[Z_AXIS] = final_step[Z_AXIS]; |
166 | 167 |
|
167 | 168 | final_step[X_AXIS] = (final_step[X_AXIS]) / (settings.steps_per_mm[X_AXIS]); |
168 | 169 | final_step[Y_AXIS] = (final_step[Y_AXIS]) / (settings.steps_per_mm[Y_AXIS]); |
169 | 170 | final_step[Z_AXIS] = (final_step[Z_AXIS]) / (settings.steps_per_mm[Z_AXIS]); |
| 171 | + |
| 172 | + plan_buffer_line(final_step, feed_rate, invert_feed_rate); |
170 | 173 |
|
171 | | - plan_buffer_line(final_step, feed_rate, invert_feed_rate); |
172 | | - |
173 | | - //DB_PRINT_STR( "%d %d %d\r\n", final_step[X_AXIS], final_step[Y_AXIS], final_step[Z_AXIS] ); |
174 | | - uarm.run_flag = true; |
175 | | - |
| 174 | + //DB_PRINT_STR( "%d %d %d\r\n", final_step[X_AXIS], final_step[Y_AXIS], final_step[Z_AXIS] ); |
| 175 | + uarm.run_flag = true; |
| 176 | + |
| 177 | + |
| 178 | + } |
| 179 | + } |
| 180 | + else |
| 181 | + { |
| 182 | + return STATUS_OK; |
176 | 183 | } |
177 | 184 | return STATUS_OK; |
178 | 185 | } |
@@ -459,7 +466,7 @@ void mc_reset() |
459 | 466 | if ((sys.state & (STATE_CYCLE | STATE_HOMING)) || (sys.suspend == SUSPEND_ENABLE_HOLD)) { |
460 | 467 | if (sys.state == STATE_HOMING) { bit_true_atomic(sys_rt_exec_alarm, EXEC_ALARM_HOMING_FAIL); } |
461 | 468 | else { bit_true_atomic(sys_rt_exec_alarm, EXEC_ALARM_ABORT_CYCLE); } |
462 | | - st_go_idle(); // Force kill steppers. Position has likely been lost. |
| 469 | + st_go_idle_2(); // Force kill steppers. Position has likely been lost. |
463 | 470 | } |
464 | 471 | } |
465 | 472 | } |
|
0 commit comments