[175] | 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 | }
|
---|