custom scara on makelangelo

[ekaggrat kalsi]

hi,
I have communicated before on instagram under the name of mectolab. I am creating this topic for better documentation. So far I used your extension to the firmware with a scara extension. I changed the kinematics for 2 robots that i am building . One is a parallel scara and the other is a 5 bar scara . Currently i am testing the 5 bar scara. I compiled and uploaded the firmware for a atmega1284(sanguinololu) based board. But i am unable to communicate to the board. I have tested the same board with marlin and it works fine. Is there any setting I am missing?

configuration :

#pragma once
//------------------------------------------------------------------------------
// Makelangelo - firmware for various robot kinematic models
// [email protected] 2013-12-26
// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// CONSTANTS
//------------------------------------------------------------------------------

//#define VERBOSE           (1)  // add to get a lot more serial output.
//#define DEBUG_STEPPING         // uncomment to debug stepper internal timer

//------------------------------------------------------------------------------
// Robot styles supported
//------------------------------------------------------------------------------

#define POLARGRAPH       1  // polargraph like Makelangelo
#define TRADITIONALXY    3  // gantry 3 axis setup.
#define COREXY           2  // gantry CoreXY setup.
#define ZARPLOTTER       4  // 4 motor, x-shaped 2D motion
#define SKYCAM           5  // 4 motor, x-shaped 3D motion
#define DELTA            6  // 3 arm delta robot, rotary action.  untested.
#define STEWART          7  // 6 arm stewart platform, rotary action.  untested.
#define ARM3             8  // 3DOF palletizing robot arm.
#define SIXI             9  // 6DOF robot arm.
#define TRADITIONAL6    10  // 6 axis machine, no restrictions.
#define SCARA           11  // 2 axis SCARA.

// default value
#define MACHINE_STYLE POLARGRAPH

//------------------------------------------------------------------------------
// LCD panels supported
//------------------------------------------------------------------------------

#define LCD_NONE       0
#define LCD_IS_128X64  1  // reprapdiscount Full Graphic Smart LCD Controller
#define LCD_IS_SMART   2  // reprapdiscount Smart LCD Controller (including XXL model)

// default value
#define LCD_TYPE LCD_NONE

//------------------------------------------------------------------------------
// Microcontrollers supported
//------------------------------------------------------------------------------

#define BOARD_RUMBA        1  // Reprap discount Rumba board
#define BOARD_RAMPS        2  // Mega2560 + Ramps 1.4
#define BOARD_SANGUINOLULU 3  // Sanguinolulu
#define BOARD_TEENSYLU     4  // Teensylu
#define BOARD_WEMOS        5  // Wemos D1 R2 + CNC Shield v3 (see board_wemos.h)
#define BOARD_SIXI_MEGA    6  // Arduino Mega + custom shield for Sixi 2 robot
#define BOARD_CNCV3        7  // Mega2560 + CNC Shield v3

// default value
#define MOTHERBOARD BOARD_SANGUINOLULU

//------------------------------------------------------------------------------
// YOUR CHANGES GO HERE
//------------------------------------------------------------------------------

#if __has_include("local_config.h")
// Your local changes go here.
// Do not send your local_config.h in a pull request.  Thank you!
#include "local_config.h"
#endif

//------------------------------------------------------------------------------
// 
//------------------------------------------------------------------------------

#include "robot_polargraph.h"
#include "robot_traditionalxy.h"
#include "robot_corexy.h"
#include "robot_zarplotter.h"
#include "robot_skycam.h"
#include "robot_delta.h"
#include "robot_stewart.h"
#include "robot_arm3.h"
#include "robot_sixi.h"
#include "robot_traditional6.h"
#include "robot_scara.h"

#include "board_rumba.h"
#include "board_ramps.h"
#include "board_sanguinolulu.h"
#include "board_teensylu.h"
#include "board_wemos.h"
#include "board_sixi_mega.h"
#include "board_cncv3.h"

#include "clock.h"
#include "motor.h"
#include "parser.h"
#include "gripper_hande.h"

//------------------------------------------------------------------------------
// SANITY CHECKS
//------------------------------------------------------------------------------

#if NUM_MOTORS > MAX_MOTORS
#error "The number of motors needed is more than this board supports."
#endif
#if NUM_SERVOS > MAX_BOARD_SERVOS
#error "The number of servos needed is more than this board supports."
#endif

