0

This code receives sine wave parameters from a serial connection, then formats and writes the sine wave positions to 8 servo motors.

The positions are successfully written to 6 of the 8 motors.

Of the two that don't work, I tried changing the motors to see if they were faulty; they are not. I changed the output pins to see if they were damaged; they are not.

I changed the order the equations are solved, and the positions are written to ensure there wasn't some bizarre timing issue. I saved a full cycle of the written positions and they output the correct numbers (i.e. the correct amplitude). You can see them here (format is "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.h>

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;
    }
  }
}

I don't know what to try next.

1

Given that it's the last two that aren't working, perhaps you've got a timing problem. Now I don't know how good the compiler is about detecting common code, but you might want to make it easier for it, and perhaps speed things up by making some temporary variables:

//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;

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.