1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063 |
- #include "servostep.h"
- #include "global.h"
- StepperDataInfo tXAxisStepper @".ccram";
- StepperDataInfo tYAxisStepper @".ccram";
- word_bits_t cAxisDriveState @".ccram";
- word_bits_t cAxisAlarmState @".ccram";
- static StepperDataInfo *StepInfo[] = {
- &tXAxisStepper,
- &tYAxisStepper
- };
- #define X_FreqTab_Number 150
- #define Y_FreqTab_Number 150
- #define DATA_SIZE 100
- const unsigned long FreqTab_XX[] = {
- 2000, 4000, 6774, 9440, 15998, 19998, 24002, 27997, 32009, 35994,
- 39997, 44016, 47979, 51982, 55995, 60022, 64018, 67952, 72046, 76008,
- 79994, 83940, 87945, 91968, 95958, 100083, 104086, 107894, 111991, 116107,
- 119882, 123912, 127852, 132050, 136113, 139989, 144093, 147949, 152016, 155763,
- 160278, 163840, 168200, 172127, 176242, 179824, 184320, 188241, 192333, 195738,
- 200166, 203856, 207684, 211659, 215789, 220083, 224552, 228024, 231606, 236560,
- 240417, 244402, 248521, 251345, 255704, 260216, 263314, 268101, 271391, 276480,
- 279979, 283569, 287251, 292958, 296891, 300930, 302991, 307200, 311526, 315977,
- 320556, 322896, 327680, 332607, 335127, 340283, 342920, 348321, 351085, 356748,
- 359648, 362596, 368640, 371737, 374888, 381351, 384667, 388042, 391476, 394971,
- 398529, 402152, 405842, 409600, 413428, 417328, 421302, 425353, 429483, 433694,
- 437988, 442368,
- };
- unsigned short FreqTab_X[X_FreqTab_Number]@".ccram";
- static unsigned short FreqTab_X2[X_FreqTab_Number]@".ccram";
- const unsigned long FreqTab_YY[] = {
- 2000, 4000, 9999, 11668, 13332, 15000, 16667, 18332, 19998, 21663,
- 23331, 25006, 26664, 28338, 29991, 31665, 33335, 34997, 36680, 38333,
- 39997, 41654, 43326, 45001, 46663, 48346, 49985, 51678, 53361, 55020,
- 56641, 58359, 60022, 61697, 63376, 64958, 66621, 67331, 67952, 68690,
- 69336, 69994, 70665, 71349, 72046, 72638, 73361, 73974, 74724, 75360,
- 76008, 76666, 77337, 78019, 78713, 79277, 79994, 80724, 81317, 82071,
- 82685, 83308, 83940, 84744, 85399, 86063, 86738, 87252, 87945, 88650,
- 89367, 89912, 90649, 91398, 91968, 92739, 93326, 93921, 94725, 95337,
- 95958, 96586, 97437, 98086, 98742, 99408, 100083, 100767, 101228, 101928,
- 102637, 103357, 104086, 104578, 105325, 106083, 106594, 107370, 107894, 108689,
- 109226, 110041, 110592, 111427, 111991,
- };
- static unsigned short FreqTab_Y[X_FreqTab_Number]@".ccram";
- static unsigned short FreqTab_Y2[X_FreqTab_Number]@".ccram";
- void CalculateSModelLine(unsigned short *fre,float len, float fre_max, float fre_min, float flexible)
- {
- int i=0;
- float deno ;
- float melo ;
- float delt = fre_max-fre_min;
- float freq_buff;
-
- for(i=0; i<len; i++)
- {
- melo = flexible * (i-len/2) / (len/2);
- deno = 1.0 / (1 + expf(-melo)); //expf is a library function of exponential(e)
- freq_buff = delt * deno + fre_min;
- *(fre+i) = 4000000/(unsigned short)freq_buff;
- }
-
-
- for(i=(int)len;i<DATA_SIZE;i++)
- {
- *(fre+i) = *(fre+(unsigned short)(len - 1));
- }
- }
- /*
- void CalAccDecTab(unsigned char *acc_dec,unsigned short acc_dec_time,unsigned short max_speed)
- {
- float acc_time_buff,deno;
- unsigned short i;
- acc_time_buff = acc_dec_time;
- acc_time_buff = acc_time_buff / (max_speed);
-
- for(i = 0;i<max_speed;i++)
- {
- *(acc_dec+i) = (unsigned char)acc_time_buff;
- }
-
- cPulseStepDEC = 0;
-
- for(i = 0;i<DATA_SIZE;i++)
- {
- if(*(acc_dec+i) == 0)
- cPulseStepDEC += 1;
- else
- cPulseStepDEC += *(acc_dec+i);
- }
-
- cDecPulseBuff = cPulseStepDEC;
- }*/
- // APB2_TIM9 X
- void CalFreqTab_X(void)
- {
- short i = 0;
- short j = 0;
- j = sizeof(FreqTab_XX) / 4;
- for (i = 0; i < j; i++)
- {
- if (FreqTab_XX[i] > 0)
- {
- FreqTab_X[i] = 4000000/(FreqTab_XX[i]/7);
- }
- }
- }
- void CalFreqTab_X_Free(unsigned char divide_var)
- {
- short i = 0;
- short j = 0;
- j = sizeof(FreqTab_XX) / 4;
- for (i = 0; i < j; i++)
- {
- if (FreqTab_XX[i] > 0)
- {
- FreqTab_X[i] = 4000000/(FreqTab_XX[i]/divide_var);
- }
- }
- }
- void CalFreqTab_X_Free_JY(unsigned char divide_var)
- {
- short i = 0;
- short j = 0;
- j = sizeof(FreqTab_XX) / 4;
- for (i = 0; i < j; i=i+3)
- {
- if (FreqTab_XX[i] > 0)
- {
- if(i >= j)i= j-1;
- FreqTab_X[i] = 4000000/(FreqTab_XX[3*i]/divide_var);
- }
- }
- }
- // APB1_TIM13 Y
- void CalFreqTab_Y(void)
- {
- short i = 0;
- short j = 0;
- j = sizeof(FreqTab_YY) / 4;
- for (i = 0; i < j; i++)
- {
- if (FreqTab_YY[i] > 0)
- {
- FreqTab_Y[i] = 4000000/(FreqTab_YY[i]/7);
- }
- }
- }
- //#pragma inline
- void XAxis_PWMProc(void)
- {
- if (bXDirction) tXAxisStepper.cRealPosi--;
- else tXAxisStepper.cRealPosi++;
- PWMAction();
- if (tXAxisStepper.bAuto_Mov == 1)
- {
- tXAxisStepper.cPmovPulse--;
- if (tXAxisStepper.cPmovPulse == 0)
- {
- hw_pwm_stop(axis_x->axis_no);
- tXAxisStepper.bAuto_Mov = 0;
- tXAxisStepper.cCurSpeed = 0;
- }
- else if (tXAxisStepper.cPmovPulse > tXAxisStepper.cCalDecPulse)
- {
- if (tXAxisStepper.cCurSpeed < tXAxisStepper.cCalSpeed)
- {
- tXAxisStepper.cAccDecVal++;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- else
- {
- if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
- {
- tXAxisStepper.cAccDecVal++;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
-
- }
- else if(tXAxisStepper.bAuto_Mov == 2)
- {
- tXAxisStepper.cPmovPulse--;
- if (tXAxisStepper.cPmovPulse == 0)
- {
- hw_pwm_stop(axis_x->axis_no);
- tXAxisStepper.bAuto_Mov = 0;
- tXAxisStepper.cCurSpeed = 0;
- }
- else if (tXAxisStepper.cPmovPulse > tXAxisStepper.HighSpeedPulse + tXAxisStepper.cCalDecPulse)
- {
- if (tXAxisStepper.cCurSpeed < tXAxisStepper.FirstLowSpeed)
- {
- tXAxisStepper.cAccDecVal++;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- else if (tXAxisStepper.cPmovPulse > tXAxisStepper.cCalDecPulse)
- {
- if (tXAxisStepper.cCurSpeed < tXAxisStepper.cCalSpeed)
- {
- tXAxisStepper.cAccDecVal++;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- else
- {
- if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
- {
- tXAxisStepper.cAccDecVal++;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- }
- else
- {
- if (tXAxisStepper.State == 3)
- {
- if (hw_io_pin_input(5))
- {
- tXAxisStepper.cZeroFilter++;
- if (tXAxisStepper.cZeroFilter >= 3)
- {
- hw_pwm_stop(axis_x->axis_no);
- tXAxisStepper.cRealPosi = 0;
- tXAxisStepper.State = 0;
- tXAxisStepper.cCurSpeed = 0;
- }
- }
- else
- {
- tXAxisStepper.cZeroFilter = 0;
- }
-
- if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- else if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- else if (tXAxisStepper.State == 1)
- {
- if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- else if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- else if (tXAxisStepper.State == 2)
- {
- if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- else
- {
- hw_pwm_stop(axis_x->axis_no);
- tXAxisStepper.State = 0;
- tXAxisStepper.cCurSpeed = 0;
- }
- }
- else if (tXAxisStepper.State == 4)
- {
- if(tXAxisStepper.cPmovPulse)tXAxisStepper.cPmovPulse--;
-
- if (hw_io_pin_input(4))
- {
- tXAxisStepper.cZeroFilter++;
- if (tXAxisStepper.cZeroFilter >= 3)
- {
- hw_pwm_stop(axis_x->axis_no);
- tXAxisStepper.State = 0;
- tXAxisStepper.cCurSpeed = 0;
- }
- }
- else
- {
- tXAxisStepper.cZeroFilter = 0;
- }
-
- if(tXAxisStepper.cPmovPulse == 0)
- {
- if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetStartSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- else
- {
- if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- }
- else if (tXAxisStepper.State == 5)
- {
- tXAxisStepper.cPmovPulse--;
- if (tXAxisStepper.cPmovPulse == 0)
- {
- hw_pwm_stop(axis_x->axis_no);
- tXAxisStepper.bAuto_Mov = 0;
- tXAxisStepper.cCurSpeed = 0;
- }
- if (tXAxisStepper.cCurSpeed < tXAxisStepper.cSetSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetAccVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- else if (tXAxisStepper.cCurSpeed > tXAxisStepper.cSetSpeed)
- {
- ++tXAxisStepper.cAccDecVal;
- if (tXAxisStepper.cAccDecVal >= tXAxisStepper.cSetDecVal)
- {
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- }
- }
- }
- }
- }
- /********************************************
- MTU9-PWM????????ж????
- *********************************************/
- //#pragma inline
- void YAxis_PWMProc(void)
- { // y zhou
- if (bYDirction) tYAxisStepper.cRealPosi++;
- else tYAxisStepper.cRealPosi--;
-
- if (tYAxisStepper.bAuto_Mov)
- {
- if (--tYAxisStepper.cPmovPulse == 0)
- {
- hw_pwm_stop(axis_y->axis_no);
- Y_DRV = 0;
- tYAxisStepper.cCurSpeed = 0;
- tYAxisStepper.bAuto_Mov = 0;
- }
- else if (tYAxisStepper.cPmovPulse > tYAxisStepper.cCalDecPulse)
- {
- if (tYAxisStepper.cCurSpeed < tYAxisStepper.cCalSpeed)
- {
- if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetAccVal)
- {
- tYAxisStepper.cAccDecVal = 0;
- tYAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
- }
- }
- }
- else
- {
- if (tYAxisStepper.cCurSpeed > tYAxisStepper.cSetStartSpeed)
- {
- if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetDecVal)
- {
- tYAxisStepper.cAccDecVal = 0;
- tYAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
- }
- }
- }
- }
- else
- {
- if (tYAxisStepper.State == 3)
- {
- if (1)
- {
- tYAxisStepper.cZeroFilter++;
- if (tYAxisStepper.cZeroFilter >= 50)
- {
- hw_pwm_stop(axis_y->axis_no);;
- Y_DRV = 0;
- tYAxisStepper.cRealPosi = 0;
- tYAxisStepper.State = 0;
- tYAxisStepper.cCurSpeed = 0;
- }
- }
- else tYAxisStepper.cZeroFilter = 0;
- }
- else if (tYAxisStepper.State == 1)
- {
- if (tYAxisStepper.cCurSpeed < tYAxisStepper.cSetSpeed)
- {
- if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetAccVal)
- {
- tYAxisStepper.cAccDecVal = 0;
- tYAxisStepper.cCurSpeed++;
- hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
- }
- }
- else tYAxisStepper.State = 0;
- }
- else if (tYAxisStepper.State == 2)
- {
- if (tYAxisStepper.cCurSpeed > tYAxisStepper.cSetStartSpeed)
- {
- if (++tYAxisStepper.cAccDecVal >= tYAxisStepper.cSetDecVal)
- {
- tYAxisStepper.cAccDecVal = 0;
- tYAxisStepper.cCurSpeed--;
- hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
- }
- }
- else
- {
- hw_pwm_stop(axis_y->axis_no);
- Y_DRV = 0;
- tYAxisStepper.State = 0;
- tYAxisStepper.cCurSpeed = 0;
- }
- }
- }
- }
- void StartPWM(unsigned short cAxisNo)
- {
- if (cAxisNo == X_AXIS)
- {
- tXAxisStepper.cCurSpeed = tXAxisStepper.cSetStartSpeed;
- hw_pwm_set_period(axis_x->axis_no,FreqTab_X[tXAxisStepper.cCurSpeed]);
- // X_CNT = 0;
- hw_pwm_start(axis_x->axis_no);
- // X_DRV = 1;
- }
- else if (cAxisNo == Y_AXIS)
- {
- tYAxisStepper.cCurSpeed = tYAxisStepper.cSetStartSpeed;
- hw_pwm_set_period(axis_y->axis_no,FreqTab_Y[tYAxisStepper.cCurSpeed]);
- // Y_CNT = 0;
- hw_pwm_start(axis_y->axis_no);
- }
- }
- void SetAxisDir_Move(StepperDataInfo *tCurSetStpper, unsigned short dir)
- {
- switch (tCurSetStpper->cAxisNo)
- {
- case X_AXIS:
- hw_pwm_set_dir(axis_x->axis_no, dir);
- break;
- case Y_AXIS:
- hw_pwm_set_dir(axis_y->axis_no, dir);
- break;
- default:
- tCurSetStpper->cPmovPulse = 0;
- break;
- }
- }
- void LCalRunPulse(StepperDataInfo *tCurSetStpper)
- {
- if (tCurSetStpper->cDestPosi & 0x80000000)
- {
- tCurSetStpper->cPmovPulse = 0 - tCurSetStpper->cDestPosi;
- SetAxisDir_Move(tCurSetStpper, 1);
- }
- else
- {
- tCurSetStpper->cPmovPulse = tCurSetStpper->cDestPosi;
- SetAxisDir_Move(tCurSetStpper, 0);
- }
- tCurSetStpper->cCalAccPulse = tCurSetStpper->cSetAccVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->cSetStartSpeed);
- tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->cSetStartSpeed);
- tCurSetStpper->cCurSpeed = tCurSetStpper->cSetStartSpeed;
- tCurSetStpper->cAccDecVal = 0;
- if (tCurSetStpper->cPmovPulse < (tCurSetStpper->cCalAccPulse + tCurSetStpper->cCalDecPulse))
- {
- tCurSetStpper->cCalSpeed = tCurSetStpper->cPmovPulse / (tCurSetStpper->cSetAccVal + tCurSetStpper->cSetDecVal);
- tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cCalSpeed - tCurSetStpper->cSetStartSpeed);
- }
- else
- {
- tCurSetStpper->cCalSpeed = tCurSetStpper->cSetSpeed;
- }
- }
- void LCalRunPulse2(StepperDataInfo *tCurSetStpper)
- {
- if (tCurSetStpper->cDestPosi & 0x80000000)
- {
- tCurSetStpper->cPmovPulse = 0 - tCurSetStpper->cDestPosi;
- SetAxisDir_Move(tCurSetStpper, 0);
- }
- else
- {
- tCurSetStpper->cPmovPulse = tCurSetStpper->cDestPosi;
- SetAxisDir_Move(tCurSetStpper, 1);
- }
-
- tCurSetStpper->FirstAccPulse = tCurSetStpper->cSetAccVal * (tCurSetStpper->FirstLowSpeed - tCurSetStpper->cSetStartSpeed) + tCurSetStpper->LowSpeedPulse;
- tCurSetStpper->cCalAccPulse = tCurSetStpper->cSetAccVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->FirstLowSpeed);
- tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cSetSpeed - tCurSetStpper->cSetStartSpeed);
- tCurSetStpper->cCurSpeed = tCurSetStpper->cSetStartSpeed;
- tCurSetStpper->cAccDecVal = 0;
- if (tCurSetStpper->cPmovPulse < (tCurSetStpper->cCalAccPulse + tCurSetStpper->cCalDecPulse + tCurSetStpper->FirstAccPulse))
- {
- tCurSetStpper->cCalSpeed = tCurSetStpper->FirstLowSpeed;
- tCurSetStpper->FirstAccPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->FirstLowSpeed - tCurSetStpper->cSetStartSpeed);
- tCurSetStpper->cCalDecPulse = tCurSetStpper->cSetDecVal * (tCurSetStpper->cCalSpeed - tCurSetStpper->FirstLowSpeed);
- tCurSetStpper->HighSpeedPulse = tCurSetStpper->cPmovPulse - tCurSetStpper->cCalDecPulse;
- }
- else
- {
- tCurSetStpper->HighSpeedPulse = tCurSetStpper->cPmovPulse - tCurSetStpper->cCalAccPulse - tCurSetStpper->FirstAccPulse - tCurSetStpper->cCalDecPulse;
- tCurSetStpper->cCalSpeed = tCurSetStpper->cSetSpeed;
- }
- }
- void MV_Set_Startv_CPU(unsigned short cAxisNo, unsigned short cSpeed)
- {
- StepInfo[cAxisNo]->cSetStartSpeed = cSpeed;
- }
- void MV_Set_Speed_CPU(unsigned short cAxisNo, unsigned short cSpeed)
- {
- if (cSpeed < StepInfo[X_AXIS]->cSetStartSpeed)
- {
- cSpeed = StepInfo[X_AXIS]->cSetStartSpeed;
- }
- StepInfo[cAxisNo]->cSetSpeed = cSpeed;
- }
- void MoveChangSpeed(unsigned short cAxisNo, unsigned short cSpeed)
- {
- MV_Set_Speed_CPU(cAxisNo, cSpeed);
- }
- void MoveChangSpeedPos(unsigned short cAxisNo, unsigned short cSpeed,long pulse)
- {
- StepInfo[cAxisNo]->cCalDecPulse = StepInfo[cAxisNo]->cCalDecPulse + pulse;
- StepInfo[cAxisNo]->cSetStartSpeed = cSpeed;
- //MV_Set_Speed_CPU(cAxisNo, cSpeed);
- }
- void MV_Set_FirstSpeed(unsigned short cAxisNo, unsigned short cSpeed)
- {
- StepInfo[cAxisNo]->FirstLowSpeed = cSpeed;
- }
- void MV_Set_FirstLowPulse(unsigned short cAxisNo,long pulse)
- {
- StepInfo[cAxisNo]->LowSpeedPulse = pulse;
- }
- void MV_Set_Acc_CPU(unsigned short cAxisNo, unsigned short cSetAcc)
- {
- StepInfo[cAxisNo]->cSetAccVal = cSetAcc;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- }
- void MV_AccDec_Set_CPU(unsigned short cAxisNo, unsigned short cSetAcc, unsigned short cSetDec)
- {
- StepInfo[cAxisNo]->cSetAccVal = cSetAcc;
- StepInfo[cAxisNo]->cSetDecVal = cSetDec;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- }
- void MV_Set_Dec_CPU(unsigned short cAxisNo, unsigned short cSetDec)
- {
- StepInfo[cAxisNo]->cSetDecVal = cSetDec;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- }
- long MV_Get_Command_Pos_CPU(unsigned short cAxisNo)
- {
- long cRealPosi = 0;
- cRealPosi = StepInfo[cAxisNo]->cRealPosi;
- return cRealPosi;
- }
- void MV_Set_Command_Pos_CPU(unsigned short cAxisNo, long pos)
- {
- StepInfo[cAxisNo]->cRealPosi = pos;
- }
- void MV_Pmove_CPU(unsigned short cAxisNo, long dwPosi)
- {
- StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
- //StepInfo[cAxisNo]->cSetStartSpeed = 3;
- StepInfo[cAxisNo]->State = 0;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- LCalRunPulse(StepInfo[cAxisNo]);
- if (StepInfo[cAxisNo]->cPmovPulse)
- {
- StepInfo[cAxisNo]->bAuto_Mov = 1;
- StartPWM(cAxisNo);
- }
- }
- void MV_Set_Command_SlMTP_CPU(unsigned short cAxisNo, unsigned long dwMaxLen)
- {
- StepInfo[cAxisNo]->cMaxPulse = dwMaxLen;
- }
- void MV_Set_Command_SlMTN_CPU(unsigned short cAxisNo, unsigned long dwMinLen)
- {
- StepInfo[cAxisNo]->cMinPulse = dwMinLen;
- }
- void MV_Suddent_Stop(unsigned short cAxisNo)
- {
- switch (cAxisNo)
- {
- case X_AXIS:
- hw_pwm_stop(axis_x->axis_no);
- X_DRV = 0;
- tXAxisStepper.bAuto_Mov = 0;
- break;
- case Y_AXIS:
- hw_pwm_stop(axis_y->axis_no);
- Y_DRV = 0;
- tYAxisStepper.bAuto_Mov = 0;
- break;
- }
- StepInfo[cAxisNo]->bAuto_Mov = 0;
- StepInfo[cAxisNo]->cRstStep = 0;
- StepInfo[cAxisNo]->State = 0;
- StepInfo[cAxisNo]->cCurSpeed = 0;
- }
- void MV_Dec_Stop_CPU(unsigned short cAxisNo)
- {
- StepInfo[cAxisNo]->bAuto_Mov = 0;
- StepInfo[cAxisNo]->State = 2;
- }
- unsigned long MV_Cal_Dec_pulse(unsigned short high_speed,unsigned short low_speed,unsigned short dec_time)
- {
- if(high_speed > low_speed)
- return ((high_speed - low_speed)*dec_time);
- else
- return 0;
- }
- void MV_Continue_Move_CPU(unsigned short cAxisNo, unsigned short dir)
- {
- StepInfo[cAxisNo]->bAuto_Mov = 0;
- switch (cAxisNo)
- {
- case X_AXIS:
- if(SET_PULSE_TYPE != X_AXIS_PULSE_Y_ON)hw_pwm_set_dir(axis_x->axis_no, dir);
- break;
- case Y_AXIS:
- hw_pwm_set_dir(axis_y->axis_no, dir);
- break;
- }
- StepInfo[cAxisNo]->State = 1;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- StartPWM(cAxisNo);
- }
- void MV_Continue_Move_CPU_With_Stop(unsigned short cAxisNo, unsigned short dir)
- {
- StepInfo[cAxisNo]->bAuto_Mov = 0;
- switch (cAxisNo)
- {
- case X_AXIS:
- hw_pwm_set_dir(axis_x->axis_no, dir);
- break;
- case Y_AXIS:
- hw_pwm_set_dir(axis_y->axis_no, dir);
- break;
- }
- StepInfo[cAxisNo]->State = 3;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- StartPWM(cAxisNo);
- }
- void MV_Pmove2(unsigned short cAxisNo, unsigned long dwPosi,long back_lowpos)
- {
- StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
- StepInfo[cAxisNo]->State = 0;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- LCalRunPulse2(StepInfo[cAxisNo]);
- StepInfo[cAxisNo]->cCalDecPulse += back_lowpos;
- if (StepInfo[cAxisNo]->cPmovPulse)
- {
- StepInfo[cAxisNo]->bAuto_Mov = 2;
- StartPWM(cAxisNo);
- }
- }
- /**
- * 连续走
- *
- * @author xt (2019/7/23)
- *
- * @param cAxisNo
- * @param dir
- * @param startv
- * @param cSpeed
- */
- void MoveAction_Const(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(cAxisNo, 20);
- MV_AccDec_Set_CPU(cAxisNo, 10, 30);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Continue_Move_CPU(cAxisNo,dir);
- }
- else if (cAxisNo == Y_AXIS)
- {
- MV_Set_Startv_CPU(Y_AXIS, 1);
- MV_AccDec_Set_CPU(Y_AXIS, 10, 10);
- MV_Set_Speed_CPU(Y_AXIS, cSpeed);
- MV_Continue_Move_CPU(cAxisNo,dir);
- }
- }
- void MoveAction_Const_AccDec(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short start_speed,unsigned short accpulse,unsigned short dec_pulse)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(cAxisNo, start_speed);
- MV_AccDec_Set_CPU(cAxisNo, accpulse, dec_pulse);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Continue_Move_CPU(cAxisNo,dir);
- }
- else if (cAxisNo == Y_AXIS)
- {
- MV_Set_Startv_CPU(Y_AXIS, start_speed);
- MV_AccDec_Set_CPU(Y_AXIS, accpulse, dec_pulse);
- MV_Set_Speed_CPU(Y_AXIS, cSpeed);
- MV_Continue_Move_CPU(cAxisNo,dir);
- }
- }
- void MoveAction_Const_Back(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short low_speed,unsigned long pulse)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(cAxisNo, low_speed);
- MV_AccDec_Set_CPU(cAxisNo, 10, 30);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- StepInfo[cAxisNo]->bAuto_Mov = 0;
- switch (cAxisNo)
- {
- case X_AXIS:
- hw_pwm_set_dir(axis_x->axis_no, dir);
- break;
- }
- StepInfo[cAxisNo]->State = 4;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- tXAxisStepper.cPmovPulse = pulse;
- StartPWM(cAxisNo);
- }
- }
- void MoveAction_Const_Back_AccDec(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short low_speed,unsigned long pulse,unsigned short start_speed,unsigned short cAccPulse,unsigned short cDecPulse)
- {
- MV_Set_Startv_CPU(cAxisNo, start_speed);
- MV_AccDec_Set_CPU(cAxisNo, cAccPulse, cDecPulse);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- StepInfo[cAxisNo]->bAuto_Mov = 0;
- hw_pwm_set_dir(axis_x->axis_no, dir);
- StepInfo[cAxisNo]->State = 4;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- tXAxisStepper.cPmovPulse = pulse;
- StartPWM(cAxisNo);
- }
- void MoveAction_Const_Stop(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(cAxisNo, 20);
- MV_AccDec_Set_CPU(cAxisNo, 10, 30);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Continue_Move_CPU_With_Stop(cAxisNo,dir);
- }
- }
- void MoveAction_Const_Stop_AccDec(unsigned short cAxisNo, unsigned short dir, unsigned short cSpeed,unsigned short cStartSpeed,unsigned short cAccPulse,unsigned short cDecPulse)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(cAxisNo, cStartSpeed);
- MV_AccDec_Set_CPU(cAxisNo, cAccPulse, cDecPulse);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Continue_Move_CPU_With_Stop(cAxisNo,dir);
- }
- }
- // 走相对位置脉冲
- void MoveAction_Pulse(unsigned short cAxisNo, long dwPosi, unsigned short cSpeed)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(X_AXIS, 2);
- MV_Set_Acc_CPU(X_AXIS, 10);
- MV_Set_Dec_CPU(X_AXIS, 5);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Pmove_CPU(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS));
- }
- else if (cAxisNo == Y_AXIS)
- {
- unsigned short acc = 10;
- MV_Set_Startv_CPU(Y_AXIS, 3);
- MV_Set_Acc_CPU(Y_AXIS, acc);
- MV_Set_Dec_CPU(Y_AXIS, acc);
- MV_Set_Speed_CPU(Y_AXIS, cSpeed);
- MV_Pmove_CPU(Y_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS));
- }
- }
- // 走相对位置脉冲
- void MoveAction_Pulse_AccDec(unsigned short cAxisNo, long dwPosi, unsigned short cSpeed,unsigned short cStartSpeed,unsigned short cAccPulse,unsigned short cDecPulse)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(X_AXIS, cStartSpeed);
- MV_Set_Acc_CPU(X_AXIS, cAccPulse);
- MV_Set_Dec_CPU(X_AXIS, cDecPulse);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Pmove_CPU(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS));
- }
- }
- // 两段速走相对位置脉冲最后一段慢速长度
- void MoveAction_Pulse2(unsigned short cAxisNo, long dwPosi,long lowpos,long back_lowpos, unsigned short cSpeed,unsigned short low_speed)
- {
- if (cAxisNo == X_AXIS)
- {
- unsigned short acc = 10;
- MV_Set_Startv_CPU(X_AXIS, 3);
- MV_Set_Acc_CPU(X_AXIS, acc);
- MV_Set_Dec_CPU(X_AXIS, acc);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Set_FirstSpeed(X_AXIS, low_speed);
- MV_Set_FirstLowPulse(X_AXIS,lowpos);
- MV_Pmove2(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS),back_lowpos);
- }
- else if (cAxisNo == Y_AXIS)
- {
- unsigned short acc = 10;
- MV_Set_Startv_CPU(Y_AXIS, 3);
- MV_Set_Acc_CPU(Y_AXIS, acc);
- MV_Set_Dec_CPU(Y_AXIS, acc);
- MV_Set_Speed_CPU(Y_AXIS, cSpeed);
- MV_Set_FirstSpeed(Y_AXIS, low_speed);
- MV_Set_FirstLowPulse(Y_AXIS,lowpos);
- MV_Pmove2(Y_AXIS, dwPosi + MV_Get_Command_Pos_CPU(Y_AXIS),back_lowpos);
- }
- }
- // 两段速走相对位置脉冲最后一段慢速长度
- void MoveAction_Pulse2_AccDec(unsigned short cAxisNo, long dwPosi,long lowpos,long back_lowpos, unsigned short cSpeed,unsigned short low_speed,unsigned short start_speed, unsigned short acc_pulse,unsigned short dec_pulse)
- {
- if (cAxisNo == X_AXIS)
- {
- MV_Set_Startv_CPU(X_AXIS, start_speed);
- MV_Set_Acc_CPU(X_AXIS, acc_pulse);
- MV_Set_Dec_CPU(X_AXIS, dec_pulse);
- MV_Set_Speed_CPU(X_AXIS, cSpeed);
- MV_Set_FirstSpeed(X_AXIS, low_speed);
- MV_Set_FirstLowPulse(X_AXIS,lowpos);
- MV_Pmove2(X_AXIS, dwPosi + MV_Get_Command_Pos_CPU(X_AXIS),back_lowpos);
- }
- else if (cAxisNo == Y_AXIS)
- {
- MV_Set_Startv_CPU(Y_AXIS, start_speed);
- MV_Set_Acc_CPU(Y_AXIS, acc_pulse);
- MV_Set_Dec_CPU(Y_AXIS, dec_pulse);
- MV_Set_Speed_CPU(Y_AXIS, cSpeed);
- MV_Set_FirstSpeed(Y_AXIS, low_speed);
- MV_Set_FirstLowPulse(Y_AXIS,lowpos);
- MV_Pmove2(Y_AXIS, dwPosi + MV_Get_Command_Pos_CPU(Y_AXIS),back_lowpos);
- }
- }
- /**
- * 必须与当前运动方向相同
- *
- * @author zhang (2019/12/6)
- *
- * @param cAxisNo
- * @param dwPosi
- */
- void MV_Move_To_Position(unsigned short cAxisNo, long dwPosi)
- {
- if (dwPosi < StepInfo[cAxisNo]->cRealPosi)
- {
- StepInfo[cAxisNo]->cDestPosi = StepInfo[cAxisNo]->cRealPosi - dwPosi;
- }
- else
- {
- StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
- }
- {
- //StepInfo[cAxisNo]->cDestPosi = dwPosi - StepInfo[cAxisNo]->cRealPosi;
- StepInfo[cAxisNo]->cPmovPulse = StepInfo[cAxisNo]->cDestPosi;
- StepInfo[cAxisNo]->State = 0;
- StepInfo[cAxisNo]->cAccDecVal = 0;
- StepInfo[cAxisNo]->cCalSpeed = StepInfo[cAxisNo]->cSetSpeed;
- StepInfo[cAxisNo]->cCalDecPulse = (StepInfo[cAxisNo]->cSetSpeed - StepInfo[cAxisNo]->cSetStartSpeed);
- StepInfo[cAxisNo]->cCalDecPulse *= StepInfo[cAxisNo]->cSetDecVal;
- StepInfo[cAxisNo]->cCalDecPulse += 50;
- StepInfo[cAxisNo]->bAuto_Mov = 1;
- }
- }
- void ResetServoDrv_CPU(unsigned short axis)
- {
- if (axis == X_AXIS)
- {
- hw_pwm_set_enable(axis_x->axis_no,1);
- CalFreqTab_X();
- tXAxisStepper.cAxisNo = X_AXIS;
- tXAxisStepper.cDestPosi = 0;
- tXAxisStepper.cSetStartSpeed = 0;
- tXAxisStepper.cSetSpeed = 20;
- tXAxisStepper.cSetAccVal = 50;
- tXAxisStepper.cSetDecVal = 50;
- tXAxisStepper.State = 0;
- tXAxisStepper.cAccDecVal = 0;
- tXAxisStepper.PulseOffset = 0;
- }
- else if (axis == Y_AXIS)
- {
- hw_pwm_set_enable(axis_y->axis_no,1);
- CalFreqTab_Y();
- tYAxisStepper.cAxisNo = Y_AXIS;
- tYAxisStepper.cDestPosi = 0;
- tYAxisStepper.cSetStartSpeed = 0;
- tYAxisStepper.cSetSpeed = 20;
- tYAxisStepper.cSetAccVal = 50;
- tYAxisStepper.cSetDecVal = 50;
- tYAxisStepper.State = 0;
- tYAxisStepper.cAccDecVal = 0;
- tYAxisStepper.PulseOffset = 0;
- }
- }
- void SetRemainLength(unsigned short cAxisNo, long dwPosi,unsigned short set_speed)
- {
- if(dwPosi == 0)
- {
- AxisEgmStop(X_AXIS);
- }
- else
- {
- StepInfo[cAxisNo]->bAuto_Mov = 0;
- StepInfo[cAxisNo]->State = 5;
- StepInfo[cAxisNo]->cSetSpeed = set_speed;
- StepInfo[cAxisNo]->cPmovPulse = PosToPulse(cAxisNo,dwPosi);
- }
-
- }
|