Again, I have not touched this version of the code base in maybe 2+ years, so I am rusty on it!.
However I do think that version has a version of my debug terminal, so if your botboarduino is plugged into the USB port and you open a Serial monitor on the appropriate com port and you configure that window for I believe 57600 and configure it to send NL&CR, if you hit the enter key, the terminal may show you a set of cryptic commands.
One of the commands is one for updating servos offsets. I personally use this command for two reasons. The first reason is to help me debug stuff. When you enter this command, it will move all of the servos to their zero points. That is all of the servos should be at their center position, where the coxas should be aligned, either with the legs parallel with each other or in some case all pointing out from the center. The Femurs should be pointing straight out parallel to the ground and the Tibias should be pointing down (right angle)… If not close, either wrong servos on wrong pins, wrong servo horn positions, wrong offsets.
In this command, you step through the different servos (have robot up on some form of stand). You can step through each servo by hitting the * key. Note in arduino debug monitor nothing is sent until CR, so if using the debug monitor enter *. The code will will logically go to the next logical servo, it will first wiggle it and display a name for it. This helps to make sure that you can verify that each servo is responding and the right servo is connected to the right SSC-32 IO pin…
This has found several issues for me over the years.
Note: your update for legindex < -3 is probably not correct as either none or all of them will meet this criteria. That is the legindex is in the range 0-5, so if it is signed math none of them will less than -3. If it is unsigned, then the -3 may be interpreted as a large number and all will be…
What I was suggesting you may need to play with, is in the code, that is something like:
if (LegIndex < 3) {
wCoxaSSCV = ((long)(-sCoxaAngle1 +900))*1000/cPwmDiv+cPFConst;
wFemurSSCV = ((long)(-sFemurAngle1+900))*1000/cPwmDiv+cPFConst;
wTibiaSSCV = ((long)(-sTibiaAngle1+900))*1000/cPwmDiv+cPFConst;
} else {
wCoxaSSCV = ((long)(sCoxaAngle1 +900))*1000/cPwmDiv+cPFConst;
wFemurSSCV = ((long)((long)(sFemurAngle1+900))*1000/cPwmDiv+cPFConst);
wTibiaSSCV = ((long)(sTibiaAngle1+900))*1000/cPwmDiv+cPFConst;
}
Note: I removed the Tars from this as you dont have.
When LegIndex < 3 we are processing the right legs. when we are in the else clause we are in the left legs processing.
The only differences here in these sections is in some cases we negate the main values. That is for right you see: wCoxaSSCV = ((long)(-sCoxaAngle1 …
and for left you see: wCoxaSSCV = ((long)(sCoxaAngle1 …
notice the -sCoxaAngle1 in the right hand case and just sCoxaAngle1 in the left. So for example if the coxas (horizontal hip) is not going the right direction on one side or the other, you may need to either add or remove the - for that type of servo. Likewise for Femurs (Hip Vertical) and likewise for Tibia (Knee).
Also in cases like this pictures help. Example show a picture when the start button is pressed, showing where all the servos end up…
Kurt
