我用mega2560控制5个伺服马达做圆周运动，先规划好轨迹，然后命令马达动…
但是动起来，时不时一卡一卡的，是什么的地方的问题呢？！
#include <Servo.h>
Servo L1;
Servo L2;
Servo L3;
Servo L4;
Servo L5;// create servo object to control a servo
// a maximum of eight servo objects can be created
int pos = 0;
double L1pos[360]={81.2538,81.2542,81.2555,81.2575,81.2603,81.2639,81.2683,81.2735,81.2795,81.2863,81.2939,81.3022,81.3114,81.3212,81.3318,81.3432,81.3553,81.3681,81.3816,81.3959,81.4108,81.4264,81.4426,81.4595,81.4771,81.4953,81.5141,81.5335,81.5534,81.5740,81.5951,81.6167,81.6388,81.6615,81.6847,81.7083,81.7324,81.7569,81.7818,81.8072,81.8330,81.8591,81.8856,81.9124,81.9396,81.9670,81.9948,82.0228,82.0511,82.0797,82.1084,82.1374,82.1666,82.1960,82.2255,82.2552,82.2850,82.3149,82.3450,82.3751,82.4054,82.4356,82.4660,82.4964,82.5268,82.5572,82.5876,82.6180,82.6484,82.6787,82.7090,82.7392,82.7694,82.7995,82.8295,82.8594,82.8892,82.9189,82.9484,82.9778,83.0071,83.0362,83.0652,83.0940,83.1226,83.1510,83.1792,83.2073,83.2351,83.2627,83.2902,83.3174,83.3443,83.3711,83.3976,83.4238,83.4498,83.4756,83.5011,83.5263,83.5513,83.5760,83.6004,83.6246,83.6485,83.6721,83.6954,83.7185,83.7412,83.7637,83.7859,83.8077,83.8293,83.8506,83.8716,83.8922,83.9126,83.9327,83.9524,83.9719,83.9910,84.0098,84.0283,84.0465,84.0644,84.0820,84.0993,84.1162,84.1328,84.1491,84.1651,84.1808,84.1962,84.2112,84.2259,84.2403,84.2544,84.2682,84.2816,84.2947,84.3076,84.3200,84.3322,84.3441,84.3556,84.3668,84.3777,84.3883,84.3985,84.4085,84.4181,84.4274,84.4364,84.4450,84.4534,84.4614,84.4691,84.4765,84.4836,84.4904,84.4968,84.5030,84.5088,84.5143,84.5195,84.5243,84.5289,84.5331,84.5371,84.5407,84.5440,84.5470,84.5496,84.5520,84.5540,84.5557,84.5572,84.5583,84.5590,84.5595,84.5597,84.5595,84.5590,84.5583,84.5572,84.5557,84.5540,84.5520,84.5496,84.5470,84.5440,84.5407,84.5371,84.5331,84.5289,84.5243,84.5195,84.5143,84.5088,84.5030,84.4968,84.4904,84.4836,84.4765,84.4691,84.4614,84.4534,84.4450,84.4364,84.4274,84.4181,84.4085,84.3985,84.3883,84.3777,84.3668,84.3556,84.3441,84.3322,84.3200,84.3076,84.2947,84.2816,84.2682,84.2544,84.2403,84.2259,84.2112,84.1962,84.1808,84.1651,84.1491,84.1328,84.1162,84.0993,84.0820,84.0644,84.0465,84.0283,84.0098,83.9910,83.9719,83.9524,83.9327,83.9126,83.8922,83.8716,83.8506,83.8293,83.8077,83.7859,83.7637,83.7412,83.7185,83.6954,83.6721,83.6485,83.6246,83.6004,83.5760,83.5513,83.5263,83.5011,83.4756,83.4498,83.4238,83.3976,83.3711,83.3443,83.3174,83.2902,83.2627,83.2351,83.2073,83.1792,83.1510,83.1226,83.0940,83.0652,83.0362,83.0071,82.9778,82.9484,82.9189,82.8892,82.8594,82.8295,82.7995,82.7694,82.7392,82.7090,82.6787,82.6484,82.6180,82.5876,82.5572,82.5268,82.4964,82.4660,82.4356,82.4054,82.3751,82.3450,82.3149,82.2850,82.2552,82.2255,82.1960,82.1666,82.1374,82.1084,82.0797,82.0511,82.0228,81.9948,81.9670,81.9396,81.9124,81.8856,81.8591,81.8330,81.8072,81.7818,81.7569,81.7324,81.7083,81.6847,81.6615,81.6388,81.6167,81.5951,81.5740,81.5534,81.5335,81.5141,81.4953,81.4771,81.4595,81.4426,81.4264,81.4108,81.3959,81.3816,81.3681,81.3553,81.3432,81.3318,81.3212,81.3114,81.3022,81.2939,81.2863,81.2795,81.2735,81.2683,81.2639,81.2603,81.2575,81.2555,81.2542};
double L2pos[360]={69.8588,68.7586,67.6493,66.5333,65.4127,64.2897,63.1664,62.0448,60.9268,59.8144,58.7092,57.6130,56.5271,55.4529,54.3918,53.3448,52.3130,51.2971,50.2980,49.3163,48.3525,47.4070,46.4801,45.5722,44.6832,43.8133,42.9625,42.1308,41.3180,40.5239,39.7485,38.9913,38.2522,37.5308,36.8269,36.1400,35.4699,34.8162,34.1784,33.5563,32.9493,32.3573,31.7796,31.2161,30.6662,30.1296,29.6059,29.0949,28.5960,28.1090,27.6335,27.1691,26.7156,26.2726,25.8399,25.4170,25.0038,24.5998,24.2049,23.8188,23.4412,23.0719,22.7106,22.3571,22.0111,21.6725,21.3410,21.0164,20.6985,20.3872,20.0822,19.7833,19.4904,19.2034,18.9220,18.6462,18.3756,18.1103,17.8501,17.5948,17.3443,17.0985,16.8573,16.6205,16.3881,16.1599,15.9358,15.7158,15.4997,15.2875,15.0790,14.8742,14.6730,14.4753,14.2810,14.0901,13.9025,13.7182,13.5370,13.3590,13.1840,13.0121,12.8431,12.6770,12.5138,12.3534,12.1958,12.0409,11.8888,11.7393,11.5925,11.4484,11.3068,11.1679,11.0315,10.8976,10.7663,10.6376,10.5114,10.3877,10.2665,10.1479,10.0318,9.9183,9.8074,9.6990,9.5932,9.4901,9.3897,9.2919,9.1970,9.1048,9.0154,8.9290,8.8455,8.7651,8.6878,8.6137,8.5429,8.4755,8.4116,8.3513,8.2948,8.2422,8.1936,8.1493,8.1094,8.0740,8.0434,8.0179,7.9976,7.9828,7.9738,7.9709,7.9744,7.9846,8.0019,8.0266,8.0593,8.1002,8.1499,8.2089,8.2776,8.3565,8.4463,8.5474,8.6605,8.7861,8.9248,9.0773,9.2441,9.4257,9.6229,9.8360,10.0657,10.3122,10.5761,10.8577,11.1572,11.4749,11.8107,12.1648,12.5371,12.9274,13.3355,13.7611,14.2038,14.6632,15.1388,15.6300,16.1363,16.6570,17.1916,17.7394,18.2997,18.8720,19.4555,20.0497,20.6539,21.2676,21.8901,22.5210,23.1597,23.8057,24.4586,25.1178,25.7829,26.4536,27.1294,27.8100,28.4950,29.1840,29.8769,30.5733,31.2728,31.9754,32.6806,33.3882,34.0981,34.8100,35.5238,36.2391,36.9559,37.6739,38.3931,39.1131,39.8339,40.5554,41.2773,41.9996,42.7220,43.4446,44.1671,44.8894,45.6114,46.3330,47.0541,47.7745,48.4942,49.2131,49.9309,50.6477,51.3633,52.0777,52.7906,53.5021,54.2119,54.9201,55.6265,56.3309,57.0334,57.7337,58.4318,59.1276,59.8210,60.5118,61.2000,61.8854,62.5679,63.2474,63.9238,64.5970,65.2668,65.9332,66.5959,67.2549,67.9101,68.5612,69.2082,69.8509,70.4891,71.1228,71.7517,72.3757,72.9946,73.6083,74.2166,74.8193,75.4162,76.0071,76.5919,77.1703,77.7421,78.3071,78.8651,79.4159,79.9591,80.4947,81.0222,81.5415,82.0523,82.5542,83.0471,83.5305,84.0042,84.4679,84.9213,85.3639,85.7954,86.2155,86.6237,87.0197,87.4031,87.7734,88.1302,88.4731,88.8015,89.1150,89.4130,89.6952,89.9609,90.2095,90.4406,90.6535,90.8476,91.0224,91.1771,91.3112,91.4240,91.5147,91.5828,91.6274,91.6480,91.6437,91.6139,91.5578,91.4747,91.3638,91.2244,91.0558,90.8573,90.6282,90.3679,90.0756,89.7508,89.3930,89.0015,88.5761,88.1162,87.6216,87.0921,86.5275,85.9277,85.2929,84.6233,83.9191,83.1808,82.4091,81.6046,80.7682,79.9009,79.0040,78.0788,77.1267,76.1493,75.1484,74.1258,73.0835,72.0235,70.9479};
double L3pos[360]={15.8915,15.9044,15.9431,16.0075,16.0975,16.2128,16.3533,16.5186,16.7084,16.9223,17.1599,17.4207,17.7043,18.0101,18.3376,18.6862,19.0554,19.4446,19.8531,20.2804,20.7259,21.1890,21.6690,22.1655,22.6778,23.2052,23.7474,24.3037,24.8735,25.4565,26.0519,26.6594,27.2785,27.9087,28.5496,29.2007,29.8616,30.5320,31.2113,31.8994,32.5957,33.3000,34.0119,34.7312,35.4574,36.1904,36.9298,37.6755,38.4270,39.1842,39.9469,40.7148,41.4877,42.2655,43.0478,43.8345,44.6255,45.4205,46.2194,47.0221,47.8283,48.6379,49.4508,50.2669,51.0860,51.9079,52.7326,53.5600,54.3899,55.2222,56.0568,56.8936,57.7326,58.5735,59.4164,60.2611,61.1075,61.9556,62.8053,63.6564,64.5090,65.3629,66.2181,67.0744,67.9319,68.7904,69.6498,70.5102,71.3714,72.2334,73.0961,73.9594,74.8233,75.6877,76.5526,77.4178,78.2834,79.1492,80.0152,80.8813,81.7475,82.6137,83.4799,84.3459,85.2117,86.0772,86.9424,87.8073,88.6716,89.5354,90.3985,91.2610,92.1227,92.9835,93.8434,94.7023,95.5600,96.4165,97.2718,98.1256,98.9779,99.8286,100.6776,101.5247,102.3699,103.2130,104.0538,104.8923,105.7283,106.5615,107.3920,108.2194,109.0437,109.8646,110.6818,111.4953,112.3048,113.1100,113.9106,114.7065,115.4973,116.2826,117.0623,117.8359,118.6031,119.3634,120.1165,120.8619,121.5992,122.3278,123.0471,123.7567,124.4559,125.1441,125.8205,126.4845,127.1352,127.7719,128.3936,128.9995,129.5886,130.1599,130.7122,131.2445,131.7557,132.2446,132.7099,133.1505,133.5650,133.9522,134.3109,134.6398,134.9378,135.2037,135.4365,135.6352,135.7990,135.9272,136.0192,136.0746,136.0930,136.0746,136.0192,135.9272,135.7990,135.6352,135.4365,135.2037,134.9378,134.6398,134.3109,133.9522,133.5650,133.1505,132.7099,132.2446,131.7557,131.2445,130.7122,130.1599,129.5886,128.9995,128.3936,127.7719,127.1352,126.4845,125.8205,125.1441,124.4559,123.7567,123.0471,122.3278,121.5992,120.8619,120.1165,119.3634,118.6031,117.8359,117.0623,116.2826,115.4973,114.7065,113.9106,113.1100,112.3048,111.4953,110.6818,109.8646,109.0437,108.2194,107.3920,106.5615,105.7283,104.8923,104.0538,103.2130,102.3699,101.5247,100.6776,99.8286,98.9779,98.1256,97.2718,96.4165,95.5600,94.7023,93.8434,92.9835,92.1227,91.2610,90.3985,89.5354,88.6716,87.8073,86.9424,86.0772,85.2117,84.3459,83.4799,82.6137,81.7475,80.8813,80.0152,79.1492,78.2834,77.4178,76.5526,75.6877,74.8233,73.9594,73.0961,72.2334,71.3714,70.5102,69.6498,68.7904,67.9319,67.0744,66.2181,65.3629,64.5090,63.6564,62.8053,61.9556,61.1075,60.2611,59.4164,58.5735,57.7326,56.8936,56.0568,55.2222,54.3899,53.5600,52.7326,51.9079,51.0860,50.2669,49.4508,48.6379,47.8283,47.0221,46.2194,45.4205,44.6255,43.8345,43.0478,42.2655,41.4877,40.7148,39.9469,39.1842,38.4270,37.6755,36.9298,36.1904,35.4574,34.7312,34.0119,33.3000,32.5957,31.8994,31.2113,30.5320,29.8616,29.2007,28.5496,27.9087,27.2785,26.6594,26.0519,25.4565,24.8735,24.3037,23.7474,23.2052,22.6778,22.1655,21.6690,21.1890,20.7259,20.2804,19.8531,19.4446,19.0554,18.6862,18.3376,18.0101,17.7043,17.4207,17.1599,16.9223,16.7084,16.5186,16.3533,16.2128,16.0975,16.0075,15.9431,15.9044};
double L4pos[360]={70.7504,69.6630,68.5925,67.5409,66.5102,65.5026,64.5197,63.5634,62.6352,61.7367,60.8691,60.0337,59.2313,58.4630,57.7294,57.0310,56.3684,55.7417,55.1511,54.5967,54.0784,53.5960,53.1492,52.7377,52.3609,52.0186,51.7099,51.4345,51.1915,50.9804,50.8004,50.6507,50.5307,50.4395,50.3765,50.3407,50.3315,50.3481,50.3897,50.4556,50.5450,50.6572,50.7915,50.9472,51.1236,51.3200,51.5358,51.7703,52.0230,52.2932,52.5804,52.8840,53.2034,53.5381,53.8877,54.2515,54.6292,55.0203,55.4243,55.8409,56.2695,56.7098,57.1614,57.6240,58.0971,58.5804,59.0736,59.5764,60.0884,60.6094,61.1390,61.6769,62.2230,62.7769,63.3384,63.9072,64.4832,65.0660,65.6554,66.2512,66.8533,67.4614,68.0754,68.6949,69.3200,69.9503,70.5857,71.2260,71.8711,72.5209,73.1751,73.8336,74.4963,75.1630,75.8336,76.5079,77.1859,77.8674,78.5522,79.2403,79.9316,80.6258,81.3229,82.0229,82.7254,83.4306,84.1382,84.8482,85.5604,86.2747,86.9911,87.7094,88.4295,89.1514,89.8749,90.5999,91.3264,92.0541,92.7831,93.5133,94.2444,94.9765,95.7094,96.4430,97.1773,97.9120,98.6471,99.3824,100.1179,100.8535,101.5890,102.3242,103.0591,103.7935,104.5274,105.2604,105.9926,106.7237,107.4535,108.1820,108.9088,109.6340,110.3571,111.0781,111.7967,112.5127,113.2259,113.9359,114.6426,115.3456,116.0447,116.7395,117.4297,118.1150,118.7949,119.4691,120.1371,120.7985,121.4529,122.0998,122.7385,123.3687,123.9898,124.6011,125.2020,125.7920,126.3704,126.9366,127.4898,128.0295,128.5549,129.0655,129.5606,130.0397,130.5021,130.9474,131.3752,131.7849,132.1764,132.5494,132.9038,133.2394,133.5563,133.8546,134.1345,134.3963,134.6402,134.8668,135.0765,135.2698,135.4471,135.6092,135.7566,135.8899,136.0097,136.1166,136.2112,136.2942,136.3661,136.4274,136.4787,136.5206,136.5534,136.5776,136.5938,136.6023,136.6034,136.5977,136.5853,136.5667,136.5421,136.5118,136.4761,136.4352,136.3894,136.3388,136.2836,136.2241,136.1604,136.0927,136.0210,135.9456,135.8665,135.7839,135.6978,135.6084,135.5158,135.4199,135.3210,135.2190,135.1140,135.0061,134.8953,134.7817,134.6652,134.5460,134.4240,134.2993,134.1718,134.0417,133.9088,133.7733,133.6351,133.4942,133.3506,133.2043,133.0553,132.9036,132.7491,132.5919,132.4319,132.2691,132.1034,131.9349,131.7634,131.5890,131.4116,131.2312,131.0478,130.8611,130.6714,130.4783,130.2820,130.0824,129.8793,129.6727,129.4626,129.2489,129.0315,128.8103,128.5852,128.3562,128.1231,127.8859,127.6444,127.3987,127.1484,126.8937,126.6342,126.3700,126.1009,125.8267,125.5474,125.2627,124.9726,124.6770,124.3755,124.0682,123.7548,123.4351,123.1091,122.7764,122.4369,122.0905,121.7369,121.3759,121.0072,120.6308,120.2462,119.8534,119.4520,119.0418,118.6225,118.1939,117.7557,117.3076,116.8492,116.3804,115.9008,115.4100,114.9078,114.3938,113.8676,113.3290,112.7775,112.2128,111.6345,111.0424,110.4359,109.8147,109.1785,108.5268,107.8593,107.1757,106.4755,105.7585,105.0243,104.2725,103.5029,102.7152,101.9092,101.0847,100.2414,99.3793,98.4982,97.5982,96.6793,95.7416,94.7853,93.8106,92.8180,91.8079,90.7808,89.7375,88.6787,87.6053,86.5184,85.4192,84.3088,83.1889,82.0608,80.9263,79.7872,78.6453,77.5026,76.3613,75.2233,74.0910,72.9666,71.8523};
double L5pos[360]={76.2538,76.2542,76.2555,76.2575,76.2603,76.2639,76.2683,76.2735,76.2795,76.2863,76.2939,76.3022,76.3114,76.3212,76.3318,76.3432,76.3553,76.3681,76.3816,76.3959,76.4108,76.4264,76.4426,76.4595,76.4771,76.4953,76.5141,76.5335,76.5534,76.5740,76.5951,76.6167,76.6388,76.6615,76.6847,76.7083,76.7324,76.7569,76.7818,76.8072,76.8330,76.8591,76.8856,76.9124,76.9396,76.9670,76.9948,77.0228,77.0511,77.0797,77.1084,77.1374,77.1666,77.1960,77.2255,77.2552,77.2850,77.3149,77.3450,77.3751,77.4054,77.4356,77.4660,77.4964,77.5268,77.5572,77.5876,77.6180,77.6484,77.6787,77.7090,77.7392,77.7694,77.7995,77.8295,77.8594,77.8892,77.9189,77.9484,77.9778,78.0071,78.0362,78.0652,78.0940,78.1226,78.1510,78.1792,78.2073,78.2351,78.2627,78.2902,78.3174,78.3443,78.3711,78.3976,78.4238,78.4498,78.4756,78.5011,78.5263,78.5513,78.5760,78.6004,78.6246,78.6485,78.6721,78.6954,78.7185,78.7412,78.7637,78.7859,78.8077,78.8293,78.8506,78.8716,78.8922,78.9126,78.9327,78.9524,78.9719,78.9910,79.0098,79.0283,79.0465,79.0644,79.0820,79.0993,79.1162,79.1328,79.1491,79.1651,79.1808,79.1962,79.2112,79.2259,79.2403,79.2544,79.2682,79.2816,79.2947,79.3076,79.3200,79.3322,79.3441,79.3556,79.3668,79.3777,79.3883,79.3985,79.4085,79.4181,79.4274,79.4364,79.4450,79.4534,79.4614,79.4691,79.4765,79.4836,79.4904,79.4968,79.5030,79.5088,79.5143,79.5195,79.5243,79.5289,79.5331,79.5371,79.5407,79.5440,79.5470,79.5496,79.5520,79.5540,79.5557,79.5572,79.5583,79.5590,79.5595,79.5597,79.5595,79.5590,79.5583,79.5572,79.5557,79.5540,79.5520,79.5496,79.5470,79.5440,79.5407,79.5371,79.5331,79.5289,79.5243,79.5195,79.5143,79.5088,79.5030,79.4968,79.4904,79.4836,79.4765,79.4691,79.4614,79.4534,79.4450,79.4364,79.4274,79.4181,79.4085,79.3985,79.3883,79.3777,79.3668,79.3556,79.3441,79.3322,79.3200,79.3076,79.2947,79.2816,79.2682,79.2544,79.2403,79.2259,79.2112,79.1962,79.1808,79.1651,79.1491,79.1328,79.1162,79.0993,79.0820,79.0644,79.0465,79.0283,79.0098,78.9910,78.9719,78.9524,78.9327,78.9126,78.8922,78.8716,78.8506,78.8293,78.8077,78.7859,78.7637,78.7412,78.7185,78.6954,78.6721,78.6485,78.6246,78.6004,78.5760,78.5513,78.5263,78.5011,78.4756,78.4498,78.4238,78.3976,78.3711,78.3443,78.3174,78.2902,78.2627,78.2351,78.2073,78.1792,78.1510,78.1226,78.0940,78.0652,78.0362,78.0071,77.9778,77.9484,77.9189,77.8892,77.8594,77.8295,77.7995,77.7694,77.7392,77.7090,77.6787,77.6484,77.6180,77.5876,77.5572,77.5268,77.4964,77.4660,77.4356,77.4054,77.3751,77.3450,77.3149,77.2850,77.2552,77.2255,77.1960,77.1666,77.1374,77.1084,77.0797,77.0511,77.0228,76.9948,76.9670,76.9396,76.9124,76.8856,76.8591,76.8330,76.8072,76.7818,76.7569,76.7324,76.7083,76.6847,76.6615,76.6388,76.6167,76.5951,76.5740,76.5534,76.5335,76.5141,76.4953,76.4771,76.4595,76.4426,76.4264,76.4108,76.3959,76.3816,76.3681,76.3553,76.3432,76.3318,76.3212,76.3114,76.3022,76.2939,76.2863,76.2795,76.2735,76.2683,76.2639,76.2603,76.2575,76.2555,76.2542};
void setup()
{
// Serial.begin(9600);
L1.attach(2);
L2.attach(3);
L3.attach(4);
L4.attach(5);
L5.attach(6);
}
void loop()
{
for(pos = 359; pos>=0 ; pos--)
{
L1.write(180-L1pos[pos]);
L2.write(L2pos[pos]);
L3.write(L3pos[pos]);
L4.write(L4pos[pos]);
L5.write(170-L5pos[pos]);
delay(10) ;
}
if(pos <0)
{
pos=359;
L1.write(L1pos[pos]);
L2.write(L2pos[pos]);
L3.write(L3pos[pos]);
L4.write(L4pos[pos]);
L5.write(L5pos[pos]);
delay(10);
}
}
|
发表于 2013-4-1 12:06
发表于 2013-4-1 13:38