custom scara on makelangelo

Home Forum Everything Else custom scara on makelangelo

Tagged: 

Viewing 2 posts - 1 through 2 (of 2 total)
  • Author
    Posts
  • #28933
    Avatarekaggrat kalsi
    Participant

    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
    // dan@marginallycelver.com 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
    // dan@marginallycelver.com 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!

    #28936
    DanDan
    Keymaster

    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

Viewing 2 posts - 1 through 2 (of 2 total)
  • You must be logged in to reply to this topic.