Sophomores majoring in mechanics build a manipulator by hand -- full of compromise

What I have built is a three degree of freedom link manipulator , The reason for choosing to build this manipulator is that it is easy to operate , Easy to master , Easy to learn .

Here are some of the processes I'm learning to make this manipulator ：

Main parts

Main control panel ：arduino uno r3

Expansion board ：cnc shield v3 Engraving machine expansion board

42 Stepper motor ,SG90 The motor ,PVC plate ,A4988 Driver module ,HC-06 modular

Because I like to list one thing , So I made a mind map ：

And I also made a mathematical settlement on the coordinate self-determination ：

 

  Here's the code section ：

In the code section , Due to my weak programming ability , Part of the code and programming thinking have learned from many B standing UP Lord , And this article is only to record my learning process of manipulator .

Phone mode and keyboard mode code ：

/* The stepping mode of the motor must be in cnc Debugging on the extended version of the motor .
 *  Start the program , The program starts in keyboard control mode by default 
 *  In keyboard mode ,'m' You can switch to the phone mode with one touch .
 *  Mobile phone mode 
 *  In mobile mode ,'m' You can switch to keyboard mode with one key .
 */

#include <Servo.h>
#include <AccelStepper.h>
#include <SoftwareSerial.h>
SoftwareSerial BTserial(12, 13); //  establish SoftwareSerial object ,RX Pin 12(SpnEn) Pick up SpnDir, TX Pin 13(SpnDir) Pick up SpnEn
 
//  Define constants for motor control 
const int enablePin = 8;  //  Enable the control pin 
 
const int xdirPin = 5;     // x Direction control pin 
const int xstepPin = 2;    // x Step control pin 
const int ydirPin = 6;     // y Direction control pin 
const int ystepPin = 3;    // y Step control pin 
const int zdirPin = 7;     // z Direction control pin 
const int zstepPin = 4;    // z Step control pin 

const int clawMin = 144;    // Store motor limits 
const int clawMax = 180;



 
const int moveSteps = 5;    // The number of operating steps used by the stepper motor for operation ( Only available in phone mode )

char cmd;        // Motor command characters 
int data;        // Motor command parameters 
char stepperName;  // Control motor number 
int stepNumber;   // Running steps of stepping motor 
int clawstep;              // Target angle of gripper motor 
bool mode = 1;       //mode = 1:  Keyboard mode ,  mode = 0:  Mobile phone mode 

int DSD = 15; //Default Servo Delay ( Default motor motion delay time )
              // This variable is used to control the running speed of the motor . Increasing the value of this variable will 
              // Reduce the running speed of the motor to control the action speed of the manipulator .

int clawMoveStep = 2;  //  Every time you press the handle button , Steering gear movement （ Only available in phone mode ）

Servo claw ;                                         // Create servo motor object ——— Gripper motor 
AccelStepper base_stepper(1,xstepPin,xdirPin);       // Create stepper motor object ——— Base motor 
AccelStepper forearm_stepper(1,ystepPin,ydirPin);    // Create stepper motor object ——— Forearm motor 
AccelStepper upperarm_stepper(1,zstepPin,zdirPin);   // Create stepper motor object ——— Rear arm motor 

void setup() {
  pinMode(xstepPin,OUTPUT);     // Arduino control A4988x The step pin is in output mode 
  pinMode(xdirPin,OUTPUT);      // Arduino control A4988x The direction pin is in output mode 
  pinMode(ystepPin,OUTPUT);     // Arduino control A4988y The step pin is in output mode 
  pinMode(ydirPin,OUTPUT);      // Arduino control A4988y The direction pin is in output mode 
  pinMode(zstepPin,OUTPUT);     // Arduino control A4988z The step pin is in output mode 
  pinMode(zdirPin,OUTPUT);      // Arduino control A4988z The direction pin is in output mode 

  pinMode(enablePin,OUTPUT);   // Arduino control A4988 The enable pin is in output mode 
  digitalWrite(enablePin,LOW); //  Set the enable control pin to the low level so that 
                               //  The motor drive board enters the working state 
  claw.attach(11);      // claw  Servo actuator connection pin Z+   Steering gear code 'c'
  delay(200);          //  Stability wait 

  claw.write(90);  
  delay(10); 
  
  Serial.begin(9600);
  BTserial.begin(9600); // HC-06  Default baud rate  9600
  
  Serial.println(F("/\\\\\\\\\\\\\\\\\\"));     
  Serial.println(F("****The robotic arm serves you*****"));   
  Serial.println(F("/\\\\\\\\\\\\\\\\\\"));  
  Serial.println(F(""));  
  Serial.println(F("Please input motor command:")); 
 
  Serial.print("HC-06 DEMO/TEST  ");
  
  BTserial.print("AT");   // Settings can be entered here HC-06 Bluetooth module AT Instructions .
                          // this AT Instructions must be HC-06 Bluetooth status input not connected .



  base_stepper.setMaxSpeed(100);   //  Set the maximum motor speed to 500
  base_stepper.setAcceleration(50.0);      //  Initialize the motor speed to 0  setSpeed(0);
  
  forearm_stepper.setMaxSpeed(100);   
  forearm_stepper.setAcceleration(50.0);      

  upperarm_stepper.setMaxSpeed(100);
  upperarm_stepper.setAcceleration(50.0); 
claw.write(144);
base_stepper.setCurrentPosition(0);
forearm_stepper.setCurrentPosition(0);
upperarm_stepper.setCurrentPosition(0);
}

