Michael Spencer / Smoothie

Fork of Smoothie to port to mbed non-LPC targets.

Dependencies:   mbed

Fork of Smoothie by Stéphane Cachat

libs/StepTicker.cpp@3:f151d08d335c, 2014-03-02 (annotated)

Committer:
Bigcheese
Date:
Sun Mar 02 06:33:08 2014 +0000
Revision:
3:f151d08d335c
Parent:
2:1df0b61d3b5a 
Bunch of stuff. Need to locally merge in updated USB changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Michael J. Spencer 2:1df0b61d3b5a 1 /*
Michael J. Spencer 2:1df0b61d3b5a 2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
Michael J. Spencer 2:1df0b61d3b5a 3 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.
Michael J. Spencer 2:1df0b61d3b5a 4 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.
Michael J. Spencer 2:1df0b61d3b5a 5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
Michael J. Spencer 2:1df0b61d3b5a 6 */
Michael J. Spencer 2:1df0b61d3b5a 7
Michael J. Spencer 2:1df0b61d3b5a 8
Michael J. Spencer 2:1df0b61d3b5a 9 #include "StepTicker.h"
Michael J. Spencer 2:1df0b61d3b5a 10
Michael J. Spencer 2:1df0b61d3b5a 11 using namespace std;
Michael J. Spencer 2:1df0b61d3b5a 12 #include <vector>
Michael J. Spencer 2:1df0b61d3b5a 13
Michael J. Spencer 2:1df0b61d3b5a 14 #include "libs/nuts_bolts.h"
Michael J. Spencer 2:1df0b61d3b5a 15 #include "libs/Module.h"
Michael J. Spencer 2:1df0b61d3b5a 16 #include "libs/Kernel.h"
Michael J. Spencer 2:1df0b61d3b5a 17 #include "StepperMotor.h"
Michael J. Spencer 2:1df0b61d3b5a 18
Michael J. Spencer 2:1df0b61d3b5a 19 #include "system_LPC17xx.h" // mbed.h lib
Michael J. Spencer 2:1df0b61d3b5a 20
Michael J. Spencer 2:1df0b61d3b5a 21 #include <mri.h>
Michael J. Spencer 2:1df0b61d3b5a 22
Michael J. Spencer 2:1df0b61d3b5a 23 // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
Michael J. Spencer 2:1df0b61d3b5a 24 // It has a list of those, and calls their tick() functions at regular intervals
Michael J. Spencer 2:1df0b61d3b5a 25 // They then do Bresenham stuff themselves
Michael J. Spencer 2:1df0b61d3b5a 26
Michael J. Spencer 2:1df0b61d3b5a 27 StepTicker* global_step_ticker;
Michael J. Spencer 2:1df0b61d3b5a 28
Michael J. Spencer 2:1df0b61d3b5a 29 StepTicker::StepTicker(){
Michael J. Spencer 2:1df0b61d3b5a 30 global_step_ticker = this;
Michael J. Spencer 2:1df0b61d3b5a 31
Michael J. Spencer 2:1df0b61d3b5a 32 // Configure the timer
Michael J. Spencer 2:1df0b61d3b5a 33 LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register
Michael J. Spencer 2:1df0b61d3b5a 34 LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1
Michael J. Spencer 2:1df0b61d3b5a 35 LPC_TIM0->TCR = 0; // Disable interrupt
Michael J. Spencer 2:1df0b61d3b5a 36
Michael J. Spencer 2:1df0b61d3b5a 37 LPC_SC->PCONP |= (1 << 2); // Power Ticker ON
Michael J. Spencer 2:1df0b61d3b5a 38 LPC_TIM1->MR0 = 1000000;
Michael J. Spencer 2:1df0b61d3b5a 39 LPC_TIM1->MCR = 1;
Michael J. Spencer 2:1df0b61d3b5a 40 LPC_TIM1->TCR = 1; // Enable interrupt
Michael J. Spencer 2:1df0b61d3b5a 41
Michael J. Spencer 2:1df0b61d3b5a 42 // Default start values
Michael J. Spencer 2:1df0b61d3b5a 43 this->moves_finished = false;
Michael J. Spencer 2:1df0b61d3b5a 44 this->reset_step_pins = false;
Michael J. Spencer 2:1df0b61d3b5a 45 this->debug = 0;
Michael J. Spencer 2:1df0b61d3b5a 46 this->has_axes = 0;
Michael J. Spencer 2:1df0b61d3b5a 47 this->set_frequency(0.001);
Michael J. Spencer 2:1df0b61d3b5a 48 this->set_reset_delay(100);
Michael J. Spencer 2:1df0b61d3b5a 49 this->last_duration = 0;
Michael J. Spencer 2:1df0b61d3b5a 50 for (int i = 0; i < 12; i++){
Michael J. Spencer 2:1df0b61d3b5a 51 this->active_motors[i] = NULL;
Michael J. Spencer 2:1df0b61d3b5a 52 }
Michael J. Spencer 2:1df0b61d3b5a 53 this->active_motor_bm = 0;
Michael J. Spencer 2:1df0b61d3b5a 54
Michael J. Spencer 2:1df0b61d3b5a 55 NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
Michael J. Spencer 2:1df0b61d3b5a 56 NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
Michael J. Spencer 2:1df0b61d3b5a 57 }
Michael J. Spencer 2:1df0b61d3b5a 58
Michael J. Spencer 2:1df0b61d3b5a 59 // Set the base stepping frequency
Michael J. Spencer 2:1df0b61d3b5a 60 void StepTicker::set_frequency( float frequency ){
Michael J. Spencer 2:1df0b61d3b5a 61 this->frequency = frequency;
Michael J. Spencer 2:1df0b61d3b5a 62 this->period = int(floor((SystemCoreClock/4)/frequency)); // SystemCoreClock/4 = Timer increments in a second
Michael J. Spencer 2:1df0b61d3b5a 63 LPC_TIM0->MR0 = this->period;
Michael J. Spencer 2:1df0b61d3b5a 64 if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
Michael J. Spencer 2:1df0b61d3b5a 65 LPC_TIM0->TCR = 3; // Reset
Michael J. Spencer 2:1df0b61d3b5a 66 LPC_TIM0->TCR = 1; // Reset
Michael J. Spencer 2:1df0b61d3b5a 67 }
Michael J. Spencer 2:1df0b61d3b5a 68 }
Michael J. Spencer 2:1df0b61d3b5a 69
Michael J. Spencer 2:1df0b61d3b5a 70 // Set the reset delay
Michael J. Spencer 2:1df0b61d3b5a 71 void StepTicker::set_reset_delay( float seconds ){
Michael J. Spencer 2:1df0b61d3b5a 72 this->delay = int(floor(float(SystemCoreClock/4)*( seconds ))); // SystemCoreClock/4 = Timer increments in a second
Michael J. Spencer 2:1df0b61d3b5a 73 LPC_TIM1->MR0 = this->delay;
Michael J. Spencer 2:1df0b61d3b5a 74 }
Michael J. Spencer 2:1df0b61d3b5a 75
Michael J. Spencer 2:1df0b61d3b5a 76 // Add a stepper motor object to our list of steppers we must take care of
Michael J. Spencer 2:1df0b61d3b5a 77 StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
Michael J. Spencer 2:1df0b61d3b5a 78 this->stepper_motors.push_back(stepper_motor);
Michael J. Spencer 2:1df0b61d3b5a 79 stepper_motor->step_ticker = this;
Michael J. Spencer 2:1df0b61d3b5a 80 this->has_axes = true;
Michael J. Spencer 2:1df0b61d3b5a 81 return stepper_motor;
Michael J. Spencer 2:1df0b61d3b5a 82 }
Michael J. Spencer 2:1df0b61d3b5a 83
Michael J. Spencer 2:1df0b61d3b5a 84 // Call tick() on each active motor
Michael J. Spencer 2:1df0b61d3b5a 85 inline void StepTicker::tick(){
Michael J. Spencer 2:1df0b61d3b5a 86 _isr_context = true;
Michael J. Spencer 2:1df0b61d3b5a 87 int i;
Michael J. Spencer 2:1df0b61d3b5a 88 uint32_t bm = 1;
Michael J. Spencer 2:1df0b61d3b5a 89 // We iterate over each active motor
Michael J. Spencer 2:1df0b61d3b5a 90 for (i = 0; i < 12; i++, bm <<= 1){
Michael J. Spencer 2:1df0b61d3b5a 91 if (this->active_motor_bm & bm){
Michael J. Spencer 2:1df0b61d3b5a 92 this->active_motors[i]->tick();
Michael J. Spencer 2:1df0b61d3b5a 93 }
Michael J. Spencer 2:1df0b61d3b5a 94 }
Michael J. Spencer 2:1df0b61d3b5a 95 _isr_context = false;
Michael J. Spencer 2:1df0b61d3b5a 96 }
Michael J. Spencer 2:1df0b61d3b5a 97
Michael J. Spencer 2:1df0b61d3b5a 98 // Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
Michael J. Spencer 2:1df0b61d3b5a 99 // all tick()s are called before we do the move finishing
Michael J. Spencer 2:1df0b61d3b5a 100 void StepTicker::signal_moves_finished(){
Michael J. Spencer 2:1df0b61d3b5a 101 _isr_context = true;
Michael J. Spencer 2:1df0b61d3b5a 102
Michael J. Spencer 2:1df0b61d3b5a 103 uint16_t bitmask = 1;
Michael J. Spencer 2:1df0b61d3b5a 104 for ( uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
Michael J. Spencer 2:1df0b61d3b5a 105 if (this->active_motor_bm & bitmask){
Michael J. Spencer 2:1df0b61d3b5a 106 if(this->active_motors[motor]->is_move_finished){
Michael J. Spencer 2:1df0b61d3b5a 107 this->active_motors[motor]->signal_move_finished();
Michael J. Spencer 2:1df0b61d3b5a 108 if(this->active_motors[motor]->moving == false){
Michael J. Spencer 2:1df0b61d3b5a 109 if (motor > 0){
Michael J. Spencer 2:1df0b61d3b5a 110 motor--;
Michael J. Spencer 2:1df0b61d3b5a 111 bitmask >>= 1;
Michael J. Spencer 2:1df0b61d3b5a 112 }
Michael J. Spencer 2:1df0b61d3b5a 113 }
Michael J. Spencer 2:1df0b61d3b5a 114 }
Michael J. Spencer 2:1df0b61d3b5a 115 }
Michael J. Spencer 2:1df0b61d3b5a 116 }
Michael J. Spencer 2:1df0b61d3b5a 117 this->moves_finished = false;
Michael J. Spencer 2:1df0b61d3b5a 118
Michael J. Spencer 2:1df0b61d3b5a 119 _isr_context = false;
Michael J. Spencer 2:1df0b61d3b5a 120 }
Michael J. Spencer 2:1df0b61d3b5a 121
Michael J. Spencer 2:1df0b61d3b5a 122 // Reset step pins on all active motors
Michael J. Spencer 2:1df0b61d3b5a 123 inline void StepTicker::reset_tick(){
Michael J. Spencer 2:1df0b61d3b5a 124 _isr_context = true;
Michael J. Spencer 2:1df0b61d3b5a 125
Michael J. Spencer 2:1df0b61d3b5a 126 int i;
Michael J. Spencer 2:1df0b61d3b5a 127 uint32_t bm;
Michael J. Spencer 2:1df0b61d3b5a 128 for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
Michael J. Spencer 2:1df0b61d3b5a 129 {
Michael J. Spencer 2:1df0b61d3b5a 130 if (this->active_motor_bm & bm)
Michael J. Spencer 2:1df0b61d3b5a 131 this->active_motors[i]->unstep();
Michael J. Spencer 2:1df0b61d3b5a 132 }
Michael J. Spencer 2:1df0b61d3b5a 133
Michael J. Spencer 2:1df0b61d3b5a 134 _isr_context = false;
Michael J. Spencer 2:1df0b61d3b5a 135 }
Michael J. Spencer 2:1df0b61d3b5a 136
Michael J. Spencer 2:1df0b61d3b5a 137 extern "C" void TIMER1_IRQHandler (void){
Michael J. Spencer 2:1df0b61d3b5a 138 LPC_TIM1->IR |= 1 << 0;
Michael J. Spencer 2:1df0b61d3b5a 139 global_step_ticker->reset_tick();
Michael J. Spencer 2:1df0b61d3b5a 140 }
Michael J. Spencer 2:1df0b61d3b5a 141
Michael J. Spencer 2:1df0b61d3b5a 142 // The actual interrupt handler where we do all the work
Michael J. Spencer 2:1df0b61d3b5a 143 extern "C" void TIMER0_IRQHandler (void){
Michael J. Spencer 2:1df0b61d3b5a 144
Michael J. Spencer 2:1df0b61d3b5a 145 // Reset interrupt register
Michael J. Spencer 2:1df0b61d3b5a 146 LPC_TIM0->IR |= 1 << 0;
Michael J. Spencer 2:1df0b61d3b5a 147
Michael J. Spencer 2:1df0b61d3b5a 148 // Step pins
Michael J. Spencer 2:1df0b61d3b5a 149 uint16_t bitmask = 1;
Michael J. Spencer 2:1df0b61d3b5a 150 for (uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
Michael J. Spencer 2:1df0b61d3b5a 151 if (global_step_ticker->active_motor_bm & bitmask){
Michael J. Spencer 2:1df0b61d3b5a 152 global_step_ticker->active_motors[motor]->tick();
Michael J. Spencer 2:1df0b61d3b5a 153 }
Michael J. Spencer 2:1df0b61d3b5a 154 }
Michael J. Spencer 2:1df0b61d3b5a 155
Michael J. Spencer 2:1df0b61d3b5a 156 // We may have set a pin on in this tick, now we start the timer to set it off
Michael J. Spencer 2:1df0b61d3b5a 157 if( global_step_ticker->reset_step_pins ){
Michael J. Spencer 2:1df0b61d3b5a 158 LPC_TIM1->TCR = 3;
Michael J. Spencer 2:1df0b61d3b5a 159 LPC_TIM1->TCR = 1;
Michael J. Spencer 2:1df0b61d3b5a 160 global_step_ticker->reset_step_pins = false;
Michael J. Spencer 2:1df0b61d3b5a 161 }else{
Michael J. Spencer 2:1df0b61d3b5a 162 // Nothing happened, nothing after this really matters
Michael J. Spencer 2:1df0b61d3b5a 163 // TODO : This could be a problem when we use Actuators instead of StepperMotors, because this flag is specific to step generation
Michael J. Spencer 2:1df0b61d3b5a 164 LPC_TIM0->MR0 = global_step_ticker->period;
Michael J. Spencer 2:1df0b61d3b5a 165 return;
Michael J. Spencer 2:1df0b61d3b5a 166 }
Michael J. Spencer 2:1df0b61d3b5a 167
Michael J. Spencer 2:1df0b61d3b5a 168 // If a move finished in this tick, we have to tell the actuator to act accordingly
Michael J. Spencer 2:1df0b61d3b5a 169 if( global_step_ticker->moves_finished ){
Michael J. Spencer 2:1df0b61d3b5a 170
Michael J. Spencer 2:1df0b61d3b5a 171 // Do not get out of here before everything is nice and tidy
Michael J. Spencer 2:1df0b61d3b5a 172 LPC_TIM0->MR0 = 20000000;
Michael J. Spencer 2:1df0b61d3b5a 173
Michael J. Spencer 2:1df0b61d3b5a 174 global_step_ticker->signal_moves_finished();
Michael J. Spencer 2:1df0b61d3b5a 175
Michael J. Spencer 2:1df0b61d3b5a 176 // If we went over the duration an interrupt is supposed to last, we have a problem
Michael J. Spencer 2:1df0b61d3b5a 177 // That can happen tipically when we change blocks, where more than usual computation is done
Michael J. Spencer 2:1df0b61d3b5a 178 // This can be OK, if we take notice of it, which we do now
Michael J. Spencer 2:1df0b61d3b5a 179 if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO: remove the size condition
Michael J. Spencer 2:1df0b61d3b5a 180
Michael J. Spencer 2:1df0b61d3b5a 181 uint32_t start_tc = LPC_TIM0->TC;
Michael J. Spencer 2:1df0b61d3b5a 182
Michael J. Spencer 2:1df0b61d3b5a 183 // 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 )
Michael J. Spencer 2:1df0b61d3b5a 184 uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
Michael J. Spencer 2:1df0b61d3b5a 185
Michael J. Spencer 2:1df0b61d3b5a 186 // Next step is now to reduce this to how many steps we can *actually* skip
Michael J. Spencer 2:1df0b61d3b5a 187 uint32_t ticks_we_actually_can_skip = ticks_to_skip;
Michael J. Spencer 2:1df0b61d3b5a 188
Michael J. Spencer 2:1df0b61d3b5a 189 int i;
Michael J. Spencer 2:1df0b61d3b5a 190 uint32_t bm;
Michael J. Spencer 2:1df0b61d3b5a 191 for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
Michael J. Spencer 2:1df0b61d3b5a 192 {
Michael J. Spencer 2:1df0b61d3b5a 193 if (global_step_ticker->active_motor_bm & bm)
Michael J. Spencer 2:1df0b61d3b5a 194 ticks_we_actually_can_skip =
Michael J. Spencer 2:1df0b61d3b5a 195 min(ticks_we_actually_can_skip,
Michael J. Spencer 2:1df0b61d3b5a 196 (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)
Michael J. Spencer 2:1df0b61d3b5a 197 );
Michael J. Spencer 2:1df0b61d3b5a 198 }
Michael J. Spencer 2:1df0b61d3b5a 199
Michael J. Spencer 2:1df0b61d3b5a 200 // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
Michael J. Spencer 2:1df0b61d3b5a 201 for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
Michael J. Spencer 2:1df0b61d3b5a 202 {
Michael J. Spencer 2:1df0b61d3b5a 203 if (global_step_ticker->active_motor_bm & bm)
Michael J. Spencer 2:1df0b61d3b5a 204 global_step_ticker->active_motors[i]->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32);
Michael J. Spencer 2:1df0b61d3b5a 205 }
Michael J. Spencer 2:1df0b61d3b5a 206
Michael J. Spencer 2:1df0b61d3b5a 207 // 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 )
Michael J. Spencer 2:1df0b61d3b5a 208 LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
Michael J. Spencer 2:1df0b61d3b5a 209
Michael J. Spencer 2:1df0b61d3b5a 210 // This is so that we know how long this computation takes, and we can take it into account next time
Michael J. Spencer 2:1df0b61d3b5a 211 int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
Michael J. Spencer 2:1df0b61d3b5a 212 if( difference > 0 ){ global_step_ticker->last_duration = (uint32_t)difference; }
Michael J. Spencer 2:1df0b61d3b5a 213
Michael J. Spencer 2:1df0b61d3b5a 214 }else{
Michael J. Spencer 2:1df0b61d3b5a 215 LPC_TIM0->MR0 = global_step_ticker->period;
Michael J. Spencer 2:1df0b61d3b5a 216 }
Michael J. Spencer 2:1df0b61d3b5a 217
Michael J. Spencer 2:1df0b61d3b5a 218 while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
Michael J. Spencer 2:1df0b61d3b5a 219 LPC_TIM0->MR0 += global_step_ticker->period;
Michael J. Spencer 2:1df0b61d3b5a 220 }
Michael J. Spencer 2:1df0b61d3b5a 221
Michael J. Spencer 2:1df0b61d3b5a 222 }
Michael J. Spencer 2:1df0b61d3b5a 223
Michael J. Spencer 2:1df0b61d3b5a 224 }
Michael J. Spencer 2:1df0b61d3b5a 225
Michael J. Spencer 2:1df0b61d3b5a 226
Michael J. Spencer 2:1df0b61d3b5a 227 // We make a list of steppers that want to be called so that we don't call them for nothing
Michael J. Spencer 2:1df0b61d3b5a 228 void StepTicker::add_motor_to_active_list(StepperMotor* motor)
Michael J. Spencer 2:1df0b61d3b5a 229 {
Michael J. Spencer 2:1df0b61d3b5a 230 uint32_t bm;
Michael J. Spencer 2:1df0b61d3b5a 231 int i;
Michael J. Spencer 2:1df0b61d3b5a 232 for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
Michael J. Spencer 2:1df0b61d3b5a 233 {
Michael J. Spencer 2:1df0b61d3b5a 234 if (this->active_motors[i] == motor)
Michael J. Spencer 2:1df0b61d3b5a 235 {
Michael J. Spencer 2:1df0b61d3b5a 236 this->active_motor_bm |= bm;
Michael J. Spencer 2:1df0b61d3b5a 237 if( this->active_motor_bm != 0 ){
Michael J. Spencer 2:1df0b61d3b5a 238 LPC_TIM0->TCR = 1; // Enable interrupt
Michael J. Spencer 2:1df0b61d3b5a 239 }
Michael J. Spencer 2:1df0b61d3b5a 240 return;
Michael J. Spencer 2:1df0b61d3b5a 241 }
Michael J. Spencer 2:1df0b61d3b5a 242 if (this->active_motors[i] == NULL)
Michael J. Spencer 2:1df0b61d3b5a 243 {
Michael J. Spencer 2:1df0b61d3b5a 244 this->active_motors[i] = motor;
Michael J. Spencer 2:1df0b61d3b5a 245 this->active_motor_bm |= bm;
Michael J. Spencer 2:1df0b61d3b5a 246 if( this->active_motor_bm != 0 ){
Michael J. Spencer 2:1df0b61d3b5a 247 LPC_TIM0->TCR = 1; // Enable interrupt
Michael J. Spencer 2:1df0b61d3b5a 248 }
Michael J. Spencer 2:1df0b61d3b5a 249 return;
Michael J. Spencer 2:1df0b61d3b5a 250 }
Michael J. Spencer 2:1df0b61d3b5a 251 }
Michael J. Spencer 2:1df0b61d3b5a 252 return;
Michael J. Spencer 2:1df0b61d3b5a 253 }
Michael J. Spencer 2:1df0b61d3b5a 254
Michael J. Spencer 2:1df0b61d3b5a 255 // Remove a stepper from the list of active motors
Michael J. Spencer 2:1df0b61d3b5a 256 void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
Michael J. Spencer 2:1df0b61d3b5a 257 {
Michael J. Spencer 2:1df0b61d3b5a 258 uint32_t bm; int i;
Michael J. Spencer 2:1df0b61d3b5a 259 for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
Michael J. Spencer 2:1df0b61d3b5a 260 {
Michael J. Spencer 2:1df0b61d3b5a 261 if (this->active_motors[i] == motor)
Michael J. Spencer 2:1df0b61d3b5a 262 {
Michael J. Spencer 2:1df0b61d3b5a 263 this->active_motor_bm &= ~bm;
Michael J. Spencer 2:1df0b61d3b5a 264 // If we have no motor to work on, disable the whole interrupt
Michael J. Spencer 2:1df0b61d3b5a 265 if( this->active_motor_bm == 0 ){
Michael J. Spencer 2:1df0b61d3b5a 266 LPC_TIM0->TCR = 0; // Disable interrupt
Michael J. Spencer 2:1df0b61d3b5a 267 }
Michael J. Spencer 2:1df0b61d3b5a 268 return;
Michael J. Spencer 2:1df0b61d3b5a 269 }
Michael J. Spencer 2:1df0b61d3b5a 270 }
Michael J. Spencer 2:1df0b61d3b5a 271 }