source: rtos_arduino/trunk/arduino_lib/libraries/Braccio/src/Braccio.cpp@ 175

Last change on this file since 175 was 175, checked in by ertl-honda, 8 years ago

ライブラリを Arduino IDE 1.7.9 にupdate
File size: 5.2 KB
Line 
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
22extern Servo base;
23extern Servo shoulder;
24extern Servo elbow;
25extern Servo wrist_rot;
26extern Servo wrist_ver;
27extern Servo gripper;
28
29extern int step_base = 0;
30extern int step_shoulder = 45;
31extern int step_elbow = 180;
32extern int step_wrist_rot = 180;
33extern int step_wrist_ver = 90;
34extern 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 */
47unsigned 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 */
93int _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}
Note: See TracBrowser for help on using the repository browser.