サーボが指定位置に移動しない

このコードはシリアル接続から正弦波パラメータを受け取り、正弦波の位置を8つのサーボモータにフォーマットして書き込みます。

位置は8台のモーターのうちの6台に書き込まれます。

動作しない2つのうち、私は彼らが故障しているかどうかを見るためにモーターを交換しようとしました。ではない。出力ピンを変更して、それらが損傷していないかどうかを確認しました。ではない。

私は方程式が解かれた順番を変えました、そして、奇妙なタイミングの問題がないことを確実にするためにポジションが書かれています。私は書かれた位置の完全なサイクルを保存し、正しい数(すなわち正しい振幅)を出力する。ここで見ることができます(フォーマットは "servo#": "position"):

0:1779 1:1337 2:1750 3:1480 4:1357 5:1438 6:1452 7:1490

0:1779 1:1337 2:1750 3:1480 4:1357 5:1438 6:1452 7:1490

0:1779 1:1337 2:1750 3:1480 4:1357 5:1438 6:1452 7:1490

0:1779 1:1337 2:1750 3:1480 4:1357 5:1438 6:1452 7:1490

0:1779 1:1337 2:1750 3:1480 4:1357 5:1438 6:1452 7:1490

0:1779 1:1337 2:1750 3:1480 4:1357 5:1438 6:1452 7:1490

0:1779 1:1337 2:1750 3:1480 4:1357 5:1438 6:1452 7:1490

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1846 1:1404 2:1704 3:1434 4:1403 5:1484 6:1385 7:1423

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1880 1:1438 2:1630 3:1360 4:1477 5:1558 6:1351 7:1389

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1870 1:1428 2:1549 3:1279 4:1558 5:1639 6:1361 7:1399

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1819 1:1377 2:1485 3:1215 4:1621 5:1702 6:1412 7:1450

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1742 1:1300 2:1458 3:1188 4:1649 5:1730 6:1489 7:1527

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1662 1:1220 2:1474 3:1204 4:1633 5:1714 6:1569 7:1607

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1603 1:1161 2:1530 3:1260 4:1577 5:1658 6:1628 7:1666

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1582 1:1140 2:1609 3:1339 4:1498 5:1579 6:1649 7:1687

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1604 1:1162 2:1687 3:1417 4:1420 5:1501 6:1627 7:1665

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1665 1:1223 2:1742 3:1472 4:1365 5:1446 6:1567 7:1605

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1745 1:1303 2:1757 3:1487 4:1350 5:1431 6:1486 7:1524

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1821 1:1379 2:1728 3:1458 4:1379 5:1460 6:1410 7:1448

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

0:1871 1:1429 2:1663 3:1393 4:1443 5:1524 6:1360 7:1398

The entire code is below. The relevant section begins after if (Afreq > 0) near the bottom. The positions that do not write correctly are pos[6] and pos[7]:

/*
This is a program that recieves sine wave parameters from a serial connection in a single string.
The string is parsed and converted into numbers, which are plugged into 8 sine equations to generate
positions for each joint of the robot.
*/

#include 

Servo SB, SF, HB, HF, EB, EF, KB, KF;
int iAmp, iF_offset, iB_offset, iFk_offset, iBk_offset, pos[] = {0,1,2,3,4,5,6,7};
unsigned int t = 0;
unsigned long previousMillis = 0;
float fKnee_amp, fAfreq, fPhase, fGait;

void setup(){
  //Start serial stream at 115200 baud
  Serial.begin(115200);

  //Connect servos to Arduino
  HB.attach(2);
  KF.attach(3);
  SB.attach(4); 
  EB.attach(10);
  HF.attach(6); 
  KB.attach(7);
  SF.attach(8); 
  EF.attach(11);

  delay(3000);
}

