172 lines
6.3 KiB
C++
172 lines
6.3 KiB
C++
#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;
|
|
}
|
|
}
|
|
}
|