void loop() {
    if (Serial.available()>0) { //Serial.available()>0 //BTserial.available()>0
       cmd = Serial.read();
        Serial.print(cmd);
    if( mode == 1 ){
      keyboard_input(cmd);  // Keyboard control mode 

    } else {
      cellphone_input(cmd); // Mobile control mode 
    }  
  }


   base_stepper.run();     // The base motor runs 
   forearm_stepper.run();  // Forearm motor operation 
   upperarm_stepper.run(); // The rear arm motor operates 
}


//Arduino Perform the corresponding operation according to the serial instruction 
//
void keyboard_input(char cmd){
  // Determine whether the wrong instruction information is input due to the wrong mode （ Enter the phone mode information in keyboard mode ）
    if (   cmd == 'w' || cmd == 's' || cmd == 'a' || cmd == 'd' 
        || cmd == '5' || cmd == '4' || cmd == '6' || cmd == '8' ){
    Serial.println("Warning!!! Warning!!! Warning!!! Currently in the keyboard_input"); 
    delay(100);
    while(Serial.available()>0) char wrongCommand = Serial.read();  // Clear the wrong instruction of serial port cache 
    return;
  } 
  
if( cmd == 'b' || cmd == 'f' || cmd == 'u'){
  data = Serial.parseInt();
  RunCommand(cmd,data);
  }else{
    switch (cmd){
           case 'm' :// Switch to phone control mode 
           mode = 0; 
           Serial.println("Enter the cellphone control mode");
           break;
           case 'o' :// Reset the mechanical arm 
           reset_arm();
           Serial.println("Receive command");
           break;
           case 'c' :// Gripper motor operates 
           int servoData = Serial.parseInt();
           servoCmd(cmd, servoData, DSD);  //  Manipulator steering gear operation function （ Parameters ： Steering gear name , Target angle , Delay / Speed ）
           Serial.println("Receive the claw operation command");
           break;
           default :  // Unknown instruction feedback 
           Serial.println("Unknown Command.");
      } 
    }


  
  }
  void cellphone_input (char cmd ){
      // Determine whether humans enter the wrong command information due to the wrong mode （ Input keyboard mode information in mobile phone mode ）
  if (cmd == 'b' || cmd == 'f' || cmd == 'u' || cmd == 'v'){
    Serial.println("Warning!!! Warning!!! Warning!!! Currently in the cellphone_input");
    delay(100);
    while(Serial.available()>0) char wrongCommand = Serial.read();  // Clear the wrong instruction of serial port cache 
    return;
  } 
  int basestep;
  int forearmstep;
  int upperarmstep;
  //int clawstep
 switch(cmd){
        case 'a' ://base_stepper towards the left 
        basestep = base_stepper.currentPosition() - moveSteps;
        Serial.println("A run command was received");
        delay(500);
        base_stepper.moveTo(basestep);
        //CellphoneRunCommand('a',basestep);
        break;
        case 'd' ://base_stepper towards the right 
        basestep = base_stepper.currentPosition() + moveSteps;
        Serial.println("A run command was received");
        delay(500);
        base_stepper.moveTo(basestep);
        //CellphoneRunCommand('d',basestep);
        break;
        case 'w' ://upperarm_stepper Down 
        upperarmstep = upperarm_stepper.currentPosition() - moveSteps;
        Serial.println("A run command was received");
        delay(500);
        upperarm_stepper.moveTo(upperarmstep);
        //CellphoneRunCommand('w',upperarmstep);
        break;
        case 's' ://upperarm_stepper Up 
        upperarmstep = upperarm_stepper.currentPosition() + moveSteps;
        Serial.println("A run command was received");
        delay(500);
        upperarm_stepper.moveTo(upperarmstep);
        //CellphoneRunCommand('s',upperarmstep);
        break;
        case '8' ://forearm_stepper Down 
        forearmstep = forearm_stepper.currentPosition() - moveSteps;
        Serial.println("A run command was received");
        delay(500);
        forearm_stepper.moveTo(forearmstep);
        //CellphoneRunCommand('8',forearmstep);
        break;
        case '5' ://forearm_stepper Up 
        forearmstep = forearm_stepper.currentPosition() + moveSteps;
        Serial.println("A run command was received");
        delay(500);
        forearm_stepper.moveTo(forearmstep);
        //CellphoneRunCommand('5',forearmstep);       
        break;
        case '4' :// Clamping jaws open 
        Serial.println("Received Command: Claw Open Up");     
        clawstep = claw.read() - clawMoveStep;
        servoCmd('c', clawstep, DSD);
        break;
        case '6' :// Gripper closed 
        Serial.println("Received Command: Claw Close Down");        
        clawstep = claw.read() + clawMoveStep;
        servoCmd('c', clawstep, DSD);
        break;
        case 'm' :   // Switch to keyboard control mode  
        mode = 1; 
        Serial.println("Enter the keyboard control mode");
        break;
        case 'o' :// Reset the mechanical arm 
        reset_arm();
        Serial.println("Receive command");
        break;
           //default :  // Unknown instruction feedback 
          //Serial.println("Unknown Command.");       
  }
    }

  void RunCommand(char stepperName,int stepNumber){

    switch (stepperName){
           case 'b' :// The base motor runs , utilize move Function to make the motor run in corresponding steps （ Example ：b1024 -  Run the motor 1024 Step  ）
           Serial.println("A run command was received");
           //delay(500);
           base_stepper.move(stepNumber);
           break;
           case 'f' :
           Serial.println("A run command was received");
           //delay(500);
           forearm_stepper.move(stepNumber);
           break;
           case 'u' :
           Serial.println("A run command was received");
           //delay(500);
           upperarm_stepper.move(stepNumber);
           break;
           default:  // Unknown instruction feedback 
           Serial.println("Unknown Command.");  
      }

    }

  void reset_arm(){// Manipulator reset function . 
    /*runToNewPosition And moveTo The functions are basically the same . The only difference is runToNewPosition Function will “block” The program runs .
     *  That is, before the motor reaches the target position ,Arduino The following program contents will not be executed .
     */
    Serial.println("Receive command"); 
    base_stepper.runToNewPosition(0);
    forearm_stepper.runToNewPosition(0);
    upperarm_stepper.runToNewPosition(0);
    claw.write(145);
    delay(500);
    Serial.println("The reset command was completed");
    
    }
    void servoCmd(char servoName, int toPos, int servoDelay){
      int fromPos; // Build variables , Store the initial movement angle value of the motor 
      if(toPos >= clawMin && toPos <= clawMax){    
      fromPos = claw.read();  //  Gets the current motor angle value for “ Starting angle value of motor movement ”
      } else {
      Serial.println("+Warning: Claw Servo Value Out Of Limit!");
      return;        
      }
      // Command the gripper motor to run 
      if (fromPos <= toPos){  // If “ Starting angle value ” Less than “ Target angle value ”
      for (int i=fromPos; i<=toPos; i++){
      claw.write(i);
      delay (15);
       }
       }else { // otherwise “ Starting angle value ” Greater than “ Target angle value ”
          for (int i=fromPos; i>=toPos; i--){
          claw.write(i);
          delay (15);
          Serial.println("Receive command"); 
          }
          }
      }