void loop(){
  char c, buffer[10];
  String readString, amp, knee_amp, afreq, gait, phase, f_offset, b_offset, fk_offset, bk_offset;
  unsigned int sine_time = 50;

  while (Serial.available() && readString.length() < 37){
    //delay to allow buffer to fill
    delayMicroseconds(100);
    if (Serial.available() > 0){
      //get one byte from serial buffer
      char c = Serial.read();
      //creates the string "readString"
      readString += c;
    }
  }

  if (readString.length() > 36){
    //see what was received
    Serial.println("The string is: " + readString);
    Serial.println(readString.length()); 

    //parse serial string into separate parameters
    amp = readString.substring(0, 3);
    knee_amp = readString.substring(3, 7); 
    afreq = readString.substring(7, 11); 
    phase = readString.substring(11, 15); 
    gait = readString.substring(15, 20); 
    f_offset = readString.substring(20, 24); 
    b_offset = readString.substring(24, 28); 
    fk_offset = readString.substring(28, 32); 
    bk_offset = readString.substring(32, 36);

    //convert string values to numbers
    iAmp = amp.toInt();

    knee_amp.toCharArray(buffer, 10);
    fKnee_amp = atof(buffer);
    memset(buffer, 0, sizeof(buffer));

    afreq.toCharArray(buffer, 10);
    fAfreq = atof(buffer);
    memset(buffer, 0, sizeof(buffer));

    phase.toCharArray(buffer, 10);
    fPhase = atof(buffer);
    memset(buffer, 0, sizeof(buffer));

    gait.toCharArray(buffer, 10);
    fGait = atof(buffer);
    memset(buffer, 0, sizeof(buffer));

    iF_offset = f_offset.toInt();
    iB_offset = b_offset.toInt();
    iFk_offset = fk_offset.toInt();
    iBk_offset = bk_offset.toInt();

    //clear strings
    readString="";
    amp="";
    knee_amp="";
    afreq="";
    gait="";
    phase="";
    f_offset="";
    b_offset="";
    fk_offset="";
    bk_offset="";
  }

  //if the joints should be oscillating
  if (fAfreq > 0){
    //determine joint positions
    pos[0] = iAmp * sin(fAfreq*t) + 1500 + iF_offset;
    pos[1] = iAmp * sin(fAfreq*t) + 1522 - iF_offset;
    pos[2] = iAmp * sin(fAfreq*t + fGait) + 1500 + iB_offset;
    pos[3] = iAmp * sin(fAfreq*t + fGait) + 1446 - iB_offset;

    pos[4] = iAmp*fKnee_amp * sin(fAfreq*t + fGait + fPhase) + 1500 + iFk_offset;
    pos[5] = iAmp*fKnee_amp * sin(fAfreq*t + fGait + fPhase) + 1581 - iFk_offset;
    pos[6] = iAmp*fKnee_amp * sin(fAfreq*t + fPhase) + 1500 + iBk_offset;
    pos[7] = iAmp*fKnee_amp * sin(fAfreq*t + fPhase) + 1538 - iBk_offset;

    //get time in milliseconds
    unsigned long currentMillis = millis();

    //if the time threshold has been passed
    if(currentMillis - previousMillis >= sine_time){
      previousMillis = currentMillis;

      //write servo positions

      SB.writeMicroseconds(pos[0]); 
      SF.writeMicroseconds(pos[1]); 
      HB.writeMicroseconds(pos[2]);
      HF.writeMicroseconds(pos[3]); 
      EB.writeMicroseconds(pos[4]);
      EF.writeMicroseconds(pos[5]);
      KF.writeMicroseconds(pos[6]);
      KB.writeMicroseconds(pos[7]);

      //increment time parameter
      t+=1;
    }
  }
}

私は次に何を試すべきか分からない。

0

1 答え

それが動作していない最後の2つであることを考えれば、おそらくあなたはタイミングの問題を抱えています。コンパイラが一般的なコードを検出するのがどれくらい良いか分かりませんが、それを簡単にしたいと思うかもしれませんし、一時的な変数を作ることでスピードアップすることもできます:

//determine joint positions
int pos01 = iAmp * sin(fAfreq*t);
pos[0] = pos01 + 1500 + iF_offset;
pos[1] = pos01 + 1522 - iF_offset;

int pos23 = iAmp * sin(fAfreq*t + fGait);
pos[2] = pos23 + 1500 + iB_offset;
pos[3] = pos23 + 1446 - iB_offset;

int pos45 = iAmp*fKnee_amp * sin(fAfreq*t + fGait + fPhase);
pos[4] =  pos45 + 1500 + iFk_offset;
pos[5] = pos45 + 1581 - iFk_offset;

int pos67 = iAmp*fKnee_amp * sin(fAfreq*t + fPhase);
pos[6] = pos67 + 1500 + iBk_offset;
pos[7] = pos67 + 1538 - iBk_offset;
1
追加された