#if NUM_SERVOS + NUM_MOTORS != NUM_AXIES
// not always the case!  Skycam has more motors than axies.
//#error "NUM_SERVOS + NUM_MOTORS != NUM_AXIES"
#endif

//------------------------------------------------------------------------------
// STRUCTURES
//------------------------------------------------------------------------------

typedef struct {
  float limitMax;
  float limitMin;
  float pos;
  float homePos;
} Axis;

//------------------------------------------------------------------------------
// GLOBALS
//------------------------------------------------------------------------------

// eeprom values
extern int robot_uid;
extern float calibrateRight;
extern float calibrateLeft;

extern float feed_rate;
extern float acceleration;
extern float step_delay;
extern Axis axies[NUM_AXIES];

extern void pause(const long us);
extern void findStepDelay();

extern void IK(const float *const axies, long *motorStepArray);
extern int FK(long *motorStepArray, float *axies);

extern void robot_findHome();
extern void robot_setup();
extern void teleport(float *pos);
extern void lineSafe(float *pos, float new_feed_rate);
extern void arc(float cx, float cy, float *destination, char clockwise, float new_feed_rate);
extern void get_end_plus_offset(float *results);
extern void set_tool_offset(int toolID, float *pos);
extern void reportCalibration();
extern void meanwhile();
extern void setHome(float *pos);

scara code :

//------------------------------------------------------------------------------
// Makelangelo - firmware for various robot kinematic models
// [email protected] 2013-12-26
// Please see http://www.github.com/MarginallyClever/makelangeloFirmware for more information.
//------------------------------------------------------------------------------

#include "configure.h"
#include "robot_scara.h"

#if MACHINE_STYLE == SCARA

#include <Arduino.h>

// use law of cosines property to find one interior angle in a triangle.  c=arccos((aa+bb-cc)/(2ab)).
float lawOfCosines(float a, float b, float c) {
  float numerator = sq(a) + sq(b) - sq(c);
  float denominator = 2.0 * a * b;
  if (denominator == 0) return 0;
  return acos(numerator / denominator);
}
double rtod(double fradians)
{
  return (fradians * 180.0 / PI);
}

/**
   Inverse Kinematics turns XY coordinates into step counts from each motor
   @param x cartesian coordinate
   @param y cartesian coordinate
   @param motorStepArray a measure of each belt to that plotter position
*/
void IK(const float *const cartesian, long *motorStepArray) {
  // see https://appliedgo.net/roboticarm/
  float x = cartesian[0];
  float y = cartesian[1];
  float z = cartesian[2];

  //BICEP_LENGTH_MM and FOREARM_LENGTH_MM are defined in robot_scara.h.
  // TODO save the numbers in EEPROM so they can be tweaked without a recompile?

  // from cartesian x,y we can get c, the distance from origin to x,y.
  // use law of cosines to
  float c = sqrt(sq(x) + sq(y));
  // then law of cosines will give us the elbow angle
  float elbowAngle = lawOfCosines(BICEP_LENGTH_MM, FOREARM_LENGTH_MM, c);

  // shoulder angle is made of two parts: the interior corner inside the
  float shoulderAngle = atan2(y, x) + lawOfCosines(c, BICEP_LENGTH_MM, FOREARM_LENGTH_MM);

  // angles are in radians.  we need degrees
  //motorStepArray[0] = lround(shoulderAngle * TODEGREES / MICROSTEP_PER_DEGREE);
  //motorStepArray[1] = lround(elbowAngle * TODEGREES / MICROSTEP_PER_DEGREE);

  motorStepArray[NUM_MOTORS] = z;

  // my code

  float L1 = 80; // the length of the shoulder
  float L2 = 100; // the length of the elbow
  float L0 = 35; // gap between arms
  float A1 = acos((sq(L1) + (sq(L0 + x) + sq(y)) - sq(L2)) / ((2 * L1) * sqrt(sq(L0 + x)  + sq(y))));
  float A2 = acos((sq(L1) + (sq(L0 - x) + sq(y)) - sq(L2)) / ((2 * L1) * sqrt(sq(L0 - x)  + sq(y))));
  float BB1 = atan(y / (L0 + x));
  float BB2 = atan(y / (L0 - x));

  float theta1 = BB1 + A1;
  float theta2 = 3.141516 - BB2 - A2;

  double elbow_ang = theta1;
  double shoulder_ang = theta2;

  motorStepArray[0] = lround(shoulder_ang / MICROSTEP_PER_DEGREE) ;
  motorStepArray[1] = lround (elbow_ang / MICROSTEP_PER_DEGREE) ;
}