Coordinate self searching code ： 

float basepos;
float forearmpos;
float upperarmpos;
float upperarmpos1;
float upperarmpos2;
float pi=3.14;


float touying;

const int basehigh=13;
int duanbian;
int xiebian;
int x;
float y;
float z;
#include <AccelStepper.h>
//  Define constants for motor control 
const int enablePin = 8;  //  Enable the control pin 
 
const int xdirPin = 5;     // x Direction control pin 
const int xstepPin = 2;    // x Step control pin 
const int ydirPin = 6;     // y Direction control pin 
const int ystepPin = 3;    // y Step control pin 
const int zdirPin = 7;     // z Direction control pin 
const int zstepPin = 4;    // z Step control pin 



const int Num=6;
const int upperarm=16;
const int forearm=14;


AccelStepper base_stepper(1,xstepPin,xdirPin);       // Create stepper motor object ——— Base motor 
AccelStepper forearm_stepper(1,ystepPin,ydirPin);    // Create stepper motor object ——— Forearm motor 
AccelStepper upperarm_stepper(1,zstepPin,zdirPin);   // Create stepper motor object ——— Rear arm motor 
void setup() {
  pinMode(xstepPin,OUTPUT);     // Arduino control A4988x The step pin is in output mode 
  pinMode(xdirPin,OUTPUT);      // Arduino control A4988x The direction pin is in output mode 
  pinMode(ystepPin,OUTPUT);     // Arduino control A4988y The step pin is in output mode 
  pinMode(ydirPin,OUTPUT);      // Arduino control A4988y The direction pin is in output mode 
  pinMode(zstepPin,OUTPUT);     // Arduino control A4988z The step pin is in output mode 
  pinMode(zdirPin,OUTPUT);      // Arduino control A4988z The direction pin is in output mode 

  pinMode(enablePin,OUTPUT);   // Arduino control A4988 The enable pin is in output mode 
  digitalWrite(enablePin,LOW); //  Set the enable control pin to the low level so that 
                               //  The motor drive board enters the working state 
Serial.begin(9600);
  Serial.println(F("/\\\\\\\\\\\\\\\\\\"));     
  Serial.println(F("****The robotic arm serves you*****"));   
  Serial.println(F("/\\\\\\\\\\\\\\\\\\"));  
  Serial.println(F(""));  
  Serial.println(F("Please input motor command:"));
    base_stepper.setMaxSpeed(100);   //  Set the maximum motor speed to 500
  base_stepper.setAcceleration(50.0);      //  Initialize the motor speed to 0  setSpeed(0);
  
  forearm_stepper.setMaxSpeed(100);   
  forearm_stepper.setAcceleration(50.0);      

  upperarm_stepper.setMaxSpeed(100);
  upperarm_stepper.setAcceleration(50.0); 

base_stepper.setCurrentPosition(0);
forearm_stepper.setCurrentPosition(0);
upperarm_stepper.setCurrentPosition(0);
}

