First of all, you will need to plug all servos to their proper pin, in my case here they are (you can change this for your how setup):
void connectServos() {
//FR
Servos[0].attach(40); //coxa
Servos[1].attach(38); //femur
Servos[2].attach(36); //tibia
//FL
Servos[3].attach(42); //coxa
Servos[4].attach(44); //femur
Servos[5].attach(46); //tibia
//BR
Servos[6].attach(34); //coxa
Servos[7].attach(32); //femur
Servos[8].attach(30); //tibia
//BL
Servos[9].attach(48); //coxa
Servos[10].attach(50); //femur
Servos[11].attach(52); //tibia
}
As soon as the servos are corretly attached, what i made in order to put the servos on its correct position is just to dissasembly the legs, i mean, separate the servo output from the its horn, in order to let the servo move freely. This way you can power the servos, let them go to the zero position and the reassembly.
I wrote a small sketch in order to put the servos on their zero position, you can find it at servo_calibration.py file, this way you can attatch the servo very close to their zero position.
Then, a more precise calibration is needed, you will need to calibrate the offset of each servo, those offset are defined at angleToPulse.py file. This file only contains the line defining the pulse corresponding to each angle for every servo.
Being a line in the form: y = Ax + B. The offset of each servo is defined by B, which mean the pulse at the zero angle.
If you are using different servos, the slope of the line (A) will be different so you will need to identify it. What i did, is just to take 4-5 meassurement of pulse at different angles and put them on an excel where i calculate the adjucted line from those points.
def convert(FR_angles, FL_angles, BR_angles, BL_angles):
pulse = np.empty([12])
#FR
pulse[0] = int(-10.822 * np.rad2deg(-FR_angles[0])) + 950
pulse[1] = int(-10.822 * np.rad2deg(FR_angles[1])) + 2280
pulse[2] = int(10.822 * (np.rad2deg(FR_angles[2]) + 90)) + 1000
#FL
pulse[3] = int(10.822 * np.rad2deg(FL_angles[0])) + 1020
pulse[4] = int(10.822 * np.rad2deg(FL_angles[1])) + 570
pulse[5] = int(-10.822 * (np.rad2deg(FL_angles[2]) + 90)) + 1150
#BR
pulse[6] = int(10.822 * np.rad2deg(-BR_angles[0])) + 1060
pulse[7] = int(-10.822 * np.rad2deg(BR_angles[1])) + 2335
pulse[8] = int(10.822 * (np.rad2deg(BR_angles[2]) + 90)) + 1200
#BL
pulse[9] = int(-10.822 * np.rad2deg(BL_angles[0])) + 890
pulse[10] = int(10.822 * np.rad2deg(BL_angles[1])) + 710
pulse[11] = int(-10.822 * (np.rad2deg(BL_angles[2]) + 90)) + 1050
return pulse
Discussions
Become a Hackaday.io Member
Create an account to leave a comment. Already have an account? Log In.