Commit initial by Puma3d

README.md

@@ -1,6 +1,10 @@
 # community_robot_arm 
 **REPOSITORY OF COMMUNITY ROBOT ARM by 20sffactory**
 
+* arduino_firmware Version V0.62 (07Jun2021)
+  - Change position format in M114
+  - Fixed Issue of G28 Homing with endstop for to detect failure robot
+  - Change flow on homeSequence. Plus generality of different hardware
 * arduino_firmware Version V0.61 (03May2021)
   - Arduino Uno Controller Option
   - Fixed Issue of G28 Homing for Original Ftobler without endstop

arduino_firmware/robotArm_v0.62/RampsStepper.cpp

@@ -0,0 +1,84 @@
+#include "RampsStepper.h"
+#include "config.h"
+
+#include <Arduino.h>
+
+RampsStepper::RampsStepper(int aStepPin, int aDirPin, int aEnablePin, bool aInverse) {
+  setReductionRatio(MAIN_GEAR_TEETH / MOTOR_GEAR_TEETH, MICROSTEPS * STEPS_PER_REV);
+  stepPin = aStepPin;
+  dirPin = aDirPin;
+  enablePin = aEnablePin;
+  inverse = aInverse;
+  stepperStepPosition = 0;
+  stepperStepTargetPosition;
+  pinMode(stepPin, OUTPUT);
+  pinMode(dirPin, OUTPUT);
+  pinMode(enablePin, OUTPUT);
+  enable(false);
+}
+
+void RampsStepper::enable(bool value) {
+  digitalWrite(enablePin, !value);
+}
+
+bool RampsStepper::isOnPosition() const {
+  return stepperStepPosition == stepperStepTargetPosition;
+}
+
+int RampsStepper::getPosition() const {
+  return stepperStepPosition;
+}
+
+void RampsStepper::setPosition(int value) {
+  stepperStepPosition = value;
+  stepperStepTargetPosition = value;
+}
+
+void RampsStepper::stepToPosition(int value) {
+  stepperStepTargetPosition = value;
+}
+
+void RampsStepper::stepToPositionMM(float mm, float steps_per_mm) {
+  stepperStepTargetPosition = mm * steps_per_mm;
+}
+
+void RampsStepper::stepRelative(int value) {
+  value += stepperStepPosition;
+  stepToPosition(value);
+}
+
+float RampsStepper::getPositionRad() const {
+  return stepperStepPosition / radToStepFactor;
+}
+
+void RampsStepper::setPositionRad(float rad) {
+  setPosition(rad * radToStepFactor);
+}
+
+void RampsStepper::stepToPositionRad(float rad) {
+  stepperStepTargetPosition = rad * radToStepFactor;
+}
+
+void RampsStepper::stepRelativeRad(float rad) {
+  stepRelative(rad * radToStepFactor);
+}
+
+void RampsStepper::update() {   
+  while (stepperStepTargetPosition < stepperStepPosition) {  
+    digitalWrite(dirPin, !inverse);
+    digitalWrite(stepPin, HIGH);
+    digitalWrite(stepPin, LOW);
+    stepperStepPosition--;
+  }
+
+  while (stepperStepTargetPosition > stepperStepPosition) {    
+    digitalWrite(dirPin, inverse);
+    digitalWrite(stepPin, HIGH);
+    digitalWrite(stepPin, LOW);
+    stepperStepPosition++;
+  }
+}
+
+void RampsStepper::setReductionRatio(float gearRatio, int stepsPerRev) {
+  radToStepFactor = gearRatio * stepsPerRev / 2 / PI;
+};

arduino_firmware/robotArm_v0.62/RampsStepper.h

@@ -0,0 +1,33 @@
+#ifndef RAMPSSTEPPER_H_
+#define RAMPSSTEPPER_H_
+
+class RampsStepper {
+public:
+  RampsStepper(int aStepPin, int aDirPin, int aEnablePin, bool aInverse);
+  void enable(bool value = true);
+
+  bool isOnPosition() const;
+  int getPosition() const;
+  void setPosition(int value);
+  void stepToPosition(int value);
+  void stepToPositionMM(float mm, float steps_per_mm);
+  void stepRelative(int value);
+  float getPositionRad() const;
+  void setPositionRad(float rad);
+  void stepToPositionRad(float rad);
+  void stepRelativeRad(float rad);
+
+  void update();
+
+  void setReductionRatio(float gearRatio, int stepsPerRev);
+private:
+  int stepperStepTargetPosition;
+  int stepperStepPosition;
+  int stepPin;
+  int dirPin;
+  int enablePin;  
+  bool inverse;
+  float radToStepFactor;
+};
+
+#endif

arduino_firmware/robotArm_v0.62/byj_gripper.cpp

