smoothie rostock random scribble

rostock.scad

$fa = 0.01;
$fs = 2;

arm_radius = 50;
effector_radius = 10;
effector_length = 10;

arm_length = 70;

// here is where we describe the path that we want the effector to follow
// position = [sin($t * 720) * 25 * $t, cos($t * 720) * 25 * $t, $t * 0]; // draws a spiral
position = [sin($t * 1440) * 25 * $t, cos($t * 1440) * 25 * $t, $t * 0];

function lensq(a) = pow(a[0],2) + pow(a[1], 2) + pow(a[2], 2);
function len(a) = sqrt(lensq(a));

function hypot(a, b) = sqrt((a * a) + (b * b));

function rot(p, a) = [ cos(a) * p[0] - sin(a) * p[1], sin(a) * p[0] + cos(a) * p[1], p[2] ];
function trans(a, b) = [a[0] + b[0], a[1] + b[1], a[2] + b[2]];

tower_radius = arm_radius - effector_radius;

// echo(str("tower_radius is ", tower_radius));

arm_length_squared = pow(arm_length,2);

// this is where we convert effector position to actuator position

/*
actuator_position = [	sqrt(pow(arm_length,2) - lensq(position - rot([tower_radius, 0, position[2]],   0))) + position[2],
			sqrt(pow(arm_length,2) - lensq(position - rot([tower_radius, 0, position[2]],120))) + position[2],
			sqrt(pow(arm_length,2) - lensq(position - rot([tower_radius, 0, position[2]],240))) + position[2]
			];
*/

actuator_position = [	sqrt(arm_length_squared - pow(tower_radius - rot(position,    0)[0],2) - pow(rot(position,    0)[1],2)) + position[2],
			sqrt(arm_length_squared - pow(tower_radius - rot(position, 240)[0],2) - pow(rot(position, 240)[1],2)) + position[2],
			sqrt(arm_length_squared - pow(tower_radius - rot(position, 120)[0],2) - pow(rot(position, 120)[1],2)) + position[2]
			];

echo(str("to get to ", position));
echo(str("actuators are at ", actuator_position));

module tower() {
	linear_extrude(convexity=5, height=200) translate([10, 0]) square(10, center=true);
}

module carriage() {
	linear_extrude(convexity=5, height=15) translate([5, 0]) square([10,15], center=true);
}

color([0.8, 0.5, 0.5, 0.4]) cylinder(r=arm_radius + effector_radius, h=0.1);

translate([0, 0, effector_length]) {
	#hull() {
		for (i=[0:2]) {
			translate(position) rotate([0, 0, 120 * i]) translate([effector_radius, 0, 0]) sphere(2);
		}
		#translate(position - [0, 0, effector_length]) sphere(r=0.01);
	}

	for (i=[0:2]) {
		rotate([0, 0, 120 * i]) translate([arm_radius, 0, 0]) {
			translate([0, 0, -effector_length]) tower();
			translate([0, 0, actuator_position[i]]) carriage();
		}
		hull() {
			rotate([0, 0, 120 * i]) translate([arm_radius, 0, actuator_position[i]]) sphere(2);
			translate(position) rotate([0, 0, 120 * i]) translate([effector_radius, 0, 0]) sphere(2);
		}
		#translate(
			rot([arm_radius, 0, actuator_position[i]],120 * i)
		) sphere(3);
		#translate(
			trans(position, rot([effector_radius, 0, 0], 120 * i))
		) sphere(3);
		echo(str("arm ", i, " length is ",len(
			rot([arm_radius, 0, actuator_position[i]],120 * i) -
			trans(position, rot([effector_radius, 0, 0], 120 * i))
		), ", should be ", arm_length));

		color([0.5, 0.5, 0.5, 0.2]) rotate([0, 0, 120 * i]) translate([arm_radius, 0, 0]) cylinder(r=len(position - rot([tower_radius, 0, position[2]], 120 * i)), h = 1);
	}
}

RostockSolution.cpp

#include "RostockSolution.h"
#include <math.h>

#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2

RostockSolution::RostockSolution(Config* passed_config) : config(passed_config){
    alpha_steps_per_mm = passed_config->value(alpha_steps_per_mm_checksum)->by_default(80)->as_number();
    beta_steps_per_mm  = passed_config->value( beta_steps_per_mm_checksum)->by_default(80)->as_number();
    gamma_steps_per_mm = passed_config->value(gamma_steps_per_mm_checksum)->by_default(80)->as_number();

    arm_length         = passed_config->value(rostock_arm_length_checksum)->by_default(20)->as_number();

    precalc_arm_length_squared = pow(arm_length,2);

    arm_radius         = passed_config->value(rostock_arm_radius_checksum     )->by_default(20)->as_number();
    platform_radius    = passed_config->value(rostock_platform_radius_checksum)->by_default(4 )->as_number();

    precalc_tower_radius       = arm_radius - platform_radius;
}

