machine.c (8951B)
1 #include "machine.h" 2 3 #include <math.h> 4 5 #define DEFAULT_FEED_RATE 3000 // 3000 mm/m = 50 mm/s 6 #define DEFAULT_UNITS UNITS_MM 7 #define DEFAULT_POSITIONING_MODE POSITIONING_MODE_ABSOLUTE 8 9 ExecutionResult machine_execute_m_code(Machine* machine, const Command* command); 10 ExecutionResult machine_execute_g_code( 11 Machine* machine, 12 const Command* command, 13 const Time* now 14 ); 15 16 ExecutionResult machine_execute_g1( 17 Machine* machine, 18 const Command* command, 19 const Time* now 20 ); 21 ExecutionResult machine_execute_g21(Machine* machine, const Command* command); 22 ExecutionResult machine_execute_g90(Machine* machine, const Command* command); 23 ExecutionResult machine_execute_g91(Machine* machine, const Command* command); 24 ExecutionResult machine_execute_g92(Machine* machine, const Command* command); 25 26 void machine_init(Machine* machine) { 27 machine->x = 0; 28 machine->y = 0; 29 machine->z = 0; 30 machine->feed_rate = DEFAULT_FEED_RATE; 31 machine->units = DEFAULT_UNITS; 32 machine->positioning_mode = DEFAULT_POSITIONING_MODE; 33 machine->current_move.type = MOVE_NONE; 34 } 35 36 ExecutionResult machine_execute_command( 37 Machine* machine, 38 const Command* command, 39 const Time* now 40 ) { 41 switch (command->code.type) { 42 case CODE_M: 43 return machine_execute_m_code(machine, command); 44 case CODE_G: 45 return machine_execute_g_code(machine, command, now); 46 default: { 47 ExecutionResult result = { 48 .status = EXECUTE_STATUS__UNRECOGNIZED_COMMAND, 49 .has_data = false, 50 }; 51 return result; 52 } 53 } 54 } 55 56 ExecutionResult machine_execute_m_code( 57 Machine* machine, 58 const Command* command 59 ) { 60 switch (command->code.value) { 61 default: { 62 ExecutionResult result = { 63 .status = EXECUTE_STATUS__UNRECOGNIZED_MCODE, 64 .has_data = false, 65 }; 66 return result; 67 } 68 } 69 } 70 71 ExecutionResult machine_execute_g_code( 72 Machine* machine, 73 const Command* command, 74 const Time* now 75 ) { 76 switch (command->code.value) { 77 case 0: 78 case 1: 79 return machine_execute_g1(machine, command, now); 80 case 21: 81 return machine_execute_g21(machine, command); 82 case 90: 83 return machine_execute_g90(machine, command); 84 case 91: 85 return machine_execute_g91(machine, command); 86 case 92: 87 return machine_execute_g92(machine, command); 88 default: { 89 ExecutionResult result = { 90 .status = EXECUTE_STATUS__UNRECOGNIZED_GCODE, 91 .has_data = false, 92 }; 93 return result; 94 } 95 } 96 } 97 98 float x_distance(Machine* machine, const Command* command) { 99 if (!command->param_x.is_set) { 100 return 0; 101 } 102 switch (machine->positioning_mode) { 103 case POSITIONING_MODE_ABSOLUTE: 104 return command->param_x.value - machine->x; 105 case POSITIONING_MODE_RELATIVE: 106 return command->param_x.value; 107 } 108 } 109 110 float y_distance(Machine* machine, const Command* command) { 111 if (!command->param_y.is_set) { 112 return 0; 113 } 114 switch (machine->positioning_mode) { 115 case POSITIONING_MODE_ABSOLUTE: 116 return command->param_y.value - machine->y; 117 case POSITIONING_MODE_RELATIVE: 118 return command->param_y.value; 119 } 120 } 121 122 float z_distance(Machine* machine, const Command* command) { 123 if (!command->param_z.is_set) { 124 return 0; 125 } 126 switch (machine->positioning_mode) { 127 case POSITIONING_MODE_ABSOLUTE: 128 return command->param_z.value - machine->z; 129 case POSITIONING_MODE_RELATIVE: 130 return command->param_z.value; 131 default: 132 return 0; 133 } 134 } 135 136 Time g1_execute_duration(Machine* machine, const Command* command) { 137 float dx = x_distance(machine, command); 138 float dy = y_distance(machine, command); 139 float dz = z_distance(machine, command); 140 float d = sqrtf((dx * dx) + (dy * dy) + (dz * dz)); 141 float t = 60.0f * d / machine->feed_rate; 142 return time_make(0, (long)round(t * 1e9)); 143 } 144 145 ExecutionResult machine_execute_g1_start( 146 Machine* machine, 147 const Command* command, 148 const Time* now 149 ) { 150 if (command->param_f.is_set) { 151 machine->feed_rate = command->param_f.value; 152 } 153 machine->current_move.type = MOVE_LINEAR; 154 machine->current_move.start = *now; 155 machine->current_move.duration = g1_execute_duration(machine, command); 156 machine->current_move.end = time_add(now, &machine->current_move.duration); 157 machine->current_move.x.start = machine->x; 158 machine->current_move.y.start = machine->y; 159 machine->current_move.z.start = machine->z; 160 machine->current_move.x.distance = x_distance(machine, command); 161 machine->current_move.y.distance = y_distance(machine, command); 162 machine->current_move.z.distance = z_distance(machine, command); 163 if (time_eql_zero(&machine->current_move.duration)) { 164 machine->current_move.type = MOVE_NONE; 165 ExecutionResult result = { 166 .status = EXECUTE_STATUS__FINISHED, 167 .has_data = false, 168 }; 169 return result; 170 } 171 ExecutionResult result = { 172 .status = EXECUTE_STATUS__IN_PROGRESS, 173 .has_data = false, 174 }; 175 return result; 176 } 177 178 ExecutionResult machine_execute_g1_end( 179 Machine* machine, 180 const Command* command, 181 const Time* now 182 ) { 183 machine->x = machine->current_move.x.start + machine->current_move.x.distance; 184 machine->y = machine->current_move.y.start + machine->current_move.y.distance; 185 machine->z = machine->current_move.z.start + machine->current_move.z.distance; 186 machine->current_move.type = MOVE_NONE; 187 ControlData data = { 188 .x = machine->x, 189 .y = machine->y, 190 .z = machine->z, 191 }; 192 ExecutionResult result = { 193 .status = EXECUTE_STATUS__FINISHED, 194 .has_data = true, 195 .data = data, 196 }; 197 return result; 198 } 199 200 ExecutionResult machine_execute_g1_tick( 201 Machine* machine, 202 const Command* command, 203 const Time* now 204 ) { 205 Time elapsed = time_sub(now, &machine->current_move.start); 206 float percent_complete = time_div(&elapsed, &machine->current_move.duration); 207 float dx = machine->current_move.x.distance * percent_complete; 208 float dy = machine->current_move.y.distance * percent_complete; 209 float dz = machine->current_move.z.distance * percent_complete; 210 machine->x = machine->current_move.x.start + dx; 211 machine->y = machine->current_move.y.start + dy; 212 machine->z = machine->current_move.z.start + dz; 213 ControlData data = { 214 .x = machine->x, 215 .y = machine->y, 216 .z = machine->z, 217 }; 218 ExecutionResult result = { 219 .status = EXECUTE_STATUS__IN_PROGRESS, 220 .has_data = true, 221 .data = data, 222 }; 223 return result; 224 } 225 226 ExecutionResult machine_execute_g1( 227 Machine* machine, 228 const Command* command, 229 const Time* now 230 ) { 231 if (machine->current_move.type == MOVE_NONE) { 232 return machine_execute_g1_start(machine, command, now); 233 } 234 if (time_cmp(now, &machine->current_move.end) >= 0) { 235 return machine_execute_g1_end(machine, command, now); 236 } 237 return machine_execute_g1_tick(machine, command, now); 238 } 239 240 ExecutionResult machine_execute_g21(Machine* machine, const Command* command) { 241 machine->units = UNITS_MM; 242 ExecutionResult result = { 243 .status = EXECUTE_STATUS__FINISHED, 244 .has_data = false, 245 }; 246 return result; 247 } 248 249 ExecutionResult machine_execute_g90(Machine* machine, const Command* command) { 250 machine->positioning_mode = POSITIONING_MODE_ABSOLUTE; 251 ExecutionResult result = { 252 .status = EXECUTE_STATUS__FINISHED, 253 .has_data = false, 254 }; 255 return result; 256 } 257 258 ExecutionResult machine_execute_g91(Machine* machine, const Command* command) { 259 machine->positioning_mode = POSITIONING_MODE_RELATIVE; 260 ExecutionResult result = { 261 .status = EXECUTE_STATUS__FINISHED, 262 .has_data = false, 263 }; 264 return result; 265 } 266 267 ExecutionResult machine_execute_g92(Machine* machine, const Command* command) { 268 if ( 269 !command->param_x.is_set && 270 !command->param_y.is_set && 271 !command->param_z.is_set 272 ) { 273 machine->x = 0.0f; 274 machine->y = 0.0f; 275 machine->z = 0.0f; 276 ExecutionResult result = { 277 .status = EXECUTE_STATUS__FINISHED, 278 .has_data = false, 279 }; 280 return result; 281 } 282 283 if (command->param_x.is_set) { 284 machine->x = command->param_x.value; 285 } 286 if (command->param_y.is_set) { 287 machine->y = command->param_y.value; 288 } 289 if (command->param_z.is_set) { 290 machine->z = command->param_z.value; 291 } 292 ExecutionResult result = { 293 .status = EXECUTE_STATUS__FINISHED, 294 .has_data = false, 295 }; 296 return result; 297 }