Knife Bot

Knife bot. Hide your children and wives and dogs.

It’s a hot glued arrangement of servos and home depot painter sticks (they’re free!)

We used python and numpy to solve for the needed angles as a function of the end position. We were having trouble with getting angles that are in the -np.pi/2 to np.pi/2 range until the suggestion of using [0,np.pi/2] as the initial position. In hindsight, a more robust method would be to make a loop attempting a set of reasonable starting positions, and accept them if they were attainable. Or to attempt a random set of initial conditions. Your choice. For only 2 variables, the computer will just chew that problem up. If you had 10 actuators, maybe you’d have more problems.

Turns out random angles mode was more fun though.

from scipy import optimize
import numpy as np
import serial

device = '/dev/cu.usbmodem1411'
L = 1.


def pos(theta, x):
    rho = L* (np.sin(theta[0]) + np.sin(theta[0] + theta[1]))
    z = L * (np.cos(theta[0]) + np.cos(theta[0] + theta[1]))
    return np.array([rho - x[0], z - x[1]])

def objective(theta,x):
    temp = pos(theta,x)
    return temp[0]**2+temp[1]**2




anglerange = (-np.pi/2,np.pi/2)
bnds = (anglerange,anglerange)

#cons = [{'type':'ineq', 'fun': con},
#        {'type':'eq', 'fun': con_real}]

def cleanAngle(angle):
    angle = (angle + np.pi) % (2 * np.pi) - np.pi
    return angle


def findAngles(x,y,z):

    rho = np.sqrt(x**2 + y**2)
    theta0 = np.arctan2(y,x)
    sol = optimize.root(lambda theta: pos(theta, [rho,z] ), [0,np.pi/2])
    #sol = optimize.minimize(lambda theta: objective(theta, [rho,z]), np.array([0,0]), method='SLSQP', bounds=bnds)
    print sol.success
    theta = sol.x
    theta = cleanAngle(theta)
    return [theta0, theta[0], theta[1]]


ser = serial.Serial(device, 19200, timeout=1)

import signal
import sys
def signal_handler(signal, frame):
        print('You pressed Ctrl+C!')
        ser.close()
        sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)

import time
'''
while True:
    x = float(input('x: '))
    y = float(input('y: '))
    z = float(input('z: '))
    theta = findAngles(x,y,z)
    print(theta)
    ser.write(str(theta[0])+ ',')
    ser.write(str(theta[1])+ ',')
    ser.write(str(theta[2])+ ';\r\n')
    '''
while True:
    theta = np.random.rand(3) * np.pi - np.pi/2
    ser.write(str(theta[0])+ ',')
    ser.write(str(theta[1])+ ',')
    ser.write(str(theta[2])+ ';\r\n')
    time.sleep(1)



ser.close()

This is the arduino code. Servos are on 9,10,11

We dialed in the Zero and Ration numbers by iteratively try to get angle 0 and 90 degrees manually over the serial monitor for each servo in turn.

The command strings is in the format

1.2,-.3,.4;

Comma separated angles for each motor in radians.

#include <Servo.h>
//#include <SoftwareSerial.h>
#define RATIO 425/(PI/4)
#define BRATIO -400/(PI/4)
#define CRATIO -550/(PI/4)
#define AZERO 1606
//#define AZERO 2020
#define BZERO 1645
#define CZERO 2560
#define D 200.

Servo servoA, servoB, servoC;  // create servo object to control a servo
String v = "";
int sign = 1;
float a = 0.;
float b = 0.;
float c = 0.;
float x_temp;
float y_temp;
float angles[3] = {0.,0.,0.};

//SoftwareSerial ser(2,3);

void setup()
{
  servoA.attach(9);
  servoB.attach(10);
  servoC.attach(11);  // attaches the servo on pin 9 to the servo object
  move(angles);
  Serial.begin(19200);
  //ser.begin(9600);
}


void loop() {
  // put your main code here, to run repeatedly:
  
  
  if (Serial.available()) {
    char ch = Serial.read();
    //Serial.write(ch);
    
    switch(ch) {
      case ';':
        getAngles(v,angles);
        for(int i = 0; i<3 ; i++){
        Serial.println(angles[i]);
        }
        move(angles);
        resetV();
      break;
      default:
        v += ch;
      break;
    }
  }
}

void resetV() {
  v = "";
}

void getAngles(String input, float *angles) {
  int comma_1 = 0;
  int comma_2 = 0;
  for (int i = 0; i < input.length(); i++) {
    if (input.substring(i, i+1) == ",") {
      if (comma_1 == 0) {
        comma_1 = i;
      } else {
        comma_2 = i;
      }
    }
  }
  angles[0] = input.substring(0, comma_1).toFloat();
  angles[1] = input.substring(comma_1+1, comma_2).toFloat();
  angles[2] = input.substring(comma_2+1, input.length()).toFloat();
}


void move(float* angles) {
  
  

      servoA.writeMicroseconds(floor(angles[0]*RATIO) + AZERO);
      servoB.writeMicroseconds(floor(angles[1]*BRATIO) + BZERO);
      servoC.writeMicroseconds(floor(angles[2]*CRATIO) + CZERO);
    
  }