Skip to content
This repository was archived by the owner on Feb 21, 2020. It is now read-only.

Commit a52ae46

Browse files
author
astronomer80
committed
add ciaoBraccio example
1 parent 0eb787a commit a52ae46

File tree

2 files changed

+467
-0
lines changed

2 files changed

+467
-0
lines changed

examples/ciaoBraccio/ciaoBraccio.ino

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

0 commit comments

Comments
 (0)