void RostockSolution::millimeters_to_steps( double millimeters[], int steps[] ){
    double rotated[3];

    steps[ALPHA_STEPPER] = solve_arm(millimeters, precalc_arm_length_squared, precalc_tower_radius ) * alpha_steps_per_mm;

    rotate_240(millimeters, rotated); // we go "backwards" because we're rotating the target position instead of the towers
    steps[BETA_STEPPER ] = solve_arm(rotated    , precalc_arm_length_squared, precalc_tower_radius ) *  beta_steps_per_mm;

    rotate_120(millimeters, rotated); // we go "backwards" because we're rotating the target position instead of the towers
    steps[GAMMA_STEPPER] = solve_arm(rotated    , precalc_arm_length_squared, precalc_tower_radius ) * gamma_steps_per_mm;
}

double RostockSolution::solve_arm( double millimeters[], double arm_length_squared, double tower_radius )
{
    return sqrt(arm_length_squared - pow(millimeters[X_AXIS] - precalc_tower_radius, 2) - pow(millimeters[Y_AXIS], 2)) + millimeters[Z_AXIS];
}

#define SIN60 0.8660254037844386
#define COS60 0.5

#define SIN120  SIN60
#define COS120 -COS60

#define SIN240 -SIN60
#define COS240 -COS60

void RostockSolution::rotate_120( double coords[], double out[] ){
    out[X_AXIS] = COS120 * coords[X_AXIS] - SIN120 * coords[Y_AXIS];
    out[Y_AXIS] = SIN120 * coords[X_AXIS] + COS120 * coords[Y_AXIS];
    out[Z_AXIS] = coords[Z_AXIS];
}

void RostockSolution::rotate_240( double coords[], double out[] ){
    out[X_AXIS] = COS240 * coords[X_AXIS] - SIN240 * coords[Y_AXIS];
    out[Y_AXIS] = SIN240 * coords[X_AXIS] + COS240 * coords[Y_AXIS];
    out[Z_AXIS] = coords[Z_AXIS];
}



void RostockSolution::set_steps_per_millimeter( double steps[] )
{
    alpha_steps_per_mm = steps[0];
    beta_steps_per_mm  = steps[1];
    gamma_steps_per_mm = steps[2];
}

void RostockSolution::get_steps_per_millimeter( double steps[] )
{
    steps[0] = alpha_steps_per_mm;
    steps[1] = beta_steps_per_mm;
    steps[2] = gamma_steps_per_mm;
}

RostockSolution.h

#ifndef ROSTOCKSOLUTION_H
#define ROSTOCKSOLUTION_H
#include "libs/Module.h"
#include "libs/Kernel.h"
#include "BaseSolution.h"
#include "libs/nuts_bolts.h"

#include "libs/Config.h"

#define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
#define beta_steps_per_mm_checksum  CHECKSUM("beta_steps_per_mm")
#define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")

#define rostock_arm_length_checksum         CHECKSUM("arm_length")
#define rostock_arm_radius_checksum         CHECKSUM("arm_radius")
#define rostock_carriage_radius_checksum    CHECKSUM("carriage_radius")

class RostockSolution : public BaseSolution {
    public:
        RostockSolution(Config* passed_config);
        void millimeters_to_steps( double millimeters[], int steps[] );
//         void steps_to_millimeters( int steps[], double millimeters[] );

        void set_steps_per_millimeter( double steps[] );
        void get_steps_per_millimeter( double steps[] );

        double solve_arm( double millimeters[], double arm_length, double tower_radius );
        void rotate_120( double coords[], double out[] );
        void rotate_240( double coords[], double out[] );

        Config* config;
        double alpha_steps_per_mm;
        double beta_steps_per_mm;
        double gamma_steps_per_mm;

        double arm_length;
        double arm_radius;
        double carriage_radius;

        double precalc_arm_length_squared;
        double precalc_tower_radius;
};






#endif // ROSTOCKSOLUTION_H

ported this so I can use it with minor modifications in logxgens branch, see the branch delta-triffid-solution in [email protected]:wolfmanjm/Smoothie.git