/**
   Forward Kinematics - turns step counts into XY coordinates
   @param motorStepArray a measure of each belt to that plotter position
   @param axies the resulting cartesian coordinate
   @return 0 if no problem, 1 on failure.
*/
int FK(long *motorStepArray, float *axies) {
  float a = motorStepArray[0] * MICROSTEP_PER_DEGREE * TORADIANS;
  float b = motorStepArray[1] * MICROSTEP_PER_DEGREE * TORADIANS;
    float L1 = 80; // the length of the shoulder
  float L2 = 100; // the length of the elbow
  float L0 = 35; // gap between arms

  
   float x1 = cos(a)*L1;
   float y1 = sin(a)*L1;
   float x2 = cos(b)*L1+(L0*2);
   float y2 = sin(b)*L1;
   float DD = sqrt(sq(x2-x1)+sq(y2-y1)); 
   float Lm = atan2((y2-y1),(x2-x1));
   float Mu = acos(DD/(2*L2));
   axies[0] = x1+(L2*cos(Lm+Mu));
   axies[1] = y1+(L2*sin(Lm+Mu));
   

  //axies[0] = cos(a) * BICEP_LENGTH_MM - cos(b) * FOREARM_LENGTH_MM;
  //axies[1] = sin(a) * BICEP_LENGTH_MM - sin(b) * FOREARM_LENGTH_MM;
  axies[2] = motorStepArray[NUM_MOTORS];
}

void robot_findHome() {
  wait_for_empty_segment_buffer();
  motor_engage();

  findStepDelay();

  Serial.println(F("Finding..."));

  uint8_t i, hits;
  // back up until all switches are hit
  do {
    hits = 0;
    // for each stepper,
    for (ALL_MOTORS(i)) {
      digitalWrite(motors[i].dir_pin, HIGH);
      // if this switch hasn't been hit yet
      if (digitalRead(motors[i].limit_switch_pin) == HIGH) {
        // move "down"
        Serial.print('|');
        digitalWrite(motors[i].step_pin, HIGH);
        digitalWrite(motors[i].step_pin, LOW);
      } else {
        ++hits;
        Serial.print('*');
      }
    }
    Serial.println();
    pause(step_delay);
  } while (hits < NUM_MOTORS);
  Serial.println(F("Found."));

  float zeros[2] = {0, 0};
  teleport(zeros);
}

void robot_setup() {}

#endif  // SCARA

instagram

thanks for your input!

[Dan]

Please create a new branch on the code and make a pull request. then I can see what you’re working with and run it for myself. I’d also like it if we can set up a discord meeting and see your machine via video. Our discord channel is https://discord.gg/HdB5QQ

[phearl3ss1]

I found this software for a scara today and am looking it over. It seems like it will have issues for a scara, but it won’t be difficult to find out. The issue I have noticed is the motors and steps per degree are assumed to be the same for both motors and for a scara the x motor also moves the Y motor and are different. The Sara h. file

#define BICEP_LENGTH_MM      (350.0)
#define FOREARM_LENGTH_MM    (287.81)

In the motors.h it states // choose one of the following

//#define NORMAL_MOTOR_STEPS   200  // 1.8 degrees per step
#define NORMAL_MOTOR_STEPS   400  // 0.9 degrees per step

whereas the firmware I am using now in the Config h. file:

#define XAXIS_STEPS_PER_UNIT 2170.0  //Cartesian Steps/mm, Delta abaikan, Scara Steps/degree
#define YAXIS_STEPS_PER_UNIT 1910.0 // Cartesian Steps/mm, Delta abaikan, Scara Steps/degree
#define ZAXIS_STEPS_PER_UNIT 2.00  // Cartesian Steps/mm, Delta Steps/mm, Scara Steps/mm
// Scara Setting
#define ARM_LENGTH 350.00                        // dalam mm. in mm
#define FOREARM_LENGTH 287.81              // dalam mm. in mm

Will it work without each motor having it’s own steps defined?

• This reply was modified 4 years, 1 month ago by Dan.
• This reply was modified 4 years, 1 month ago by Dan.

[Dan]

I think some work needs to be done to customize STEPS_PER_UNIT for each motor. I’ll open a ticket. https://github.com/MarginallyClever/Makelangelo-firmware/issues/101

[Author]

Posts