@@ -0,0 +1,106 @@
+#include "byj_gripper.h"
+#include <Arduino.h>
+
+BYJ_Gripper::BYJ_Gripper(int pin0, int pin1, int pin2, int pin3, int steps){
+  grip_steps = steps;
+  byj_pin_0 = pin0;
+  byj_pin_1 = pin1;
+  byj_pin_2 = pin2;
+  byj_pin_3 = pin3;
+  step_cycle = 0;
+  pinMode(byj_pin_0, OUTPUT);
+  pinMode(byj_pin_1, OUTPUT);
+  pinMode(byj_pin_2, OUTPUT);
+  pinMode(byj_pin_3, OUTPUT);
+}
+
+void BYJ_Gripper::cmdOn() {
+  direction = true;
+  for (int i = 1; i <= grip_steps; i++) {
+    moveSteps();
+    delay(1);
+  }
+}
+
+void BYJ_Gripper::cmdOff() {
+  direction = false;
+  for (int i = 1; i <= grip_steps; i++) {
+    moveSteps();
+    delay(1);
+  }
+}
+
+void BYJ_Gripper::setDirection(){
+  if (direction == true) {
+    step_cycle++;
+  }
+  if (direction == false) {
+    step_cycle--;
+  }
+  if (step_cycle > 7) {
+    step_cycle = 0;
+  }
+  if (step_cycle < 0) {
+    step_cycle = 7;
+  }
+}
+
+void BYJ_Gripper::moveSteps() {
+  switch (step_cycle) {
+  case 0:
+    digitalWrite(byj_pin_0, LOW);
+    digitalWrite(byj_pin_1, LOW);
+    digitalWrite(byj_pin_2, LOW);
+    digitalWrite(byj_pin_3, HIGH);
+    break;
+  case 1:
+    digitalWrite(byj_pin_0, LOW);
+    digitalWrite(byj_pin_1, LOW);
+    digitalWrite(byj_pin_2, HIGH);
+    digitalWrite(byj_pin_3, HIGH);
+    break;
+  case 2:
+    digitalWrite(byj_pin_0, LOW);
+    digitalWrite(byj_pin_1, LOW);
+    digitalWrite(byj_pin_2, HIGH);
+    digitalWrite(byj_pin_3, LOW);
+    break;
+  case 3:
+    digitalWrite(byj_pin_0, LOW);
+    digitalWrite(byj_pin_1, HIGH);
+    digitalWrite(byj_pin_2, HIGH);
+    digitalWrite(byj_pin_3, LOW);
+    break;
+  case 4:
+    digitalWrite(byj_pin_0, LOW);
+    digitalWrite(byj_pin_1, HIGH);
+    digitalWrite(byj_pin_2, LOW);
+    digitalWrite(byj_pin_3, LOW);
+    break;
+  case 5:
+    digitalWrite(byj_pin_0, HIGH);
+    digitalWrite(byj_pin_1, HIGH);
+    digitalWrite(byj_pin_2, LOW);
+    digitalWrite(byj_pin_3, LOW);
+    break;
+  case 6:
+    digitalWrite(byj_pin_0, HIGH);
+    digitalWrite(byj_pin_1, LOW);
+    digitalWrite(byj_pin_2, LOW);
+    digitalWrite(byj_pin_3, LOW);
+    break;
+  case 7:
+    digitalWrite(byj_pin_0, HIGH);
+    digitalWrite(byj_pin_1, LOW);
+    digitalWrite(byj_pin_2, LOW);
+    digitalWrite(byj_pin_3, HIGH);
+    break;
+  default:
+    digitalWrite(byj_pin_0, LOW);
+    digitalWrite(byj_pin_1, LOW);
+    digitalWrite(byj_pin_2, LOW);
+    digitalWrite(byj_pin_3, LOW);
+    break;
+  }
+  setDirection();
+}

arduino_firmware/robotArm_v0.62/byj_gripper.h

@@ -0,0 +1,21 @@
+#ifndef BYJ_GRIPPER_H_
+#define BYJ_GRIPPER_H_
+
+
+class BYJ_Gripper {
+public:
+  BYJ_Gripper(int pin0, int pin1, int pin2, int pin3, int steps);
+  void cmdOn();
+  void cmdOff();
+private:
+  bool direction;
+  void moveSteps();
+  void setDirection();
+  int byj_pin_0;
+  int byj_pin_1;
+  int byj_pin_2;
+  int byj_pin_3;
+  int grip_steps;
+  int step_cycle;
+};
+#endif

arduino_firmware/robotArm_v0.62/command.cpp

