Lab_interaccio/2022/Robot_billete_v2/Robot_billete_v2.ino

#include "BraccioV2.h"
Braccio arm;
/*
 Basic_Movement.ino - version 0.1
 Written by Lukas Severinghaus
 Demonstrates basic movement of the arm using single joint absolute
 and relative positioning. Also shows the use of safe delay with
 and without custom intervals.

 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.
 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

//#define A
#ifdef A
  #define error 2 //A
#else
  #define error 4 //B
#endif

//Set these values from the min/max gripper joint values below.
#define GRIPPER_CLOSED 85
#define GRIPPER_OPENED 20
#define mode 2

void setup() {
  Serial.begin(9600);
  Serial.println("Initializing... Please Wait");//Start of initialization, see note below regarding begin method.
  pinMode(A0, INPUT_PULLUP);
  //Update these lines with the calibration code outputted by the calibration program.
  arm.setJointCenter(BASE_ROT, 0);
  arm.setJointCenter(SHOULDER, 125);
  arm.setJointCenter(ELBOW, 0 + error);
  arm.setJointCenter(WRIST, 71 + error);
  #if mode==1
    arm.setJointCenter(WRIST_ROT, 0);
  #else
    arm.setJointCenter(WRIST_ROT, 100);
  #endif
  arm.setJointCenter(GRIPPER, 80);//Rough center of gripper, default opening position

  //Set max/min values for joints as needed. Default is min: 0, max: 180
  //The only two joints that should need this set are gripper and shoulder.
  arm.setJointMax(GRIPPER, 100);//Gripper closed, can go further, but risks damage to servos
  arm.setJointMin(GRIPPER, 15);//Gripper open, can't open further

  //There are two ways to start the arm:
  //1. Start to default position.
  arm.begin(true);// Start to default vertical position.
  //This method moves the arm to the values specified by setJointCenter
  //and by default will make the arm be roughly straight up.

  //2. Start to custom position.
  //arm.begin(false);
  //arm.setAllNow(base_rot_val, shoulder_val, elbow_val, wrist_val, wrist_rot_val, gripper_val);
  //This method allows a custom start position to be set, but the setAllNow method MUST be run
  //immediately after the begin method and before any other movement commands are issued.


  //NOTE: The begin method takes approximately 8 seconds to start, due to the time required
  //to initialize the power circuitry.
  Serial.println("Initialization Complete");
}

unsigned long time_protec = millis();
unsigned long time_pinza = millis();
int flag_stop = false;

void loop() {
  if(!digitalRead(A0))
    {
      if ((millis()-time_protec)>500)
        {
          flag_stop = false;
          //#if mode==1
          for(int i=0; i<24; i++)
            {
              #ifdef A
                arm.setOneAbsolute(WRIST_ROT, 0);
              #else
                arm.setOneAbsolute(WRIST_ROT, 0);
              #endif
              arm.safeDelay(700, 1);
              #ifdef A
                arm.setOneAbsolute(WRIST_ROT, 180);
              #else
                arm.setOneAbsolute(WRIST_ROT, 180);
              #endif
              arm.safeDelay(700, 1); 
            }
          //#else
          arm.setOneAbsolute(BASE_ROT, 0);
          arm.setOneAbsolute(SHOULDER, 125);
          arm.setOneAbsolute(ELBOW, 0 + error);
          arm.setOneAbsolute(WRIST, 71 + error);
          arm.setOneAbsolute(WRIST_ROT, 100);
          arm.setOneAbsolute(GRIPPER, 80);//Rough center of gripper, default opening position
          arm.safeDelay(1000, 10);
          /*for(int i=0; i<4; i++)
            {
              #ifdef A
                arm.setOneAbsolute(WRIST, 71 + error);
                arm.setOneAbsolute(ELBOW, 0 + error);
                arm.setOneAbsolute(SHOULDER, 125);
              #else
                arm.setOneAbsolute(WRIST, 71 + error);
                arm.setOneAbsolute(ELBOW, 0 + error);
                arm.setOneAbsolute(SHOULDER, 125);
              #endif
              arm.safeDelay(500, 2);
              #ifdef A
                arm.setOneAbsolute(SHOULDER, 100);
                arm.setOneAbsolute(ELBOW, 25 + error);
                arm.setOneAbsolute(WRIST, 71 + error - 5);
              #else
                arm.setOneAbsolute(SHOULDER, 100);
                arm.setOneAbsolute(ELBOW, 25 + error);
                arm.setOneAbsolute(WRIST, 71 + error - 5);
              #endif
              arm.safeDelay(500, 2);
            }
          arm.setOneAbsolute(BASE_ROT, 0);
          arm.setOneAbsolute(SHOULDER, 125);
          arm.setOneAbsolute(ELBOW, 0 + error);
          arm.setOneAbsolute(WRIST, 71 + error);
          #if mode==1
            arm.setOneAbsolute(WRIST_ROT, 0);
          #else
            arm.setOneAbsolute(WRIST_ROT, 100);
          #endif
          arm.setOneAbsolute(GRIPPER, 80);//Rough center of gripper, default opening position
          arm.safeDelay(1000, 10);*/
          time_pinza = millis();
          //#endif  
        }
    }
  else
    {
      time_protec = millis();
      /*if ((millis()-time_pinza)>5000) 
         {
            arm.setOneAbsolute(GRIPPER, 15);//Rough center of gripper, default opening position
            arm.safeDelay(3000, 10);
         }*/
      if(!flag_stop)
        {
          arm.setOneAbsolute(BASE_ROT, 0);
          arm.setOneAbsolute(SHOULDER, 125);
          arm.setOneAbsolute(ELBOW, 0 + error);
          arm.setOneAbsolute(WRIST, 71 + error);
          #if mode==1
            arm.setOneAbsolute(WRIST_ROT, 0);
          #else
            arm.setOneAbsolute(WRIST_ROT, 100);
          #endif
          arm.setOneAbsolute(GRIPPER, 80);//Rough center of gripper, default opening position
          arm.safeDelay(3000, 10);
          time_pinza = millis();
          flag_stop = true;
        }
    }
}