Commit | Line | Data |
---|---|---|
4cff3ded AW |
1 | #include "mbed.h" |
2 | #include "libs/Module.h" | |
3 | #include "libs/Kernel.h" | |
13e4a3f9 | 4 | #include "modules/robot/Player.h" |
4cff3ded AW |
5 | #include "modules/robot/Block.h" |
6 | #include "modules/tools/extruder/Extruder.h" | |
7 | ||
0eb11a06 | 8 | Extruder::Extruder(PinName stppin, PinName dirpin) : step_pin(stppin), dir_pin(dirpin) { |
436a2cd1 | 9 | this->absolute_mode = true; |
0eb11a06 | 10 | this->direction = 1; |
ded56b35 | 11 | this->acceleration_lock = false; |
d9ebc974 AW |
12 | this->step_counter = 0; |
13 | this->counter_increment = 0; | |
436a2cd1 | 14 | } |
4cff3ded AW |
15 | |
16 | void Extruder::on_module_loaded() { | |
f5598f5b | 17 | |
ded56b35 | 18 | // Do not do anything if not enabledd |
436a2cd1 | 19 | if( this->kernel->config->value( extruder_module_enable_checksum )->by_default(false)->as_bool() == false ){ return; } |
f5598f5b | 20 | |
4cff3ded | 21 | // Settings |
da24d6ae | 22 | this->on_config_reload(this); |
4cff3ded AW |
23 | |
24 | // We work on the same Block as Stepper, so we need to know when it gets a new one and drops one | |
25 | this->register_for_event(ON_BLOCK_BEGIN); | |
26 | this->register_for_event(ON_BLOCK_END); | |
436a2cd1 | 27 | this->register_for_event(ON_GCODE_EXECUTE); |
4cff3ded | 28 | |
ded56b35 | 29 | // Start values |
4cff3ded AW |
30 | this->start_position = 0; |
31 | this->target_position = 0; | |
32 | this->current_position = 0; | |
33 | this->current_block = NULL; | |
ded56b35 AW |
34 | this->mode = OFF; |
35 | ||
ded56b35 AW |
36 | // Update speed every *acceleration_ticks_per_second* |
37 | // TODO: Make this an independent setting | |
d9ebc974 | 38 | this->kernel->slow_ticker->attach( this->kernel->stepper->acceleration_ticks_per_second , this, &Extruder::acceleration_tick ); |
ded56b35 | 39 | |
3b1e82d2 AW |
40 | // Initiate main_interrupt timer and step reset timer |
41 | this->kernel->step_ticker->attach( this, &Extruder::stepping_tick ); | |
42 | this->kernel->step_ticker->reset_attach( this, &Extruder::reset_step_pin ); | |
43 | ||
4cff3ded AW |
44 | } |
45 | ||
2bb8b390 | 46 | // Get config |
da24d6ae | 47 | void Extruder::on_config_reload(void* argument){ |
dd3b416b AW |
48 | this->microseconds_per_step_pulse = this->kernel->config->value(microseconds_per_step_pulse_ckeckusm)->by_default(5)->as_number(); |
49 | this->steps_per_millimeter = this->kernel->config->value(steps_per_millimeter_checksum )->by_default(1)->as_number(); | |
50 | this->feed_rate = this->kernel->config->value(default_feed_rate_checksum )->by_default(1)->as_number(); | |
51 | this->acceleration = this->kernel->config->value(acceleration_checksum )->by_default(1)->as_number(); | |
436a2cd1 AW |
52 | } |
53 | ||
ded56b35 | 54 | // Compute extrusion speed based on parameters and gcode distance of travel |
436a2cd1 AW |
55 | void Extruder::on_gcode_execute(void* argument){ |
56 | Gcode* gcode = static_cast<Gcode*>(argument); | |
ded56b35 | 57 | |
436a2cd1 AW |
58 | // Absolute/relative mode |
59 | if( gcode->has_letter('M')){ | |
60 | int code = gcode->get_value('M'); | |
61 | if( code == 82 ){ this->absolute_mode == true; } | |
62 | if( code == 83 ){ this->absolute_mode == false; } | |
63 | } | |
ded56b35 AW |
64 | |
65 | // The mode is OFF by default, and SOLO or FOLLOW only if we need to extrude | |
66 | this->mode = OFF; | |
67 | ||
1a2d88eb | 68 | if( gcode->has_letter('G') ){ |
ded56b35 | 69 | // G92: Reset extruder position |
1a2d88eb | 70 | if( gcode->get_value('G') == 92 ){ |
1a2d88eb | 71 | if( gcode->has_letter('E') ){ |
e2b4a32b | 72 | this->current_position = gcode->get_value('E'); |
1a2d88eb AW |
73 | this->target_position = this->current_position; |
74 | this->start_position = this->current_position; | |
75 | } | |
436a2cd1 | 76 | }else{ |
ded56b35 | 77 | // Extrusion length from 'G' Gcode |
1a2d88eb | 78 | if( gcode->has_letter('E' )){ |
ded56b35 AW |
79 | // Get relative extrusion distance depending on mode ( in absolute mode we must substract target_position ) |
80 | double relative_extrusion_distance = gcode->get_value('E'); | |
81 | if( this->absolute_mode == true ){ relative_extrusion_distance = relative_extrusion_distance - this->target_position; } | |
82 | ||
83 | // If the robot is moving, we follow it's movement, otherwise, we move alone | |
84 | if( fabs(gcode->millimeters_of_travel) < 0.0001 ){ // With floating numbers, we can have 0 != 0 ... beeeh | |
85 | this->mode = SOLO; | |
86 | this->travel_distance = relative_extrusion_distance; | |
87 | if( gcode->has_letter('F') ){ this->feed_rate = gcode->get_value('F'); } | |
1a2d88eb | 88 | }else{ |
ded56b35 AW |
89 | this->mode = FOLLOW; |
90 | // We move proportionally to the robot's movement | |
91 | this->travel_ratio = relative_extrusion_distance / gcode->millimeters_of_travel; | |
1a2d88eb | 92 | } |
ded56b35 | 93 | } |
1a2d88eb | 94 | } |
1a2d88eb | 95 | } |
ded56b35 | 96 | |
da24d6ae AW |
97 | } |
98 | ||
ded56b35 | 99 | // When a new block begins, either follow the robot, or step by ourselves ( or stay back and do nothing ) |
4cff3ded AW |
100 | void Extruder::on_block_begin(void* argument){ |
101 | Block* block = static_cast<Block*>(argument); | |
ded56b35 AW |
102 | if( this->mode == SOLO ){ |
103 | // In solo mode we take the block so we can move even if the stepper has nothing to do | |
104 | block->take(); | |
1a2d88eb | 105 | this->current_block = block; |
e2b4a32b | 106 | this->start_position = this->target_position; |
0eb11a06 | 107 | this->target_position = this->start_position + this->travel_distance ; |
ded56b35 AW |
108 | this->travel_ratio = 0.2; // TODO : Make a real acceleration thing |
109 | if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; } | |
110 | this->set_speed(int(floor((this->feed_rate/60)*this->steps_per_millimeter)));//Speed in steps per second | |
111 | }else if( this->mode == FOLLOW ){ | |
1a2d88eb AW |
112 | // In non-solo mode, we just follow the stepper module |
113 | this->current_block = block; | |
e2b4a32b AW |
114 | this->start_position = this->target_position; |
115 | this->target_position = this->start_position + ( this->current_block->millimeters * this->travel_ratio ); | |
ded56b35 AW |
116 | if( this->target_position > this->current_position ){ this->direction = 1; }else if( this->target_position < this->current_position ){ this->direction = -1; } |
117 | this->acceleration_tick(); | |
1a2d88eb | 118 | } |
e2b4a32b | 119 | |
4cff3ded AW |
120 | } |
121 | ||
ded56b35 | 122 | // When a block ends, pause the stepping interrupt |
4cff3ded AW |
123 | void Extruder::on_block_end(void* argument){ |
124 | Block* block = static_cast<Block*>(argument); | |
125 | this->current_block = NULL; | |
126 | } | |
127 | ||
ded56b35 AW |
128 | // Called periodically to change the speed to match acceleration or to match the speed of the robot |
129 | void Extruder::acceleration_tick(){ | |
1a2d88eb | 130 | |
ded56b35 AW |
131 | // Avoid trying to work when we really shouldn't ( between blocks or re-entry ) |
132 | if( this->current_block == NULL || this->acceleration_lock ){ return; } | |
133 | this->acceleration_lock = true; | |
134 | ||
135 | // In solo mode, we mode independently from the robot | |
136 | if( this->mode == SOLO ){ | |
137 | // TODO : Do real acceleration here | |
138 | this->travel_ratio += 0.03; | |
139 | if( this->travel_ratio > 1 ){ this->travel_ratio = 1; } | |
140 | this->set_speed( int(floor(((this->feed_rate/60)*this->steps_per_millimeter)*this->travel_ratio)) ); // Speed in steps per second | |
141 | ||
142 | // In follow mode we match the speed of the robot, + eventually advance | |
143 | }else if( this->mode == FOLLOW ){ | |
144 | Stepper* stepper = this->kernel->stepper; // Just for convenience | |
145 | ||
146 | // Strategy : | |
147 | // * Find where in the block will the stepper be at the next tick ( if the block will have ended then, don't change speed ) | |
148 | // * Find what position this is for us | |
149 | // * Find what speed we must go at to be at that position for the next acceleration tick | |
150 | // TODO : This works, but PLEASE PLEASE PLEASE if you know a better way to do it, do it better, I don't find this elegant at all, it's just the best I could think of | |
b6c86164 | 151 | // UPDATE: Yes, this sucks, I have ideas on how to do it better. If this is really bugging you, open a ticket and I'll make it a priority |
ded56b35 AW |
152 | |
153 | int ticks_forward = 3; | |
154 | // We need to take those values here, and then use those instead of the live values, because using the live values inside the loop can break things ( infinite loops etc ... ) | |
155 | double next_stepper_rate = stepper->trapezoid_adjusted_rate; | |
156 | double step_events_completed = (double(double(stepper->step_events_completed)/double(1<<16))); | |
157 | double position = ( this->current_position - this->start_position ) * this->direction ; | |
158 | double length = fabs( this->start_position - this->target_position ); | |
159 | double last_ratio = -1; | |
160 | ||
161 | // Do the startegy above, but if it does not work, look a bit further and try again, and again ... | |
162 | while(1){ | |
163 | ||
164 | // Find the position where we should be at the next tick | |
165 | double next_ratio = double( step_events_completed + ( next_stepper_rate / 60 / ((double(stepper->acceleration_ticks_per_second)/ticks_forward)) ) ) / double( this->current_block->steps_event_count ); | |
166 | double next_relative_position = ( length * next_ratio ); | |
167 | ||
168 | // Advance | |
169 | // TODO: Proper advance configuration | |
d9ebc974 AW |
170 | double advance = double(next_stepper_rate) * ( 0.00001 * 0.15 ) * 0.4 ; |
171 | //double advance = 0; | |
ded56b35 AW |
172 | next_relative_position += ( advance ); |
173 | ||
174 | // TODO : all of those "if->return" is very hacky, we should do the math in a way where most of those don't happen, but that requires doing tons of drawing ... | |
175 | if( last_ratio == next_ratio ){ this->acceleration_lock = false; return; }else{ last_ratio = next_ratio; } | |
176 | if( next_ratio == 0 || next_ratio > 1 ){ this->acceleration_lock = false; return; } | |
177 | if( ticks_forward > 1000 ){ this->acceleration_lock = false; return; } // This is very ugly | |
178 | ||
179 | // Hack : We have not looked far enough, we compute how far ahead we must look to get a relevant value | |
180 | if( position > next_relative_position ){ | |
181 | double far_back = position - next_relative_position; | |
182 | double far_back_ratio = far_back / length; | |
183 | double move_duration = double( this->current_block->steps_event_count ) / ( double(next_stepper_rate) / 60 ) ; | |
184 | double ticks_in_a_move = round( stepper->acceleration_ticks_per_second * move_duration ); | |
185 | double ratio_per_tick = 1 / ticks_in_a_move; | |
186 | double ticks_to_equilibrium = ceil(far_back_ratio / ratio_per_tick) + 1; | |
187 | ticks_forward += ticks_to_equilibrium; | |
188 | // Because this is a loop, and we can be interrupted by the stepping interrupt, if that interrupt changes block, the new block may not be solo, and we may get trapped into an infinite loop | |
189 | if( this->mode != FOLLOW ){ this->acceleration_lock = false; return; } | |
190 | continue; | |
191 | } | |
192 | ||
193 | // Finally, compute the speed to get to that next position | |
194 | double next_absolute_position = this->start_position + ( this->direction * next_relative_position ); | |
195 | double steps_to_next_tick = ( next_relative_position - position ) * this->steps_per_millimeter; | |
196 | double speed_to_next_tick = steps_to_next_tick / ( 1 / double(double(this->kernel->stepper->acceleration_ticks_per_second) / ticks_forward) ); | |
197 | ||
198 | // Change stepping speed | |
199 | this->set_speed( speed_to_next_tick ); | |
200 | ||
201 | this->acceleration_lock = false; | |
13e4a3f9 | 202 | return; |
ecc402eb | 203 | } |
0eb11a06 | 204 | } |
1a2d88eb | 205 | |
ded56b35 AW |
206 | this->acceleration_lock = false; |
207 | } | |
1a2d88eb | 208 | |
ded56b35 AW |
209 | // Convenience function to set stepping speed |
210 | void Extruder::set_speed( int steps_per_second ){ | |
d9ebc974 AW |
211 | |
212 | if( steps_per_second < 10 ){ | |
213 | steps_per_second = 10; | |
214 | } | |
ded56b35 AW |
215 | |
216 | // TODO : Proper limit config value | |
217 | if( steps_per_second > (this->feed_rate*double(this->steps_per_millimeter))/60 ){ | |
218 | steps_per_second = (this->feed_rate*double(this->steps_per_millimeter))/60; | |
0eb11a06 | 219 | } |
1a2d88eb | 220 | |
3b1e82d2 | 221 | this->counter_increment = int(floor(double(1<<16)/double(this->kernel->stepper->base_stepping_frequency / steps_per_second))); |
d9ebc974 | 222 | |
4cff3ded AW |
223 | } |
224 | ||
225 | inline void Extruder::stepping_tick(){ | |
4cff3ded | 226 | |
3b1e82d2 AW |
227 | this->step_counter += this->counter_increment; |
228 | if( this->step_counter > 1<<16 ){ | |
229 | this->step_counter -= 1<<16; | |
4cff3ded | 230 | |
3b1e82d2 AW |
231 | // If we still have steps to do |
232 | // TODO: Step using the same timer as the robot, and count steps instead of absolute float position | |
233 | if( ( this->current_position < this->target_position && this->direction == 1 ) || ( this->current_position > this->target_position && this->direction == -1 ) ){ | |
234 | this->current_position += (double(double(1)/double(this->steps_per_millimeter)))*double(this->direction); | |
b6c86164 | 235 | this->dir_pin = ((this->direction > 0) ? 1 : 0); |
3b1e82d2 AW |
236 | this->step_pin = 1; |
237 | }else{ | |
238 | // Move finished | |
239 | if( this->mode == SOLO && this->current_block != NULL ){ | |
240 | // In follow mode, the robot takes and releases the block, in solo mode we do | |
241 | this->current_block->release(); | |
242 | } | |
243 | } | |
244 | } | |
245 | } | |
4cff3ded | 246 | |
3b1e82d2 AW |
247 | void Extruder::reset_step_pin(){ |
248 | this->step_pin = 0; | |
249 | } |