Python Gui V2: Real Time Control of the Arm

The code below gives real time control of the arm using scale bars in a python gui. There is a slight lag in control due to the speed of the USB connection but it has proven to reliable.

Arduino Code

#include <Servo.h>  //arduino library
#include <math.h>   //standard c library

#define PI 3.141

Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo gripperServo;

int command;

struct jointAngle{
  int base;
  int shoulder;
  int elbow;

};

int desiredGrip;
int gripperPos;

int servoSpeed = 15;

struct jointAngle desiredAngle; //desired angles of the servos

//+++++++++++++++FUNCTION DECLARATIONS+++++++++++++++++++++++++++

void servoControl (int thePos, int theSpeed, Servo theServo);

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void setup()
{
  Serial.begin(9600);
  baseServo.attach(9);        // attaches the servo on pin 9 to the servo object
  shoulderServo.attach(10);
  elbowServo.attach(11);
  gripperServo.attach(6);
 
  Serial.setTimeout(50);      //ensures the the arduino does not read serial for too long
  Serial.println("started");
  baseServo.write(90);        //intial positions of servos
  shoulderServo.write(150);
  elbowServo.write(110);
}

//primary arduino loop
void loop()
{
  while (Serial.available()){

    desiredAngle.base = Serial.parseInt();
    desiredAngle.shoulder = Serial.parseInt();
    desiredAngle.elbow = Serial.parseInt();
    desiredGrip = Serial.parseInt();

    if(Serial.read() == 'd'){               // if the last byte is 'd' then stop reading and execute command 'd' stands for 'done'
        Serial.println('d');
        Serial.flush();                     //clear all other commands piled in the buffer
             
    }
 
    //move the servo to the desired position
    servoControl(desiredAngle.base, servoSpeed, baseServo);
    servoControl(desiredAngle.shoulder, servoSpeed, shoulderServo);
    servoControl(desiredAngle.elbow, servoSpeed, elbowServo);    
    servoControl(desiredGrip, servoSpeed, gripperServo);    

  } // end of serial while
 
}

//++++++++++++++++++++FUNCTION DEFITNITIONS++++++++++++++++++++++++

void servoControl (int thePos, int theSpeed, Servo theServo){
  //thePos is the desired position the servo should be driven to
  // theSpeed is the delay between each increment of the servo position in milliseconds
  // theServo is the servo object that is to be controled.
 
  Serial.println("in servoControl");

  int startPos = theServo.read();        //read the current pos
  int newPos = startPos;
 
  //define where the pos is with respect to the command
  // if the current position is less that the actual move up
  if (startPos < thePos){
        while (newPos < (thePos - 5)){
          newPos = newPos + 1;
          theServo.write(newPos);
          delay(theSpeed);
        }  
  }

  else{
       while (newPos > (thePos + 5)){
         newPos = newPos - 1;
         theServo.write(newPos);
         delay(theSpeed);
       }    
  }
} // end of function



Python Code

from Tkinter import *
import tkMessageBox
import time
import serial

#+++++++++++++Global Variables+++++++++++++++++++++

ser = serial.Serial('/dev/ttyACM0', 9600)

#++++++++++++++++Functions+++++++++++++++++++++++

def move_it(aCommand):
    #this function sends the command of joint angles to the arduino to move the servos to the desired positions
   
    aCommand = 0  #unused holder to allow function to work live with scale bar

    ser.flushInput()
    ser.flushOutput()
    command = str(base.get()) + ',' + str(shoulder.get()) +   ',' + str(elbow.get())+','+ str(gripper.get()) + 'd'
    print command
    ser.write(command)

    #wait until a repsonse if found from the arduino
    OK = 'no'
    while (OK != 'd'):
        OK = ser.read(1)
   
#++++++++++++++++++++The GUI++++++++++++++++++++++
root = Tk()

#++++++++++++++++++++Drive Motors++++++++++++++++++

motorControl = Frame(root)
motorControl.pack()

forwardFrame = Frame(motorControl)
forwardFrame.pack()

backFrame = Frame(motorControl)
backFrame.pack (side = BOTTOM)

speedControl = Frame(root)
speedControl.pack()

#+++++++++++++++++ARM+++++++++++++++++++++++++
# The scroll bars
armControl = Frame(root)
armControl.pack( )

armLabel = Label(armControl, text = "Arm Componets", bg = "red", padx = 100)
armLabel.pack()

#++++++++++++++++++++++++BASE+++++++++++++++++++++++++++

baseLabel = Label(armControl, text = "Base", bg = "green", padx = 100)
baseLabel.pack()
base = Scale(armControl, from_= 20, to = 175, length = 300, orient = HORIZONTAL, command = move_it)
base.pack()

#++++++++++++++++++++++++Shoulder+++++++++++++++++++++++++

shoulderLabel = Label(armControl, text = "shoulder", bg = "green", padx = 100)
shoulderLabel.pack()
shoulder = Scale(armControl, from_= 50, to = 180, length = 300, orient = HORIZONTAL, command = move_it)
shoulder.pack()

#++++++++++++++++++++++ELBOW++++++++++++++++++++++++++++

elbowLabel = Label(armControl, text = "elbow", bg = "green", padx = 100)
elbowLabel.pack()
elbow = Scale(armControl, from_= 5, to = 175, length = 300, orient = HORIZONTAL, command = move_it)
elbow.pack()

#++++++++++++++++++++++++++++Gripper+++++++++++++++++++

gripperLabel = Label(armControl, text = "gripper", bg = "green", padx = 100)
gripperLabel.pack()

gripper = Scale(armControl, from_= 5, to = 175, length = 300, orient = HORIZONTAL, command = move_it)
gripper.pack()

#+++++++++++++++++++++++++++Primaryu Loop+++++++++++++++++

root.mainloop()