1 | /*
|
---|
2 |
|
---|
3 | This sketch uses the rest connector to receive command for the MCU from a rest client.
|
---|
4 | Each command received will fire an action for the Braccio.
|
---|
5 | Run the sketch with Arduino Braccio - Web Examples to move your Braccio via web page.
|
---|
6 | Visit the Demo section of the Braccio's page.
|
---|
7 | http://www.arduino.org/products/tinkerkit/17-arduino-tinkerkit/arduino-tinkerkit-braccio
|
---|
8 |
|
---|
9 |
|
---|
10 |
|
---|
11 | REST command example:
|
---|
12 |
|
---|
13 | * "ledon" -> turn on led 13
|
---|
14 | * "ledoff" -> turn off led 13
|
---|
15 |
|
---|
16 | example: http://arduino.local/arduino/ledon
|
---|
17 |
|
---|
18 | NOTE: be sure to activate and configure rest connector on Linino OS
|
---|
19 | http://labs.arduino.org/Ciao
|
---|
20 |
|
---|
21 | created March 2016
|
---|
22 | by andrea[at]arduino[dot]org and a.ferrante[at]arduino[dot]org
|
---|
23 |
|
---|
24 | */
|
---|
25 |
|
---|
26 | #include <Ciao.h>
|
---|
27 | #include <Servo.h>
|
---|
28 | #include <Braccio.h>
|
---|
29 |
|
---|
30 | //Initial Value for each Motor
|
---|
31 | int m1 = 0;
|
---|
32 | int m2 = 45;
|
---|
33 | int m3 = 180;
|
---|
34 | int m4 = 180;
|
---|
35 | int m5 = 90;
|
---|
36 | int m6 = 0;
|
---|
37 |
|
---|
38 | boolean moveBraccio = false;
|
---|
39 |
|
---|
40 | Servo base;
|
---|
41 | Servo shoulder;
|
---|
42 | Servo elbow;
|
---|
43 | Servo wrist_rot;
|
---|
44 | Servo wrist_ver;
|
---|
45 | Servo gripper;
|
---|
46 |
|
---|
47 | void setup() {
|
---|
48 | //Initialization functions for Ciao
|
---|
49 | Ciao.begin();
|
---|
50 | //Initialization functions for Braccio
|
---|
51 | Braccio.begin();
|
---|
52 | }
|
---|
53 |
|
---|
54 | /**
|
---|
55 | Parse Command from REST
|
---|
56 | It parse a command like: /arduino/base/value:45
|
---|
57 | Giving "base" it return the value
|
---|
58 | @param command: The message to parse
|
---|
59 | @param type: the key for parsing
|
---|
60 | */
|
---|
61 | int parseCommand(String command, String type) {
|
---|
62 | int typeIndex = command.indexOf(type);
|
---|
63 | int dotsIndex = command.indexOf(':', typeIndex + type.length());
|
---|
64 |
|
---|
65 | int idxtmp = dotsIndex + 4;
|
---|
66 | if ((dotsIndex + 4) > command.length()) idxtmp = command.length();
|
---|
67 | String tmp = command.substring(dotsIndex + 1, idxtmp);
|
---|
68 |
|
---|
69 | return tmp.toInt();
|
---|
70 | }
|
---|
71 |
|
---|
72 | void loop() {
|
---|
73 |
|
---|
74 | //Select REST connector
|
---|
75 | CiaoData data = Ciao.read("restserver");
|
---|
76 | //If data is not empry
|
---|
77 | if (!data.isEmpty()) {
|
---|
78 | //ID of the command
|
---|
79 | String id = data.get(0);
|
---|
80 | //Sender ID
|
---|
81 | String sender = data.get(1);
|
---|
82 | //The message from sender
|
---|
83 | String message = data.get(2);
|
---|
84 |
|
---|
85 | message.toUpperCase();
|
---|
86 |
|
---|
87 | /*
|
---|
88 | For each message do the proper command
|
---|
89 | */
|
---|
90 | if (message == "LEDON") {
|
---|
91 | //Turn OFF Led 13
|
---|
92 | digitalWrite(13, HIGH);
|
---|
93 | //Return message to the sender (Eg: the browser)
|
---|
94 | Ciao.writeResponse("restserver", id, "Led D13 ON");
|
---|
95 | }
|
---|
96 | else if (message == "LEDOFF") {
|
---|
97 | digitalWrite(13, LOW);
|
---|
98 | Ciao.writeResponse("restserver", id, "Led D13 OFF");
|
---|
99 | }
|
---|
100 | //This command allow you to move a desired servo motor giving the
|
---|
101 | //PWM pin where is connected
|
---|
102 | else if (message.startsWith("SERVO")) {
|
---|
103 | //Parse the message to retrive what is the servo to move
|
---|
104 | int servo = parseCommand(message, "SERVO");
|
---|
105 | //Parse the message to retrive what is the value for the servo
|
---|
106 | int value = parseCommand(message, "VALUE");
|
---|
107 |
|
---|
108 | Ciao.writeResponse("restserver", id, "Message:" + String(message) + "SERVO: " + String(servo) + " " + String(value));
|
---|
109 | }
|
---|
110 | //Command for the base of the braccio (M1)
|
---|
111 | else if (message.startsWith("BASE")) {
|
---|
112 | m1 = parseCommand(message, "VALUE");
|
---|
113 | moveBraccio = true;
|
---|
114 | Ciao.writeResponse("restserver", id, "BASE: " + String(m1));
|
---|
115 | }
|
---|
116 | //Command for the shoulder of the braccio (M2)
|
---|
117 | else if (message.startsWith("SHOULDER")) {
|
---|
118 | m2 = parseCommand(message, "VALUE");
|
---|
119 | moveBraccio = true;
|
---|
120 | Ciao.writeResponse("restserver", id, "SHOULDER: " + String(m2));
|
---|
121 | }
|
---|
122 | //Command for the elbow of the braccio (M3)
|
---|
123 | else if (message.startsWith("ELBOW")) {
|
---|
124 | m3 = parseCommand(message, "VALUE");
|
---|
125 | moveBraccio = true;
|
---|
126 | Ciao.writeResponse("restserver", id, "ELBOW: " + String(m3));
|
---|
127 | }
|
---|
128 | //Command for the wrist of the braccio to move it up and down (M4)
|
---|
129 | else if (message.startsWith("WRISTV")) {
|
---|
130 | m4 = parseCommand(message, "VALUE");
|
---|
131 | moveBraccio = true;
|
---|
132 | Ciao.writeResponse("restserver", id, "WRISTV: " + String(m4));
|
---|
133 | }
|
---|
134 | //Command for the wrist of the braccio to rotate it (M5)
|
---|
135 | else if (message.startsWith("WRISTR")) {
|
---|
136 | m5 = parseCommand(message, "VALUE");
|
---|
137 | moveBraccio = true;
|
---|
138 | Ciao.writeResponse("restserver", id, "WRISTR: " + String(m5));
|
---|
139 | }
|
---|
140 | //Command for the gripper of the braccio to open and close it (M6)
|
---|
141 | else if (message.startsWith("GRIPPER")) {
|
---|
142 | m6 = parseCommand(message, "VALUE");
|
---|
143 | moveBraccio = true;
|
---|
144 | Ciao.writeResponse("restserver", id, "GRIPPER: " + String(m6));
|
---|
145 | }
|
---|
146 | //Command to say "Ciao"
|
---|
147 | else if (message.startsWith("SAYCIAO")) {
|
---|
148 | sayCiao();
|
---|
149 | Ciao.writeResponse("restserver", id, "SAYCIAO: " + String(m6));
|
---|
150 | }
|
---|
151 | //Command for take the sponge
|
---|
152 | else if (message.startsWith("TAKESPONGE")) {
|
---|
153 | takesponge();
|
---|
154 | Ciao.writeResponse("restserver", id, "TAKESPONGE: " + String(m6));
|
---|
155 | }
|
---|
156 | //Command for show the sponge
|
---|
157 | else if (message.startsWith("SHOWSPONGE")) {
|
---|
158 | showsponge();
|
---|
159 | Ciao.writeResponse("restserver", id, "SHOWSPONGE: " + String(m6));
|
---|
160 | }
|
---|
161 | //Command for throw away the sponge
|
---|
162 | else if (message.startsWith("THROWSPONGE")) {
|
---|
163 | throwsponge();
|
---|
164 | Ciao.writeResponse("restserver", id, "THROWSPONGE: " + String(m6));
|
---|
165 | }
|
---|
166 |
|
---|
167 | else
|
---|
168 | Ciao.writeResponse("restserver", id, "command error");
|
---|
169 |
|
---|
170 | //if flag moveBraccio is true fire the movement
|
---|
171 | if (moveBraccio) {
|
---|
172 | Braccio.ServoMovement(20, m1, m2, m3, m4, m5, m6);
|
---|
173 | moveBraccio = false;
|
---|
174 | }
|
---|
175 | }
|
---|
176 | }
|
---|
177 |
|
---|
178 | /**
|
---|
179 | The braccio Say 'Ciao' with the Tongue
|
---|
180 | */
|
---|
181 | void sayCiao() {
|
---|
182 |
|
---|
183 | Braccio.ServoMovement(20, 90, 0, 180, 160, 0, 15);
|
---|
184 |
|
---|
185 | for (int i = 0; i < 5; i++) {
|
---|
186 | Braccio.ServoMovement(10, 90, 0, 180, 160, 0, 15);
|
---|
187 | delay(500);
|
---|
188 |
|
---|
189 | Braccio.ServoMovement(10, 90, 0, 180, 160, 0, 73);
|
---|
190 | delay(500);
|
---|
191 | }
|
---|
192 | }
|
---|
193 |
|
---|
194 | /**
|
---|
195 | Braccio take the Sponge
|
---|
196 | */
|
---|
197 | void takesponge() {
|
---|
198 | //starting position
|
---|
199 | //(step delay M1 , M2 , M3 , M4 , M5 , M6);
|
---|
200 | Braccio.ServoMovement(20, 0, 45, 180, 180, 90, 0);
|
---|
201 |
|
---|
202 | //I move arm towards the sponge
|
---|
203 | Braccio.ServoMovement(20, 0, 90, 180, 180, 90, 0);
|
---|
204 |
|
---|
205 | //the gripper takes the sponge
|
---|
206 | Braccio.ServoMovement(20, 0, 90, 180, 180, 90, 60 );
|
---|
207 |
|
---|
208 | //up the sponge
|
---|
209 | Braccio.ServoMovement(20, 0, 45, 180, 45, 0, 60);
|
---|
210 | }
|
---|
211 |
|
---|
212 |
|
---|
213 | /**
|
---|
214 | Braccio show the sponge to the user
|
---|
215 | */
|
---|
216 | void showsponge() {
|
---|
217 | for (int i = 0; i < 2; i++) {
|
---|
218 |
|
---|
219 | //(step delay M1 , M2 , M3 , M4 , M5 , M6 );
|
---|
220 | Braccio.ServoMovement(10, 0, 45, 180, 45, 180, 60);
|
---|
221 |
|
---|
222 | Braccio.ServoMovement(10, 0, 45, 180, 45, 0, 60);
|
---|
223 | }
|
---|
224 | }
|
---|
225 |
|
---|
226 | /**
|
---|
227 | Braccio throw away the sponge
|
---|
228 | */
|
---|
229 | void throwsponge() {
|
---|
230 | //(step delay M1 , M2 , M3 , M4 , M5 , M6 );
|
---|
231 | Braccio.ServoMovement(20, 0, 45, 90, 45, 90, 60);
|
---|
232 |
|
---|
233 | Braccio.ServoMovement(5, 0, 45, 135, 90, 90, 60);
|
---|
234 |
|
---|
235 | Braccio.ServoMovement(5, 0, 90, 150, 90, 90, 0);
|
---|
236 | }
|
---|