このコードはシリアル接続から正弦波パラメータを受け取り、正弦波の位置を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;
}
}
}
私は次に何を試すべきか分からない。