215
Junior Member level 1
I am currently having issue with my program on my Arduino.
main.ino looks like this:
.cpp
and .h file
The problem that I am for some reason not able to complete my ISR routine.
The ISR routine creates linear ramping pulse signal and pretty much a copy of this application note.
The ISR routine might seem long, but as only one state is executed during each state it doesn't seem to be state much again.
For each interrupt will the state machine progress further in its states, those being [ACCEL, RUN,DECEL and STOP]. But for some reason stalls the program around the state STOP, and doesn't continue from there.. I am not able to communicate with it over serial, as things seem to not be responding?
What is the problem here?
main.ino looks like this:
Code C - [expand] 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 #include "speed_profile.h" void setup() { // put your setup code here, to run once: output_pin_setup(); cli(); timer1_setup(); sei(); } void loop() { init_tipper_position(); compute_speed_profile(23400, 500, 500, 1000); }
.cpp
Code C - [expand] 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 #include "speed_profile.h" volatile speed_profile profile; ros::NodeHandle_<ArduinoHardware, 1, 2, 125, 125> nh; std_msgs::Int16 status_step_count; std_msgs::Int16 status_tipper_availability; ros::Publisher chatter_status("tipper_status", &status_step_count); ros::Publisher chatter_availabilty("tipper_availability", &status_tipper_availability); volatile bool global_state = false; int received_data = 0; void call_back_control( const std_msgs::Empty & msg) { global_state = true; // For HMI received_data = (100 *23400.0)/100.0; // Converts input to motor_steps. } ros::Subscriber<std_msgs::Empty> sub_control("tipper_control", &call_back_control ); void output_pin_setup() { pinMode(en_pin, OUTPUT); pinMode(dir_pin, OUTPUT); pinMode(step_pin, OUTPUT); pinMode(slot_sensor_pin,INPUT_PULLUP); nh.initNode(); nh.advertise(chatter_status); nh.advertise(chatter_availabilty); nh.subscribe(sub_control); profile.debug = 0; profile.current_step_position = 0; delay(10); nh.getHardware()->setBaud(57600); } void init_tipper_position() { while(!(PINB & (1 << 2))) { // Waits until sensor gets activated } } void timer1_setup() { // Tells what part of speed ramp we are in. profile.run_state = STOP; TCCR1A = 0; TCCR1B = 0; TCNT1 = 0; // reset the timer // Timer/Counter 1 in mode 4 CTC (Not running). TCCR1B = (1 << WGM12); // Timer/Counter 1 Output Compare A Match Interrupt enable. TIMSK1 = (1 << OCIE1A); } void computation_of_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed) { unsigned int steps_to_speed; // Steps required to achieve the the speed desired unsigned int acceleration_step_limit; // If desired speed is not achieved, will this variable contain step_limit to when to stop accelerating. // If moving only 1 step. if (motor_steps == 1) { // Move one step profile.accel_count = -1; // in DECEL state. profile.run_state = DECEL; // Just a short delay so main() can act on 'running'. profile.first_step_delay = 1000; OCR1A = 10; // Run Timer/Counter 1 with prescaler = 8. TCCR1B |= ((0 << CS12) | (1 << CS11) | (0 << CS10)); } else if (motor_steps != 0) { // Set max speed limit, by calc min_delay to use in timer. // min_delay = (alpha / tt)/ w profile.min_time_delay = A_T_x100 / motor_speed; // Set accelration by calc the first (c0) step delay . // first_step_delay = 1/tt * sqrt(2*alpha/accel) // first_step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000 profile.first_step_delay = (T1_FREQ_148 * sqrt(A_SQ / motor_accel)) / 100; // Find out after how many steps does the speed hit the max speed limit. // steps_to_speed = speed^2 / (2*alpha*accel) steps_to_speed = (long)motor_speed * motor_speed / (long)(((long)A_x20000 * motor_accel) / 100); // If we hit max speed limit before 0,5 step it will round to 0. // But in practice we need to move atleast 1 step to get any speed at all. if (steps_to_speed == 0) { steps_to_speed = 1; } // Find out after how many steps we must start deceleration. // n1 = (n1+n2)decel / (accel + decel) acceleration_step_limit = ((long)motor_steps * motor_decel) / (motor_accel + motor_decel); // We must accelrate at least 1 step before we can start deceleration. if (acceleration_step_limit == 0) { acceleration_step_limit = 1; } // Use the limit we hit first to calc decel. if (acceleration_step_limit <= steps_to_speed) { profile.decel_length = -(motor_steps - acceleration_step_limit); //--- } else { profile.decel_length = -((long)steps_to_speed * motor_accel) / motor_decel; //-- } // We must decelrate at least 1 step to stop. if (profile.decel_length == 0) { profile.decel_length = -1; } // Find step to start decleration. profile.decel_start = motor_steps + profile.decel_length; // If the maximum speed is so low that we dont need to go via accelration state. if (profile.first_step_delay <= profile.min_time_delay) { profile.first_step_delay = profile.min_time_delay; profile.run_state = RUN; } else { profile.run_state = ACCEL; } // Reset counter. profile.accel_count = 0; } } void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed) { while (global_state == false) { //do nothing status_step_count.data = profile.current_step_position; status_tipper_availability.data = 1; chatter_status.publish( &status_step_count); chatter_availabilty.publish(&status_tipper_availability); nh.spinOnce(); } digitalWrite(en_pin, HIGH); delay(1); signed int move_steps; if(received_data > profile.current_step_position) // Compares whether received percentage (converted to motor_steps) is greater or smaller than current_step_position. { profile.dir = CCW; move_steps = received_data - profile.current_step_position; digitalWrite(dir_pin, HIGH); } else { profile.dir = CW; move_steps = profile.current_step_position - received_data; digitalWrite(dir_pin, LOW); } delay(1); // received_data = percentage converted to a step number received from tipper_control topic computation_of_speed_profile(move_steps, motor_accel, motor_decel, motor_speed); // function should be called with the correct motor_steps value OCR1A = 10; // Set Timer/Counter to divide clock by 8 TCCR1B |= ((0 << CS12) | (1 << CS11) | (0 << CS10)); //unsigned int first_step_delay; // Period time for the next timer_delay, determines the acceleration rate. = -28499 //unsigned int decel_start; // Determines at which step the deceleration should begin. = 20836 //signed int decel_length; // Set the deceleration length = 0 //signed int min_time_delay; //minium time required time_delay for achieving the wanted speed. = 1 //signed int accel_count; // counter used when computing step_delay for acceleration/decelleration. = 0 while (global_state == true) { status_step_count.data = profile.current_step_position; status_tipper_availability.data = 0; chatter_availabilty.publish(&status_tipper_availability); chatter_status.publish( &status_step_count); nh.spinOnce(); } } ISR(TIMER1_COMPA_vect) { // Holds next delay period. unsigned int new_first_step_delay; // Remember the last step delay used when accelrating. static int last_accel_delay; // Counting steps when moving. static unsigned int step_count = 0; // Keep track of remainder from new_step-delay calculation to incrase accurancy static unsigned int rest = 0; OCR1A = profile.first_step_delay; switch (profile.run_state) { case STOP: step_count = 0; global_state = false; rest = 0; TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10)); // Stop the timer, No clock source break; case ACCEL: PORTB ^= _BV(PB3); // Toggles the step_pin step_count++; if (profile.dir == CCW) { profile.current_step_position++; } else { profile.current_step_position--; } profile.accel_count++; new_first_step_delay = profile.first_step_delay - (((2 * (long)profile.first_step_delay) + rest) / (4 * profile.accel_count + 1)); rest = ((2 * (long)profile.first_step_delay) + rest) % (4 * profile.accel_count + 1); // Chech if we should start decelration. if (step_count >= profile.decel_start) { profile.accel_count = profile.decel_length; profile.run_state = DECEL; } // Chech if we hitted max speed. else if (new_first_step_delay <= profile.min_time_delay) { last_accel_delay = new_first_step_delay; new_first_step_delay = profile.min_time_delay; rest = 0; profile.run_state = RUN; } break; case RUN: PORTB ^= _BV(PB3); // Toggles the step_pin step_count++; if (profile.dir == CCW) { profile.current_step_position++; } else { profile.current_step_position--; } new_first_step_delay = profile.min_time_delay; // Chech if we should start decelration. if (step_count >= profile.decel_start) { profile.accel_count = profile.decel_length; // Start decelration with same delay as accel ended with. new_first_step_delay = last_accel_delay; profile.run_state = DECEL; } break; case DECEL: PORTB ^= _BV(PB3); // Toggles the step_pin step_count++; if (profile.dir == CCW) { profile.current_step_position++; } else { profile.current_step_position--; } profile.accel_count++; new_first_step_delay = profile.first_step_delay - (((2 * (long)profile.first_step_delay) + rest) / (4 * profile.accel_count + 1)); rest = ((2 * (long)profile.first_step_delay) + rest) % (4 * profile.accel_count + 1); // Check if we at last step if (profile.accel_count >= 0) { PORTB &= ~_BV(PB3); profile.run_state = STOP; status_tipper_availability.data = 2; chatter_availabilty.publish(&status_tipper_availability); } break; } profile.first_step_delay = new_first_step_delay; }
and .h file
Code C - [expand] 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 /* * Contains the class concerning with calculating the proper speed profile * for accelerating and decelerating the stepper motor. * */ #ifndef speed_profile_h #define speed_profile_h #include <Arduino.h> #include <ros.h> #include <std_msgs/Int16.h> #include <std_msgs/Empty.h> // Timer/Counter 1 running on 3,686MHz / 8 = 460,75kHz (2,17uS). (T1-FREQ 460750) //#define T1_FREQ 460750 #define T1_FREQ 1382400 //! Number of (full)steps per round on stepper motor in use. #define FSPR 1600 // Maths constants. To simplify maths when calculating in compute_speed_profile(). #define ALPHA (2*3.14159/FSPR) // 2*pi/spr #define A_T_x100 ((long)(ALPHA*T1_FREQ*100)) // (ALPHA / T1_FREQ)*100 #define T1_FREQ_148 ((int)((T1_FREQ*0.676)/100)) // divided by 100 and scaled by 0.676 #define A_SQ (long)(ALPHA*2*10000000000) // ALPHA*2*10000000000 #define A_x20000 (int)(ALPHA*20000) // ALPHA*20000 // Speed ramp states #define STOP 0 #define ACCEL 1 #define DECEL 2 #define RUN 3 // Pin numbering #define en_pin 13 #define dir_pin 12 #define step_pin 11 #define slot_sensor_pin 10 // Motor direction #define CW 0 #define CCW 1 typedef struct { unsigned char run_state : 3; // Determining the state of the speed profile unsigned char dir: 1; // Determining the direction the motor has to move - Start being CCW unsigned int first_step_delay; // Period time for the next timer_delay, determines the acceleration rate. unsigned int decel_start; // Determines at which step the deceleration should begin. signed int decel_length; // Set the deceleration length signed int min_time_delay; //minium time required time_delay for achieving the wanted speed. signed int accel_count; // counter used when computing step_delay for acceleration/decelleration. volatile signed int current_step_position; // contains the current step_position unsigned int debug; }speed_profile; void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed); void computation_of_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed); void init_tipper_position(); void timer1_setup(void); void output_pin_setup(void); #endif
The problem that I am for some reason not able to complete my ISR routine.
The ISR routine creates linear ramping pulse signal and pretty much a copy of this application note.
The ISR routine might seem long, but as only one state is executed during each state it doesn't seem to be state much again.
For each interrupt will the state machine progress further in its states, those being [ACCEL, RUN,DECEL and STOP]. But for some reason stalls the program around the state STOP, and doesn't continue from there.. I am not able to communicate with it over serial, as things seem to not be responding?
What is the problem here?
Last edited: