Previous posts:

This is [step 4] for the leg upgrade in my inspiRED humanoid bipedal robot project.

I used an Arduino compatible board for servo control, and Python in my laptop for high level control via serial communication.

Here is the sketch I use on my Arduino. It uses a library called varspeedservo to get speed of the servo controlled, which can’t be done by the official Arduino servo library.

#define USE_USBCON

#include <Arduino.h>
#include <VarSpeedServo.h>

VarSpeedServo leg_servo[6];
const int servo_port[6] = {9, 10, 13, 14, 15, 16};
const int servo_num = 6;

int servo_pos[6] = {90, 90, 90, 90, 90, 90};

int startbyte;
int servo;
int pos;
int spe;
int block;

void setup(){
    for(int i = 0 ; i < 6; i ++){
        leg_servo[i].attach(servo_port[i]);
        leg_servo[i].write(90);
    }

    Serial.begin(9600);
    pos = 10;
}

void serialListen(){
    if(Serial.available() > 2){
        //leg_servo[5].write(80);
        startbyte = Serial.parseInt();
        Serial.print("read ");
        Serial.println(startbyte);
        if(startbyte == 255){
            servo = Serial.parseInt();
            pos = Serial.parseInt();
            spe = Serial.parseInt();
            block = Serial.parseInt();
            if(Serial.read() == 'e'){
                leg_servo[servo].write(pos, spe, block);
            }
        }
    }
}

void loop(){
    serialListen();
/*
    leg_servo[2].write(130, 50, false);
    leg_servo[3].write(50, 50, false);
    leg_servo[4].write(130, 50, true);

    leg_servo[2].write(90, 50, false);
    leg_servo[3].write(90, 50, false);
    leg_servo[4].write(90, 50, true);
*/
    delay(10);

}

Here is the Python script. It creates a GUI so that I can drag the slide bar to get the servo controlled.

#!/usr/bin/python2

from Tkinter import *
import tkMessageBox
import serial

top = Tk()

try:
    robot_ser = serial.Serial('/dev/tty.usbmodem1421', 9600, timeout=1)
except:
    print "Cannot connect to the robot"
    tkMessageBox.showinfo("Error", "Cannot connect to the robot!")

scales = []
servo_index = ['servo 0', 'servo 1', 'servo 2', 'servo 3', 'servo 4', 'servo 5'];
servo_var = [0, 0, 0, 0, 0, 0];
r = 0
for servo_ind in servo_index:
    Label(text = servo_ind, width = 10).grid(row = r, column = 0)
    scale_one = Scale(servo_var[r], orient = HORIZONTAL, width = 10, to = 180);
    scale_one.set(90)
    scale_one.grid(row = r, column = 1)
    scales.append(scale_one)
    r = r + 1

gs = [0, 0, 0, 0, 0, 0]
def task():
    data_string = 'pos: '
    for i in range(6):
        gs[i] = scales[i].get()
        speed = 60
        block = 0
        command = str(255) + ',' + str(i) + ',' + str(gs[i]) + ',' + str(speed) + ',' + str(block) + 'e'
        robot_ser.write(command)
        data_string += ' ' + str(gs[i])
    print data_string
    #data_string += ' E'
    #robot_ser.write(data_string)

    try:
        robot_ser.inWaiting()
    except:
        print "Lost connection!"
    top.after(100, task)

top.after(100, task)
top.mainloop()