[175] | 1 | /*
|
---|
| 2 | Braccio.cpp - board library Version 1.1
|
---|
| 3 | Written by Andrea Martino
|
---|
| 4 |
|
---|
| 5 | This library is free software; you can redistribute it and/or
|
---|
| 6 | modify it under the terms of the GNU Lesser General Public
|
---|
| 7 | License as published by the Free Software Foundation; either
|
---|
| 8 | version 2.1 of the License, or (at your option) any later version.
|
---|
| 9 |
|
---|
| 10 | This library is distributed in the hope that it will be useful,
|
---|
| 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
| 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
---|
| 13 | Lesser General Public License for more details.
|
---|
| 14 |
|
---|
| 15 | You should have received a copy of the GNU Lesser General Public
|
---|
| 16 | License along with this library; if not, write to the Free Software
|
---|
| 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
---|
| 18 | */
|
---|
| 19 |
|
---|
| 20 | #include "Braccio.h"
|
---|
| 21 |
|
---|
| 22 | extern Servo base;
|
---|
| 23 | extern Servo shoulder;
|
---|
| 24 | extern Servo elbow;
|
---|
| 25 | extern Servo wrist_rot;
|
---|
| 26 | extern Servo wrist_ver;
|
---|
| 27 | extern Servo gripper;
|
---|
| 28 |
|
---|
| 29 | extern int step_base = 0;
|
---|
| 30 | extern int step_shoulder = 45;
|
---|
| 31 | extern int step_elbow = 180;
|
---|
| 32 | extern int step_wrist_rot = 180;
|
---|
| 33 | extern int step_wrist_ver = 90;
|
---|
| 34 | extern int step_gripper = 10;
|
---|
| 35 |
|
---|
| 36 | _Braccio Braccio;
|
---|
| 37 |
|
---|
| 38 | //Initialize Braccio object
|
---|
| 39 | _Braccio::_Braccio() {
|
---|
| 40 | }
|
---|
| 41 |
|
---|
| 42 | /**
|
---|
| 43 | * Braccio initialization and set intial position
|
---|
| 44 | * Modifing this function you can set up the initial position of all the
|
---|
| 45 | * servo motors of the Braccio
|
---|
| 46 | */
|
---|
| 47 | unsigned int _Braccio::begin() {
|
---|
| 48 | // initialization pin Servo motors
|
---|
| 49 | #if defined(ARDUINO_ARCH_SAMD)
|
---|
| 50 | base.attach(11);
|
---|
| 51 | shoulder.attach(7);
|
---|
| 52 | elbow.attach(9);
|
---|
| 53 | wrist_rot.attach(6);
|
---|
| 54 | wrist_ver.attach(8);
|
---|
| 55 | gripper.attach(3);
|
---|
| 56 | #else
|
---|
| 57 | base.attach(11);
|
---|
| 58 | shoulder.attach(10);
|
---|
| 59 | elbow.attach(9);
|
---|
| 60 | wrist_rot.attach(6);
|
---|
| 61 | wrist_ver.attach(5);
|
---|
| 62 | gripper.attach(3);
|
---|
| 63 | #endif
|
---|
| 64 |
|
---|
| 65 | //For each step motor this set up the initial degree
|
---|
| 66 | base.write(90);
|
---|
| 67 | shoulder.write(45);
|
---|
| 68 | elbow.write(180);
|
---|
| 69 | wrist_ver.write(180);
|
---|
| 70 | wrist_rot.write(90);
|
---|
| 71 | gripper.write(10);
|
---|
| 72 | //Previous step motor position
|
---|
| 73 | step_base = 90;
|
---|
| 74 | step_shoulder = 45;
|
---|
| 75 | step_elbow = 180;
|
---|
| 76 | step_wrist_ver = 180;
|
---|
| 77 | step_wrist_rot = 90;
|
---|
| 78 | step_gripper = 10;
|
---|
| 79 | return 1;
|
---|
| 80 | }
|
---|
| 81 |
|
---|
| 82 | /**
|
---|
| 83 | * This functions allow you to control all the servo motors in the Braccio setting the funcion parameters.
|
---|
| 84 | *
|
---|
| 85 | * @param stepDelay The delay between each servo movement
|
---|
| 86 | * @param vBase next base servo motor degree
|
---|
| 87 | * @param vShoulder next shoulder servo motor degree
|
---|
| 88 | * @param vElbow next elbow servo motor degree
|
---|
| 89 | * @param vWrist_ver next wrist rotation servo motor degree
|
---|
| 90 | * @param vWrist_rot next wrist vertical servo motor degree
|
---|
| 91 | * @param vgripper next gripper servo motor degree
|
---|
| 92 | */
|
---|
| 93 | int _Braccio::ServoMovement(int stepDelay, int vBase, int vShoulder, int vElbow,int vWrist_ver, int vWrist_rot, int vgripper) {
|
---|
| 94 |
|
---|
| 95 | // Check values
|
---|
| 96 | if (stepDelay > 30) stepDelay = 30;
|
---|
| 97 | if (stepDelay < 10) stepDelay = 10;
|
---|
| 98 | if (vBase < 0) vBase=0;
|
---|
| 99 | if (vBase > 180) vBase=180;
|
---|
| 100 | if (vShoulder < 15) vShoulder=15;
|
---|
| 101 | if (vShoulder > 165) vShoulder=165;
|
---|
| 102 | if (vElbow < 0) vElbow=0;
|
---|
| 103 | if (vElbow > 180) vElbow=180;
|
---|
| 104 | if (vWrist_ver < 0) vWrist_ver=0;
|
---|
| 105 | if (vWrist_ver > 180) vWrist_ver=180;
|
---|
| 106 | if (vWrist_rot > 180) vWrist_rot=180;
|
---|
| 107 | if (vWrist_rot < 0) vWrist_rot=0;
|
---|
| 108 | if (vgripper < 10) vgripper = 10;
|
---|
| 109 | if (vgripper > 73) vgripper = 73;
|
---|
| 110 |
|
---|
| 111 | int exit = 1;
|
---|
| 112 |
|
---|
| 113 | //Until the all motors are in the desired position
|
---|
| 114 | while (exit)
|
---|
| 115 | {
|
---|
| 116 | //For each servo motor if next degree is not the same of the previuos than do the movement
|
---|
| 117 | if (vBase != step_base)
|
---|
| 118 | {
|
---|
| 119 | base.write(step_base);
|
---|
| 120 | //One step ahead
|
---|
| 121 | if (vBase > step_base) {
|
---|
| 122 | step_base++;
|
---|
| 123 | }
|
---|
| 124 | //One step beyond
|
---|
| 125 | if (vBase < step_base) {
|
---|
| 126 | step_base--;
|
---|
| 127 | }
|
---|
| 128 | }
|
---|
| 129 |
|
---|
| 130 | if (vShoulder != step_shoulder)
|
---|
| 131 | {
|
---|
| 132 | shoulder.write(step_shoulder);
|
---|
| 133 | //One step ahead
|
---|
| 134 | if (vShoulder > step_shoulder) {
|
---|
| 135 | step_shoulder++;
|
---|
| 136 | }
|
---|
| 137 | //One step beyond
|
---|
| 138 | if (vShoulder < step_shoulder) {
|
---|
| 139 | step_shoulder--;
|
---|
| 140 | }
|
---|
| 141 |
|
---|
| 142 | }
|
---|
| 143 |
|
---|
| 144 | if (vElbow != step_elbow)
|
---|
| 145 | {
|
---|
| 146 | elbow.write(step_elbow);
|
---|
| 147 | //One step ahead
|
---|
| 148 | if (vElbow > step_elbow) {
|
---|
| 149 | step_elbow++;
|
---|
| 150 | }
|
---|
| 151 | //One step beyond
|
---|
| 152 | if (vElbow < step_elbow) {
|
---|
| 153 | step_elbow--;
|
---|
| 154 | }
|
---|
| 155 |
|
---|
| 156 | }
|
---|
| 157 |
|
---|
| 158 | if (vWrist_ver != step_wrist_rot)
|
---|
| 159 | {
|
---|
| 160 | wrist_rot.write(step_wrist_rot);
|
---|
| 161 | //One step ahead
|
---|
| 162 | if (vWrist_ver > step_wrist_rot) {
|
---|
| 163 | step_wrist_rot++;
|
---|
| 164 | }
|
---|
| 165 | //One step beyond
|
---|
| 166 | if (vWrist_ver < step_wrist_rot) {
|
---|
| 167 | step_wrist_rot--;
|
---|
| 168 | }
|
---|
| 169 |
|
---|
| 170 | }
|
---|
| 171 |
|
---|
| 172 | if (vWrist_rot != step_wrist_ver)
|
---|
| 173 | {
|
---|
| 174 | wrist_ver.write(step_wrist_ver);
|
---|
| 175 | //One step ahead
|
---|
| 176 | if (vWrist_rot > step_wrist_ver) {
|
---|
| 177 | step_wrist_ver++;
|
---|
| 178 | }
|
---|
| 179 | //One step beyond
|
---|
| 180 | if (vWrist_rot < step_wrist_ver) {
|
---|
| 181 | step_wrist_ver--;
|
---|
| 182 | }
|
---|
| 183 | }
|
---|
| 184 |
|
---|
| 185 | if (vgripper != step_gripper)
|
---|
| 186 | {
|
---|
| 187 | gripper.write(step_gripper);
|
---|
| 188 | if (vgripper > step_gripper) {
|
---|
| 189 | step_gripper++;
|
---|
| 190 | }
|
---|
| 191 | //One step beyond
|
---|
| 192 | if (vgripper < step_gripper) {
|
---|
| 193 | step_gripper--;
|
---|
| 194 | }
|
---|
| 195 | }
|
---|
| 196 |
|
---|
| 197 |
|
---|
| 198 | //The delay between each movement
|
---|
| 199 | delay(stepDelay);
|
---|
| 200 |
|
---|
| 201 | //It checks if all the servo motors are in the desired position
|
---|
| 202 | if ((vBase == step_base) && (vShoulder == step_shoulder)
|
---|
| 203 | && (vElbow == step_elbow) && (vWrist_ver == step_wrist_rot)
|
---|
| 204 | && (vWrist_rot == step_wrist_ver) && (vgripper == step_gripper)) {
|
---|
| 205 | step_base = vBase;
|
---|
| 206 | step_shoulder = vShoulder;
|
---|
| 207 | step_elbow = vElbow;
|
---|
| 208 | step_wrist_rot = vWrist_ver;
|
---|
| 209 | step_wrist_ver = vWrist_rot;
|
---|
| 210 | step_gripper = vgripper;
|
---|
| 211 | exit = 0;
|
---|
| 212 | } else {
|
---|
| 213 | exit = 1;
|
---|
| 214 | }
|
---|
| 215 |
|
---|
| 216 | }
|
---|
| 217 |
|
---|
| 218 | }
|
---|