void loop() {
  // put your main code here, to run repeatedly:
 if (Serial.available()>1){
   int data = Serial.parseInt();
   x=data/100;
   y=abs((data-x*100)/10);
   z=abs(data%10);
   

    Serial.println(x);
    Serial.println(y);
    Serial.println(z);
    calculate();
    
    Serial.print("basepos= ");Serial.println(basepos);
    Serial.print("upperarmpos= ");Serial.println(upperarmpos);
    Serial.print("forearmpos= ");Serial.println(forearmpos);
    RunCommand(basepos,upperarmpos,forearmpos);
   base_stepper.run();     // The base motor runs 
   forearm_stepper.run();  // The rear arm motor operates 
   upperarm_stepper.run(); // Forearm motor operation 
 }  
}
void calculate(){
basepos=atan(x/y)*(180/pi)/1.8;

// Bottom angle 
delay(500);
touying=sqrt(pow(x,2)+pow(y,2))-Num;

delay(500);
duanbian=abs(basehigh-z);
xiebian=sqrt(pow(duanbian,2)+pow(touying,2));
upperarmpos1=(acos((pow(upperarm,2)+pow(xiebian,2)-pow(forearm,2))/(2*upperarm*xiebian)))*(180/pi);
if(basehigh==z){
  upperarmpos2=0;
  }else if(basehigh>z){
    upperarmpos2=atan(touying/duanbian)* (180/pi)-90;
    }
    else if(basehigh<z){
      upperarmpos2=atan(duanbian/touying)-90;
      }
      upperarmpos=(upperarmpos1+upperarmpos2-15)/1.8;
// Forearm angle 


forearmpos=(acos((pow(upperarm,2)+pow(forearm,2)-pow(xiebian,2))/(2*upperarm*forearm))*(180/pi)+(upperarmpos-90))/1.8;
// Rear arm angle 

}
void RunCommand(int b,int u,int f){
      Serial.println("Receive command");
       Serial.println(b);
       Serial.println(u);
       Serial.println(f);
    
    upperarm_stepper.runToNewPosition(u);
    delay(100);
    forearm_stepper.runToNewPosition(f);
    delay(500);
    base_stepper.runToNewPosition(b);
    delay(500);
  }

As a mechanical student , I really don't want to spend too much time on code （ Crying and laughing ）, Not merging two codes , And my programming ability is very weak , There may be many errors in the code .

This article is for personal study only .