-/*
+/*
This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
+ You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
*/
#include "StepTicker.h"
#include "system_LPC17xx.h" // mbed.h lib
+#include <mri.h>
+
+// StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
+// It has a list of those, and calls their tick() functions at regular intervals
+// They then do Bresenham stuff themselves
StepTicker* global_step_ticker;
StepTicker::StepTicker(){
global_step_ticker = this;
- LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register
- LPC_TIM0->MR1 = 10000000;
- LPC_TIM0->MCR = 11; // Match on MR0, reset on MR0, match on MR1
- LPC_TIM0->TCR = 1; // Enable interrupt
+ LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register
+ LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1
+ LPC_TIM0->TCR = 0; // Disable interrupt
+
+ LPC_SC->PCONP |= (1 << 2); // Power Ticker ON
+ LPC_TIM1->MR0 = 1000000;
+ LPC_TIM1->MCR = 1;
+ LPC_TIM1->TCR = 1; // Enable interrupt
+
+ // Default start values
+ this->moves_finished = false;
+ this->reset_step_pins = false;
this->debug = 0;
this->has_axes = 0;
this->set_frequency(0.001);
this->set_reset_delay(100);
this->last_duration = 0;
- NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
+ for (int i = 0; i < 12; i++){
+ this->active_motors[i] = NULL;
+ }
+ this->active_motor_bm = 0;
+
+ NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
+ NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
}
+// Set the base stepping frequency
void StepTicker::set_frequency( double frequency ){
this->frequency = frequency;
- this->period = int(floor((SystemCoreClock/4)/frequency)); // SystemCoreClock/4 = Timer increments in a second
+ this->period = int(floor((SystemCoreClock/4)/frequency)); // SystemCoreClock/4 = Timer increments in a second
LPC_TIM0->MR0 = this->period;
if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
LPC_TIM0->TCR = 3; // Reset
}
}
+// Set the reset delay
void StepTicker::set_reset_delay( double seconds ){
- this->delay = int(floor(double(SystemCoreClock/4)*( seconds ))); // SystemCoreClock/4 = Timer increments in a second
+ this->delay = int(floor(double(SystemCoreClock/4)*( seconds ))); // SystemCoreClock/4 = Timer increments in a second
+ LPC_TIM1->MR0 = this->delay;
}
// Add a stepper motor object to our list of steppers we must take care of
StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
this->stepper_motors.push_back(stepper_motor);
- stepper_motor->step_ticker = this;
+ stepper_motor->step_ticker = this;
this->has_axes = true;
return stepper_motor;
}
-inline void StepTicker::tick(){
- for(unsigned int i=0; i < this->stepper_motors.size(); i++){
- this->stepper_motors[i]->tick();
+// Call tick() on each active motor
+inline void StepTicker::tick(){
+ _isr_context = true;
+ int i;
+ uint32_t bm = 1;
+ // We iterate over each active motor
+ for (i = 0; i < 12; i++, bm <<= 1){
+ if (this->active_motor_bm & bm){
+ this->active_motors[i]->tick();
+ }
+ }
+ _isr_context = false;
+}
+
+// Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
+// all tick()s are called before we do the move finishing
+void StepTicker::signal_moves_finished(){
+ _isr_context = true;
+
+ uint16_t bitmask = 1;
+ for ( uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
+ if (this->active_motor_bm & bitmask){
+ if(this->active_motors[motor]->is_move_finished){
+ this->active_motors[motor]->signal_move_finished();
+ if(this->active_motors[motor]->moving == false){
+ if (motor > 0){
+ motor--;
+ bitmask >>= 1;
+ }
+ }
+ }
+ }
}
+ this->moves_finished = false;
+
+ _isr_context = false;
}
+// Reset step pins on all active motors
inline void StepTicker::reset_tick(){
- for(unsigned int i=0; i < this->stepper_motors.size(); i++){
- this->stepper_motors[i]->step_pin->set(0);
+ LPC_GPIO3->FIOSET = 1<<25;
+ _isr_context = true;
+
+ int i;
+ uint32_t bm;
+ for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ {
+ if (this->active_motor_bm & bm)
+ this->active_motors[i]->step_pin->set(0);
}
+
+ _isr_context = false;
+ LPC_GPIO3->FIOCLR = 1<<25;
+}
+
+extern "C" void TIMER1_IRQHandler (void){
+ LPC_TIM1->IR |= 1 << 0;
+ global_step_ticker->reset_tick();
}
+
+//#pragma GCC push_options
+//#pragma GCC optimize ("O0")
+
+
+// The actual interrupt handler where we do all the work
extern "C" void TIMER0_IRQHandler (void){
- uint32_t start_time = LPC_TIM0->TC;
- global_step_ticker->debug++;
-
- if( !global_step_ticker->has_axes ){
- if((LPC_TIM0->IR >> 0) & 1){ LPC_TIM0->IR |= 1 << 0; }
- if((LPC_TIM0->IR >> 1) & 1){ LPC_TIM0->IR |= 1 << 1; }
- return;
- }
-
- LPC_TIM0->MR1 = 2000000;
- if((LPC_TIM0->IR >> 0) & 1){ // If interrupt register set for MR0
+ LPC_GPIO3->FIODIR |= 1<<25;
+ LPC_GPIO1->FIODIR |= 1<<22;
+ LPC_GPIO1->FIODIR |= 1<<23;
+ LPC_GPIO1->FIODIR |= 1<<30;
+ LPC_GPIO1->FIODIR |= 1<<31;
+ LPC_GPIO1->FIOSET = 1<<22;
+
+ // Reset interrupt register
+ LPC_TIM0->IR |= 1 << 0;
+
+ // Step pins
+ //global_step_ticker->tick();
+ _isr_context = true;
+ uint16_t bitmask = 1;
+ for (uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
+ if (global_step_ticker->active_motor_bm & bitmask){
+ global_step_ticker->active_motors[motor]->tick();
+ }
+ }
+ _isr_context = false;
+
+ // We may have set a pin on in this tick, now we start the timer to set it off
+ if( global_step_ticker->reset_step_pins ){
+ LPC_TIM1->TCR = 3;
+ LPC_TIM1->TCR = 1;
+ global_step_ticker->reset_step_pins = false;
+ }else{
+ // Nothing happened, nothing after this really matters
+ // TODO : This could be a problem when we use Actuators instead of StepperMotors, because this flag is specific to step generation
+ LPC_TIM0->MR0 = global_step_ticker->period;
+ LPC_GPIO1->FIOCLR = 1<<22;
+ return;
+ }
+
+ // If a move finished in this tick, we have to tell the actuator to act accordingly
+ if( global_step_ticker->moves_finished ){
+
// Do not get out of here before everything is nice and tidy
-
- //if( global_step_ticker->debug % 1000 == 1 ){ printf("start: tc: %u mr0: %u mr1: %u d: %u\r\n", LPC_TIM0->TC, LPC_TIM0->MR0, LPC_TIM0->MR1, global_step_ticker->debug); }
-
LPC_TIM0->MR0 = 2000000;
-
- LPC_TIM0->IR |= 1 << 0; // Reset it
- // Step pins
- uint32_t aa = LPC_TIM0->TC;
- global_step_ticker->tick();
- uint32_t bb = LPC_TIM0->TC;
-
- // Maybe we have spent enough time in this interrupt so that we have to reset the pins ourself
- if( LPC_TIM0->TC > global_step_ticker->delay ){
- global_step_ticker->reset_tick();
- }else{
- // Else we have to trigger this a tad later, using MR1
- LPC_TIM0->MR1 = global_step_ticker->delay;
- }
- uint32_t cc = LPC_TIM0->TC;
+ global_step_ticker->signal_moves_finished();
- // If we went over the duration an interrupt is supposed to last, we have a problem
+ // If we went over the duration an interrupt is supposed to last, we have a problem
// That can happen tipically when we change blocks, where more than usual computation is done
// This can be OK, if we take notice of it, which we do now
- if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO : remove the size condition
+ if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO: remove the size condition
+ LPC_GPIO1->FIOCLR = 1<<22;
uint32_t start_tc = LPC_TIM0->TC;
+ LPC_GPIO1->FIOSET = 1<<22;
// How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time )
uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
// Next step is now to reduce this to how many steps we can *actually* skip
uint32_t ticks_we_actually_can_skip = ticks_to_skip;
- for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = global_step_ticker->stepper_motors[i];
- if( stepper->moving ){ ticks_we_actually_can_skip = min( ticks_we_actually_can_skip, (uint32_t)((uint64_t)( (uint64_t)stepper->fx_ticks_per_step - (uint64_t)stepper->fx_counter ) >> 32) ); }
- }
- // If the number of ticks we can actually skip is smaller than the number we wanted to skip, there is something wrong in the settings
- if( ticks_we_actually_can_skip < ticks_to_skip ){ }
+ int i;
+ uint32_t bm;
+ for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ {
+ if (global_step_ticker->active_motor_bm & bm)
+ ticks_we_actually_can_skip =
+ min(ticks_we_actually_can_skip,
+ (uint32_t)((uint64_t)( (uint64_t)global_step_ticker->active_motors[i]->fx_ticks_per_step - (uint64_t)global_step_ticker->active_motors[i]->fx_counter ) >> 32)
+ );
+ }
// Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
- for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = global_step_ticker->stepper_motors[i];
- if( stepper->moving ){ stepper->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32); }
+ for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ {
+ if (global_step_ticker->active_motor_bm & bm)
+ global_step_ticker->active_motors[i]->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32);
}
- // When must we have our next MR0 ? ( +1 is here to account that we are actually a legit MR0 too, not only overtime )
- LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
+ // When must we have our next MR0 ? ( +1 is here to account that we are actually doing a legit MR0 match here too, not only overtime )
+ LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
// This is so that we know how long this computation takes, and we can take it into account next time
- global_step_ticker->last_duration = LPC_TIM0->TC - start_tc;
-
- if( global_step_ticker->debug % 50 == 1 || 0 ){
- printf("a\r\n");
-
- printf("tc:%5u ld:%5u prd:%5u tts:%5u twcas:%5u nmr0:%5u \r\n", start_tc, global_step_ticker->last_duration, global_step_ticker->period, ticks_to_skip, ticks_we_actually_can_skip, LPC_TIM0->MR0 );
- for(unsigned int j=0; j < global_step_ticker->stepper_motors.size(); j++){
- StepperMotor* stepper = global_step_ticker->stepper_motors[j];
- if( stepper->moving ){
- uint32_t dd = (uint32_t)((uint64_t)( (uint64_t)stepper->fx_ticks_per_step - (uint64_t)stepper->fx_counter ) >> 32) ;
- printf(" %u: %u\r\n", j, dd);
- }
- }
-
-
- }
-
+ int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
+ if( difference > 0 ){ global_step_ticker->last_duration = (uint32_t)difference; }
}else{
LPC_TIM0->MR0 = global_step_ticker->period;
}
- }else{
- // Else obviously it's MR1
- LPC_TIM0->IR |= 1 << 1; // Reset it
- // Reset pins
- global_step_ticker->reset_tick();
- }
-
- //printf("end: tc: %u mr0: %u mr1: %u d: %u\r\n", LPC_TIM0->TC, LPC_TIM0->MR0, LPC_TIM0->MR1, global_step_ticker->debug);
+ while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
+ LPC_TIM0->MR0 += global_step_ticker->period;
+ }
- if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
- LPC_TIM0->TCR = 3; // Reset
- LPC_TIM0->TCR = 1; // Reset
}
-
+ LPC_GPIO1->FIOCLR = 1<<22;
}
+//#pragma GCC pop_options
+
+// We make a list of steppers that want to be called so that we don't call them for nothing
+void StepTicker::add_motor_to_active_list(StepperMotor* motor)
+{
+ uint32_t bm;
+ int i;
+ for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ {
+ if (this->active_motors[i] == motor)
+ {
+ this->active_motor_bm |= bm;
+ // If we have no motor to work on, disable the whole interrupt
+ if( this->active_motor_bm == 0 ){
+ LPC_TIM0->TCR = 0; // Disable interrupt
+ }else{
+ LPC_TIM0->TCR = 1; // Enable interrupt
+ }
+ return;
+ }
+ if (this->active_motors[i] == NULL)
+ {
+ this->active_motors[i] = motor;
+ this->active_motor_bm |= bm;
+ // If we have no motor to work on, disable the whole interrupt
+ if( this->active_motor_bm == 0 ){
+ LPC_TIM0->TCR = 0; // Disable interrupt
+ }else{
+ LPC_TIM0->TCR = 1; // Enable interrupt
+ }
+ return;
+ }
+ }
+ return;
+}
+
+// Remove a stepper from the list of active motors
+void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
+{
+ uint32_t bm; int i;
+ for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
+ {
+ if (this->active_motors[i] == motor)
+ {
+ this->active_motor_bm &= ~bm;
+ // If we have no motor to work on, disable the whole interrupt
+ if( this->active_motor_bm == 0 ){
+ LPC_TIM0->TCR = 0; // Disable interrupt
+ }else{
+ LPC_TIM0->TCR = 1; // Enable interrupt
+ }
+ return;
+ }
+ }
+}
HCoop / clinton / Smoothieware.git / blobdiff