00001 /*This file has been prepared for Doxygen automatic documentation generation.*/ 00024 #include <ioavr.h> 00025 #include "global.h" 00026 #include "sm_driver.h" 00027 #include "speed_cntr.h" 00028 #include "uart.h" 00029 00031 speedRampData srd; 00032 00046 void speed_cntr_Move(signed int step, unsigned int accel, unsigned int decel, unsigned int speed) 00047 { 00049 unsigned int max_s_lim; 00051 unsigned int accel_lim; 00052 00053 // Set direction from sign on step value. 00054 if(step < 0){ 00055 srd.dir = CCW; 00056 step = -step; 00057 } 00058 else{ 00059 srd.dir = CW; 00060 } 00061 00062 // If moving only 1 step. 00063 if(step == 1){ 00064 // Move one step... 00065 srd.accel_count = -1; 00066 // ...in DECEL state. 00067 srd.run_state = DECEL; 00068 // Just a short delay so main() can act on 'running'. 00069 srd.step_delay = 1000; 00070 status.running = TRUE; 00071 OCR1A = 10; 00072 // Run Timer/Counter 1 with prescaler = 8. 00073 TCCR1B |= ((0<<CS12)|(1<<CS11)|(0<<CS10)); 00074 } 00075 // Only move if number of steps to move is not zero. 00076 else if(step != 0){ 00077 // Refer to documentation for detailed information about these calculations. 00078 00079 // Set max speed limit, by calc min_delay to use in timer. 00080 // min_delay = (alpha / tt)/ w 00081 srd.min_delay = A_T_x100 / speed; 00082 00083 // Set accelration by calc the first (c0) step delay . 00084 // step_delay = 1/tt * sqrt(2*alpha/accel) 00085 // step_delay = ( tfreq*0.676/100 )*100 * sqrt( (2*alpha*10000000000) / (accel*100) )/10000 00086 srd.step_delay = (T1_FREQ_148 * sqrt(A_SQ / accel))/100; 00087 00088 // Find out after how many steps does the speed hit the max speed limit. 00089 // max_s_lim = speed^2 / (2*alpha*accel) 00090 max_s_lim = (long)speed*speed/(long)(((long)A_x20000*accel)/100); 00091 // If we hit max speed limit before 0,5 step it will round to 0. 00092 // But in practice we need to move atleast 1 step to get any speed at all. 00093 if(max_s_lim == 0){ 00094 max_s_lim = 1; 00095 } 00096 00097 // Find out after how many steps we must start deceleration. 00098 // n1 = (n1+n2)decel / (accel + decel) 00099 accel_lim = ((long)step*decel) / (accel+decel); 00100 // We must accelrate at least 1 step before we can start deceleration. 00101 if(accel_lim == 0){ 00102 accel_lim = 1; 00103 } 00104 00105 // Use the limit we hit first to calc decel. 00106 if(accel_lim <= max_s_lim){ 00107 srd.decel_val = accel_lim - step; 00108 } 00109 else{ 00110 srd.decel_val = -((long)max_s_lim*accel)/decel; 00111 } 00112 // We must decelrate at least 1 step to stop. 00113 if(srd.decel_val == 0){ 00114 srd.decel_val = -1; 00115 } 00116 00117 // Find step to start decleration. 00118 srd.decel_start = step + srd.decel_val; 00119 00120 // If the maximum speed is so low that we dont need to go via accelration state. 00121 if(srd.step_delay <= srd.min_delay){ 00122 srd.step_delay = srd.min_delay; 00123 srd.run_state = RUN; 00124 } 00125 else{ 00126 srd.run_state = ACCEL; 00127 } 00128 00129 // Reset counter. 00130 srd.accel_count = 0; 00131 status.running = TRUE; 00132 OCR1A = 10; 00133 // Set Timer/Counter to divide clock by 8 00134 TCCR1B |= ((0<<CS12)|(1<<CS11)|(0<<CS10)); 00135 } 00136 } 00137 00143 void speed_cntr_Init_Timer1(void) 00144 { 00145 // Tells what part of speed ramp we are in. 00146 srd.run_state = STOP; 00147 // Timer/Counter 1 in mode 4 CTC (Not running). 00148 TCCR1B = (1<<WGM12); 00149 // Timer/Counter 1 Output Compare A Match Interrupt enable. 00150 TIMSK1 = (1<<OCIE1A); 00151 } 00152 00163 #pragma vector=TIMER1_COMPA_vect 00164 __interrupt void speed_cntr_TIMER1_COMPA_interrupt( void ) 00165 { 00166 // Holds next delay period. 00167 unsigned int new_step_delay; 00168 // Remember the last step delay used when accelrating. 00169 static int last_accel_delay; 00170 // Counting steps when moving. 00171 static unsigned int step_count = 0; 00172 // Keep track of remainder from new_step-delay calculation to incrase accurancy 00173 static unsigned int rest = 0; 00174 00175 OCR1A = srd.step_delay; 00176 00177 switch(srd.run_state) { 00178 case STOP: 00179 step_count = 0; 00180 rest = 0; 00181 // Stop Timer/Counter 1. 00182 TCCR1B &= ~((1<<CS12)|(1<<CS11)|(1<<CS10)); 00183 status.running = FALSE; 00184 break; 00185 00186 case ACCEL: 00187 sm_driver_StepCounter(srd.dir); 00188 step_count++; 00189 srd.accel_count++; 00190 new_step_delay = srd.step_delay - (((2 * (long)srd.step_delay) + rest)/(4 * srd.accel_count + 1)); 00191 rest = ((2 * (long)srd.step_delay)+rest)%(4 * srd.accel_count + 1); 00192 // Chech if we should start decelration. 00193 if(step_count >= srd.decel_start) { 00194 srd.accel_count = srd.decel_val; 00195 srd.run_state = DECEL; 00196 } 00197 // Chech if we hitted max speed. 00198 else if(new_step_delay <= srd.min_delay) { 00199 last_accel_delay = new_step_delay; 00200 new_step_delay = srd.min_delay; 00201 rest = 0; 00202 srd.run_state = RUN; 00203 } 00204 break; 00205 00206 case RUN: 00207 sm_driver_StepCounter(srd.dir); 00208 step_count++; 00209 new_step_delay = srd.min_delay; 00210 // Chech if we should start decelration. 00211 if(step_count >= srd.decel_start) { 00212 srd.accel_count = srd.decel_val; 00213 // Start decelration with same delay as accel ended with. 00214 new_step_delay = last_accel_delay; 00215 srd.run_state = DECEL; 00216 } 00217 break; 00218 00219 case DECEL: 00220 sm_driver_StepCounter(srd.dir); 00221 step_count++; 00222 srd.accel_count++; 00223 new_step_delay = srd.step_delay - (((2 * (long)srd.step_delay) + rest)/(4 * srd.accel_count + 1)); 00224 rest = ((2 * (long)srd.step_delay)+rest)%(4 * srd.accel_count + 1); 00225 // Check if we at last step 00226 if(srd.accel_count >= 0){ 00227 srd.run_state = STOP; 00228 } 00229 break; 00230 } 00231 srd.step_delay = new_step_delay; 00232 } 00233 00244 static unsigned long sqrt(unsigned long x) 00245 { 00246 register unsigned long xr; // result register 00247 register unsigned long q2; // scan-bit register 00248 register unsigned char f; // flag (one bit) 00249 00250 xr = 0; // clear result 00251 q2 = 0x40000000L; // higest possible result bit 00252 do 00253 { 00254 if((xr + q2) <= x) 00255 { 00256 x -= xr + q2; 00257 f = 1; // set flag 00258 } 00259 else{ 00260 f = 0; // clear flag 00261 } 00262 xr >>= 1; 00263 if(f){ 00264 xr += q2; // test flag 00265 } 00266 } while(q2 >>= 2); // shift twice 00267 if(xr < x){ 00268 return xr +1; // add for rounding 00269 } 00270 else{ 00271 return xr; 00272 } 00273 } 00274 00281 unsigned int min(unsigned int x, unsigned int y) 00282 { 00283 if(x < y){ 00284 return x; 00285 } 00286 else{ 00287 return y; 00288 } 00289 }