@@ -0,0 +1,132 @@
+#include "command.h"
+#include "logger.h"
+#include "config.h"
+#include <Arduino.h>
+
+Command::Command() {
+  //initialize Command to a zero-move value;
+  new_command.valueX = NAN; 
+  new_command.valueY = NAN;
+  new_command.valueZ = NAN;
+  new_command.valueF = 0;
+  new_command.valueE = NAN;
+  new_command.valueS = 0;
+  message = "";
+  isRelativeCoord = false;
+}
+
+bool Command::handleGcode() {
+  if (Serial.available()) {
+    char c = Serial.read();
+    if (c == '\n') {
+       return false; 
+    }
+    if (c == '\r') {
+       bool b = processMessage(message);
+       message = "";
+       return b;
+    } else {
+       message += c; 
+    }
+  }
+  return false;
+}
+
+bool Command::processMessage(String msg){
+
+  new_command.valueX = NAN; 
+  new_command.valueY = NAN;
+  new_command.valueZ = NAN;
+  new_command.valueE = NAN;
+  new_command.valueF = 0;
+  new_command.valueS = 0;  
+  msg.toUpperCase();
+  msg.replace(" ", "");
+  int active_index = 0;
+  new_command.id = msg[active_index];
+  if((new_command.id != 'G') && (new_command.id != 'M')){
+    printErr();
+    return false;
+  }
+
+  active_index++;
+  int temp_index = active_index;
+  while (temp_index<msg.length() && !isAlpha(msg[temp_index])){
+    temp_index++;
+  }
+  new_command.num = msg.substring(active_index, temp_index).toInt();
+  active_index = temp_index;
+  temp_index++;
+  while (temp_index<msg.length()){
+    while (!isAlpha(msg[temp_index]) || msg[temp_index]=='.'){
+      temp_index++;
+      if (temp_index == msg.length()){
+        break;
+      }
+    }
+    value_segment(msg.substring(active_index, temp_index));
+    active_index = temp_index;
+    temp_index++;
+  }
+  return true;
+}
+
+void Command::value_segment(String msg_segment){
+  float msg_value = msg_segment.substring(1).toFloat();
+  switch (msg_segment[0]){
+    case 'X': new_command.valueX = msg_value; break;
+    case 'Y': new_command.valueY = msg_value; break;
+    case 'Z': new_command.valueZ = msg_value; break;
+    case 'E': new_command.valueE = msg_value; break;
+    case 'F': new_command.valueF = msg_value; break;
+    case 'S': new_command.valueS = msg_value; break;
+  }
+}
+
+
+Cmd Command::getCmd() const {
+  return new_command; 
+}
+
+void Command::cmdGetPosition(Point pos, Point pos_offset, float highRad, float lowRad, float rotRad){
+  if(isRelativeCoord) {
+    Logger::logINFO("RELATIVE MODE");
+  } else {
+    Logger::logINFO("ABSOLUTE MODE");
+  }
+  Logger::logINFO("CURRENT POSITION: [X:"+String(pos.xmm - pos_offset.xmm)+" Y:"+String(pos.ymm - pos_offset.ymm)+" Z:"+String(pos.zmm - pos_offset.zmm)+" E:"+String(pos.emm - pos_offset.emm)+"]");
+  //Logger::logINFO("RADIANS: [HIGH:"+String(highRad)+" LOW:"+String(lowRad)+" ROT:"+String(rotRad));
+}
+
+void Command::cmdToRelative(){
+  isRelativeCoord = true;
+  Logger::logINFO("RELATIVE MODE ON");
+}
+
+void Command::cmdToAbsolute(){
+  isRelativeCoord = false;
+  Logger::logINFO("ABSOLUTE MODE ON");
+}
+
+void cmdMove(Cmd(&cmd), Point pos, Point pos_offset, bool isRelativeCoord){
+
+  if(isRelativeCoord == true){
+    cmd.valueX = isnan(cmd.valueX) ? pos.xmm : cmd.valueX + pos.xmm;
+    cmd.valueY = isnan(cmd.valueY) ? pos.ymm : cmd.valueY + pos.ymm;
+    cmd.valueZ = isnan(cmd.valueZ) ? pos.zmm : cmd.valueZ + pos.zmm;
+    cmd.valueE = isnan(cmd.valueE) ? pos.emm : cmd.valueE + pos.emm; 
+  } else {
+    cmd.valueX = isnan(cmd.valueX) ? pos.xmm : cmd.valueX + pos_offset.xmm;
+    cmd.valueY = isnan(cmd.valueY) ? pos.ymm : cmd.valueY + pos_offset.ymm;
+    cmd.valueZ = isnan(cmd.valueZ) ? pos.zmm : cmd.valueZ + pos_offset.zmm;
+    cmd.valueE = isnan(cmd.valueE) ? pos.emm : cmd.valueE + pos_offset.emm;
+  }
+}
+
+void cmdDwell(Cmd(&cmd)){
+  delay(int(cmd.valueS * 1000));
+}
+
+void printErr() {
+  Logger::logERROR("COMMAND NOT RECOGNIZED");
+}