From a8ccfe10d1722cdde20c964791b4a93418aaf084 Mon Sep 17 00:00:00 2001 From: Andrea Date: Fri, 26 Aug 2016 17:08:34 +0200 Subject: [PATCH 1/7] add softstart --- examples/ciaoBraccio/ciaoBraccio.ino | 2 +- examples/simpleMovements/simpleMovements.ino | 2 +- examples/takethesponge/takethesponge.ino | 2 +- examples/testBraccio90/testBraccio90.ino | 2 +- keywords.txt | 3 + src/Braccio.cpp | 108 ++++++++++++++----- src/Braccio.h | 7 +- 7 files changed, 93 insertions(+), 33 deletions(-) diff --git a/examples/ciaoBraccio/ciaoBraccio.ino b/examples/ciaoBraccio/ciaoBraccio.ino index 05515c7..d6f79b9 100644 --- a/examples/ciaoBraccio/ciaoBraccio.ino +++ b/examples/ciaoBraccio/ciaoBraccio.ino @@ -48,7 +48,7 @@ void setup() { //Initialization functions for Ciao Ciao.begin(); //Initialization functions for Braccio - Braccio.begin(); + Braccio.begin(MEDIUM); } /** diff --git a/examples/simpleMovements/simpleMovements.ino b/examples/simpleMovements/simpleMovements.ino index 30d5893..0838acb 100644 --- a/examples/simpleMovements/simpleMovements.ino +++ b/examples/simpleMovements/simpleMovements.ino @@ -29,7 +29,7 @@ void setup() { //Wrist vertical (M4): 180 degrees //Wrist rotation (M5): 90 degrees //gripper (M6): 10 degrees - Braccio.begin(); + Braccio.begin(MEDIUM); } void loop() { diff --git a/examples/takethesponge/takethesponge.ino b/examples/takethesponge/takethesponge.ino index a98defe..2e9a860 100644 --- a/examples/takethesponge/takethesponge.ino +++ b/examples/takethesponge/takethesponge.ino @@ -30,7 +30,7 @@ void setup() { //Wrist vertical (M4): 180 degrees //Wrist rotation (M5): 90 degrees //gripper (M6): 10 degrees - Braccio.begin(); + Braccio.begin(MEDIUM); } void loop() { diff --git a/examples/testBraccio90/testBraccio90.ino b/examples/testBraccio90/testBraccio90.ino index c241596..275811b 100644 --- a/examples/testBraccio90/testBraccio90.ino +++ b/examples/testBraccio90/testBraccio90.ino @@ -32,7 +32,7 @@ void setup() { //Wrist vertical (M4): 180 degrees //Wrist rotation (M5): 90 degrees //gripper (M6): 10 degrees - Braccio.begin(); + Braccio.begin(MEDIUM); } void loop() { diff --git a/keywords.txt b/keywords.txt index 6cbb2f6..c8e923d 100644 --- a/keywords.txt +++ b/keywords.txt @@ -32,3 +32,6 @@ step_elbow LITERAL1 step_wrist_rot LITERAL1 step_wrist_ver LITERAL1 step_gripper LITERAL1 +FAST LITERAL1 +MEDIUM LITERAL1 +SLOW LITERAL1 diff --git a/src/Braccio.cpp b/src/Braccio.cpp index a4549eb..8cd4eec 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -19,6 +19,10 @@ #include "Braccio.h" +#define LOW_LIMIT_TIMEOUT 2000 +#define HIGH_LIMIT_TIMEOUT 6000 + + extern Servo base; extern Servo shoulder; extern Servo elbow; @@ -32,6 +36,7 @@ extern int step_elbow = 180; extern int step_wrist_rot = 180; extern int step_wrist_ver = 90; extern int step_gripper = 10; + _Braccio Braccio; @@ -44,38 +49,85 @@ _Braccio::_Braccio() { * Modifing this function you can set up the initial position of all the * servo motors of the Braccio */ -unsigned int _Braccio::begin() { +unsigned int _Braccio::begin(int val) { + + pinMode(12,OUTPUT); + digitalWrite(12,LOW); + // initialization pin Servo motors - #if defined(ARDUINO_ARCH_SAMD) - base.attach(11); - shoulder.attach(7); - elbow.attach(9); - wrist_rot.attach(6); - wrist_ver.attach(8); - gripper.attach(3); - #else - base.attach(11); - shoulder.attach(10); - elbow.attach(9); - wrist_rot.attach(6); - wrist_ver.attach(5); - gripper.attach(3); - #endif - - //For each step motor this set up the initial degree - base.write(90); - shoulder.write(45); + base.attach(11); + shoulder.attach(10); + elbow.attach(9); + wrist_rot.attach(6); + wrist_ver.attach(5); + gripper.attach(3); + + //For each step motor this set up the initial degree + base.write(0); + shoulder.write(40); elbow.write(180); - wrist_ver.write(180); - wrist_rot.write(90); - gripper.write(10); + wrist_ver.write(170); + wrist_rot.write(0); + gripper.write(73); //Previous step motor position - step_base = 90; - step_shoulder = 45; + step_base = 0; + step_shoulder = 40; step_elbow = 180; - step_wrist_ver = 180; - step_wrist_rot = 90; - step_gripper = 10; + step_wrist_ver = 170; + step_wrist_rot = 0; + step_gripper = 73; + + + long int tmp=millis(); + // soft start implementation + if (val==FAST){ + + while(millis()-tmp < LOW_LIMIT_TIMEOUT){ + digitalWrite(12,HIGH); + delayMicroseconds(125); + digitalWrite(12,LOW); + delayMicroseconds(50); + } + + while(millis()-tmp < HIGH_LIMIT_TIMEOUT){ + digitalWrite(12,HIGH); + delayMicroseconds(125); + digitalWrite(12,LOW); + delayMicroseconds(105); + } + + }else if (val==MEDIUM){ + while(millis()-tmp < LOW_LIMIT_TIMEOUT){ + digitalWrite(12,HIGH); + delayMicroseconds(15); + digitalWrite(12,LOW); + delayMicroseconds(125); + } + + while(millis()-tmp < HIGH_LIMIT_TIMEOUT){ + digitalWrite(12,HIGH); + delayMicroseconds(15); + digitalWrite(12,LOW); + delayMicroseconds(145); + } + + + }else if (val==SLOW){ + while(millis()-tmp < LOW_LIMIT_TIMEOUT){ + digitalWrite(12,HIGH); + delayMicroseconds(5); + digitalWrite(12,LOW); + delayMicroseconds(155); + } + + while(millis()-tmp < HIGH_LIMIT_TIMEOUT){ + digitalWrite(12,HIGH); + delayMicroseconds(5); + digitalWrite(12,LOW); + delayMicroseconds(165); + } + } + digitalWrite(12,HIGH); return 1; } diff --git a/src/Braccio.h b/src/Braccio.h index 0f8de79..c65d3b9 100644 --- a/src/Braccio.h +++ b/src/Braccio.h @@ -23,6 +23,11 @@ #include #include +#define FAST 2 +#define MEDIUM 3 +#define SLOW 4 + + class _Braccio { public: @@ -33,7 +38,7 @@ class _Braccio { * Modifing this function you can set up the initial position of all the * servo motors of the Braccio */ - unsigned int begin(); + unsigned int begin(int val); /** * This functions allow you to control all the servo motors in the Braccio From 3e3bb31aa2cb0569aa929f66233c597460d18389 Mon Sep 17 00:00:00 2001 From: Angelo Ferrante Date: Wed, 7 Sep 2016 14:46:00 +0200 Subject: [PATCH 2/7] Update README.adoc --- README.adoc | 2 -- 1 file changed, 2 deletions(-) diff --git a/README.adoc b/README.adoc index 84dbe9b..8923b32 100644 --- a/README.adoc +++ b/README.adoc @@ -7,8 +7,6 @@ http://www.arduino.org/products/tinkerkit/17-arduino-tinkerkit/arduino-tinkerkit == License == -Copyright (c) Medea-Solutions. All right reserved. - This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either From 8d65150f9a42d2c8d2eea29e4067e608ea342133 Mon Sep 17 00:00:00 2001 From: Angelo Ferrante Date: Wed, 7 Sep 2016 14:48:06 +0200 Subject: [PATCH 3/7] Update README.adoc --- README.adoc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.adoc b/README.adoc index 8923b32..dc49988 100644 --- a/README.adoc +++ b/README.adoc @@ -1,6 +1,9 @@ = Braccio Library for Arduino = -The library offers easy access to the data from the onboard Arduino Braccio, and provides moves the Arduino Braccio. +The library offers easy access to the data from the onboard TinkerKit Braccio, and provides moves for the Braccio. + +- Upgrades: +Soft-Start: Used with the Braccio Shield V4 create a slow and soft start for the TinkerKit Braccio For more information about this library please visit us at http://www.arduino.org/products/tinkerkit/17-arduino-tinkerkit/arduino-tinkerkit-braccio From 0e4d6e911ff45b875af685e55c100fc9938abfca Mon Sep 17 00:00:00 2001 From: astronomer80 Date: Fri, 9 Sep 2016 17:52:07 +0200 Subject: [PATCH 4/7] Clean code --- examples/ciaoBraccio/ciaoBraccio.ino | 7 +- examples/simpleMovements/simpleMovements.ino | 2 +- examples/takethesponge/takethesponge.ino | 2 +- examples/testBraccio90/testBraccio90.ino | 2 +- src/Braccio.cpp | 118 ++++++++++--------- src/Braccio.h | 43 ++++++- 6 files changed, 105 insertions(+), 69 deletions(-) diff --git a/examples/ciaoBraccio/ciaoBraccio.ino b/examples/ciaoBraccio/ciaoBraccio.ino index d6f79b9..2e4cc58 100644 --- a/examples/ciaoBraccio/ciaoBraccio.ino +++ b/examples/ciaoBraccio/ciaoBraccio.ino @@ -45,10 +45,11 @@ Servo wrist_ver; Servo gripper; void setup() { - //Initialization functions for Ciao + //Initialization function for Ciao Ciao.begin(); - //Initialization functions for Braccio - Braccio.begin(MEDIUM); + //Initialization function for Braccio + //You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 + Braccio.begin(); } /** diff --git a/examples/simpleMovements/simpleMovements.ino b/examples/simpleMovements/simpleMovements.ino index 0838acb..30d5893 100644 --- a/examples/simpleMovements/simpleMovements.ino +++ b/examples/simpleMovements/simpleMovements.ino @@ -29,7 +29,7 @@ void setup() { //Wrist vertical (M4): 180 degrees //Wrist rotation (M5): 90 degrees //gripper (M6): 10 degrees - Braccio.begin(MEDIUM); + Braccio.begin(); } void loop() { diff --git a/examples/takethesponge/takethesponge.ino b/examples/takethesponge/takethesponge.ino index 2e9a860..a98defe 100644 --- a/examples/takethesponge/takethesponge.ino +++ b/examples/takethesponge/takethesponge.ino @@ -30,7 +30,7 @@ void setup() { //Wrist vertical (M4): 180 degrees //Wrist rotation (M5): 90 degrees //gripper (M6): 10 degrees - Braccio.begin(MEDIUM); + Braccio.begin(); } void loop() { diff --git a/examples/testBraccio90/testBraccio90.ino b/examples/testBraccio90/testBraccio90.ino index 275811b..c241596 100644 --- a/examples/testBraccio90/testBraccio90.ino +++ b/examples/testBraccio90/testBraccio90.ino @@ -32,7 +32,7 @@ void setup() { //Wrist vertical (M4): 180 degrees //Wrist rotation (M5): 90 degrees //gripper (M6): 10 degrees - Braccio.begin(MEDIUM); + Braccio.begin(); } void loop() { diff --git a/src/Braccio.cpp b/src/Braccio.cpp index 8cd4eec..3af382c 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -19,10 +19,6 @@ #include "Braccio.h" -#define LOW_LIMIT_TIMEOUT 2000 -#define HIGH_LIMIT_TIMEOUT 6000 - - extern Servo base; extern Servo shoulder; extern Servo elbow; @@ -48,11 +44,15 @@ _Braccio::_Braccio() { * Braccio initialization and set intial position * Modifing this function you can set up the initial position of all the * servo motors of the Braccio + * @param soft_start_level: SOFT_START_DISABLED, SOFT_START_SLOW, SOFT_START_MEDIUM, SOFT_START_FAST + * You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 + * SOFT_START_DISABLED disable the movement */ -unsigned int _Braccio::begin(int val) { - - pinMode(12,OUTPUT); - digitalWrite(12,LOW); +unsigned int _Braccio::begin(int soft_start_level) { + if(soft_start_level!=SOFT_START_DISABLED){ + pinMode(SOFT_START_CONTROL_PIN,OUTPUT); + digitalWrite(SOFT_START_CONTROL_PIN,LOW); + } // initialization pin Servo motors base.attach(11); @@ -78,61 +78,66 @@ unsigned int _Braccio::begin(int val) { step_gripper = 73; - long int tmp=millis(); - // soft start implementation - if (val==FAST){ + if(soft_start_level!=SOFT_START_DISABLED) + _softStart(soft_start_level); + return 1; +} - while(millis()-tmp < LOW_LIMIT_TIMEOUT){ - digitalWrite(12,HIGH); - delayMicroseconds(125); - digitalWrite(12,LOW); - delayMicroseconds(50); - } +/* +Default implementation +*/ +unsigned int _Braccio::begin() { + //SOFT_START_SLOW is the default value + return begin(SOFT_START_SLOW); +} + +/* +Software implementation of the PWM for the SOFT_START_CONTROL_PIN,HIGH +@param high_time: the time in the logic level high +@param low_time: the time in the logic level low +*/ +void _Braccio::_softwarePWM(int high_time, int low_time){ + digitalWrite(SOFT_START_CONTROL_PIN,HIGH); + delayMicroseconds(high_time); + digitalWrite(SOFT_START_CONTROL_PIN,LOW); + delayMicroseconds(low_time); +} + +/* +This function, used only with the Braccio Shield V4 and greater, +turn ON the Braccio softly and slowly. +The SOFT_START_CONTROL_PIN is used as a software PWM +@parameter soft_start_level: The soft start level: FAST, MEDIUM, SLOW +*/ +void _Braccio::_softStart(int soft_start_level){ + long int tmp=millis(); + if (soft_start_level==SOFT_START_FAST){ + while(millis()-tmp < LOW_LIMIT_TIMEOUT) + _softwarePWM(125, 50); - while(millis()-tmp < HIGH_LIMIT_TIMEOUT){ - digitalWrite(12,HIGH); - delayMicroseconds(125); - digitalWrite(12,LOW); - delayMicroseconds(105); - } + while(millis()-tmp < HIGH_LIMIT_TIMEOUT) + _softwarePWM(125, 105); - }else if (val==MEDIUM){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT){ - digitalWrite(12,HIGH); - delayMicroseconds(15); - digitalWrite(12,LOW); - delayMicroseconds(125); - } - - while(millis()-tmp < HIGH_LIMIT_TIMEOUT){ - digitalWrite(12,HIGH); - delayMicroseconds(15); - digitalWrite(12,LOW); - delayMicroseconds(145); - } + }else if (soft_start_level==SOFT_START_MEDIUM){ + while(millis()-tmp < LOW_LIMIT_TIMEOUT) + _softwarePWM(15, 125); + while(millis()-tmp < HIGH_LIMIT_TIMEOUT) + _softwarePWM(15, 145); - }else if (val==SLOW){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT){ - digitalWrite(12,HIGH); - delayMicroseconds(5); - digitalWrite(12,LOW); - delayMicroseconds(155); - } + }else if (soft_start_level==SOFT_START_SLOW){ + while(millis()-tmp < LOW_LIMIT_TIMEOUT) + _softwarePWM(15, 155); - while(millis()-tmp < HIGH_LIMIT_TIMEOUT){ - digitalWrite(12,HIGH); - delayMicroseconds(5); - digitalWrite(12,LOW); - delayMicroseconds(165); - } + while(millis()-tmp < HIGH_LIMIT_TIMEOUT) + _softwarePWM(5, 165); } - digitalWrite(12,HIGH); - return 1; + + digitalWrite(SOFT_START_CONTROL_PIN,HIGH); } /** - * This functions allow you to control all the servo motors in the Braccio setting the funcion parameters. + * This functions allow you to control all the servo motors * * @param stepDelay The delay between each servo movement * @param vBase next base servo motor degree @@ -144,7 +149,7 @@ unsigned int _Braccio::begin(int val) { */ int _Braccio::ServoMovement(int stepDelay, int vBase, int vShoulder, int vElbow,int vWrist_ver, int vWrist_rot, int vgripper) { - // Check values + // Check values, to avoid dangerous positions for the Braccio if (stepDelay > 30) stepDelay = 30; if (stepDelay < 10) stepDelay = 10; if (vBase < 0) vBase=0; @@ -245,9 +250,8 @@ int _Braccio::ServoMovement(int stepDelay, int vBase, int vShoulder, int vElbow, step_gripper--; } } - - //The delay between each movement + //delay between each movement delay(stepDelay); //It checks if all the servo motors are in the desired position @@ -264,7 +268,5 @@ int _Braccio::ServoMovement(int stepDelay, int vBase, int vShoulder, int vElbow, } else { exit = 1; } - } - } diff --git a/src/Braccio.h b/src/Braccio.h index c65d3b9..16c70e9 100644 --- a/src/Braccio.h +++ b/src/Braccio.h @@ -23,10 +23,20 @@ #include #include -#define FAST 2 -#define MEDIUM 3 -#define SLOW 4 +// You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 +#define SOFT_START_DISABLED 0 +//You can set different values for the softstart. SOFT_START_SLOW is the default value +#define SOFT_START_FAST 2 +#define SOFT_START_MEDIUM 3 +#define SOFT_START_SLOW 4 +//The software PWM is connected to PIN 12. You cannot use the pin 12 if you are using +//a Braccio shield V4 or newer +#define SOFT_START_CONTROL_PIN 12 + +//Low and High Limit Timeout for the Software PWM +#define LOW_LIMIT_TIMEOUT 2000 +#define HIGH_LIMIT_TIMEOUT 6000 class _Braccio { @@ -38,12 +48,35 @@ class _Braccio { * Modifing this function you can set up the initial position of all the * servo motors of the Braccio */ - unsigned int begin(int val); - + unsigned int begin(); + + /** + * @param soft_start_level: the softstart_level: SOFT_START_DISABLED, SOFT_START_SLOW, SOFT_START_MEDIUM, SOFT_START_FAST + * You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 + */ + unsigned int begin(int soft_start_level); + /** * This functions allow you to control all the servo motors in the Braccio */ int ServoMovement(int delay, int Vbase,int Vshoulder, int Velbow, int Vwrist_ver, int Vwrist_rot, int Vgripper); + + +private: + /* + * This function, used only with the Braccio Shield V4 and greater, + * turn ON the Braccio softly and slowly. + * The SOFT_START_CONTROL_PIN is used as a software PWM + * @parameter soft_start_level: The soft start level: FAST, MEDIUM, SLOW + */ + void _softStart(int soft_start_level); + + /* + * Software implementation of the PWM for the SOFT_START_CONTROL_PIN,HIGH + * @param high_time: the time in the logic level high + * @param low_time: the time in the logic level low + */ + void _softwarePWM(int high_time, int low_time); }; From 8df40132eaf2236e177fda3e753e37bc43c5881e Mon Sep 17 00:00:00 2001 From: astronomer80 Date: Mon, 12 Sep 2016 15:25:05 +0200 Subject: [PATCH 5/7] Some fix to the soft start and to the comments --- src/Braccio.cpp | 26 +++++--------------------- src/Braccio.h | 6 +----- 2 files changed, 6 insertions(+), 26 deletions(-) diff --git a/src/Braccio.cpp b/src/Braccio.cpp index 3af382c..63fec73 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -88,7 +88,7 @@ Default implementation */ unsigned int _Braccio::begin() { //SOFT_START_SLOW is the default value - return begin(SOFT_START_SLOW); + return begin(0); } /* @@ -111,27 +111,11 @@ The SOFT_START_CONTROL_PIN is used as a software PWM */ void _Braccio::_softStart(int soft_start_level){ long int tmp=millis(); - if (soft_start_level==SOFT_START_FAST){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT) - _softwarePWM(125, 50); + while(millis()-tmp < LOW_LIMIT_TIMEOUT) + _softwarePWM(30+soft_start_level, 500 - soft_start_level); - while(millis()-tmp < HIGH_LIMIT_TIMEOUT) - _softwarePWM(125, 105); - - }else if (soft_start_level==SOFT_START_MEDIUM){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT) - _softwarePWM(15, 125); - - while(millis()-tmp < HIGH_LIMIT_TIMEOUT) - _softwarePWM(15, 145); - - }else if (soft_start_level==SOFT_START_SLOW){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT) - _softwarePWM(15, 155); - - while(millis()-tmp < HIGH_LIMIT_TIMEOUT) - _softwarePWM(5, 165); - } + while(millis()-tmp < HIGH_LIMIT_TIMEOUT) + _softwarePWM(25 + soft_start_level, 480 - soft_start_level); digitalWrite(SOFT_START_CONTROL_PIN,HIGH); } diff --git a/src/Braccio.h b/src/Braccio.h index 16c70e9..7118b7f 100644 --- a/src/Braccio.h +++ b/src/Braccio.h @@ -24,11 +24,7 @@ #include // You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 -#define SOFT_START_DISABLED 0 -//You can set different values for the softstart. SOFT_START_SLOW is the default value -#define SOFT_START_FAST 2 -#define SOFT_START_MEDIUM 3 -#define SOFT_START_SLOW 4 +#define SOFT_START_DISABLED -999 //The software PWM is connected to PIN 12. You cannot use the pin 12 if you are using //a Braccio shield V4 or newer From 5374d3c052192a6ab3e10a08a6ca405e69db8b85 Mon Sep 17 00:00:00 2001 From: astronomer80 Date: Tue, 13 Sep 2016 14:26:54 +0200 Subject: [PATCH 6/7] some minor fix and adding example sketch braccioOfUnoWiFi --- README.adoc | 2 +- .../braccioOfUnoWiFi/braccioOfUnoWiFi.ino | 265 ++++++++++++++++++ keywords.txt | 5 - library.properties | 2 +- src/Braccio.cpp | 46 +-- src/Braccio.h | 21 +- 6 files changed, 292 insertions(+), 49 deletions(-) create mode 100644 examples/braccioOfUnoWiFi/braccioOfUnoWiFi.ino diff --git a/README.adoc b/README.adoc index dc49988..210bbbc 100644 --- a/README.adoc +++ b/README.adoc @@ -3,7 +3,7 @@ The library offers easy access to the data from the onboard TinkerKit Braccio, and provides moves for the Braccio. - Upgrades: -Soft-Start: Used with the Braccio Shield V4 create a slow and soft start for the TinkerKit Braccio +Soft-Start: Used with the Braccio Shield V4 create perform a soft start for the TinkerKit Braccio. It protects the servo motors and the junctions from damages For more information about this library please visit us at http://www.arduino.org/products/tinkerkit/17-arduino-tinkerkit/arduino-tinkerkit-braccio diff --git a/examples/braccioOfUnoWiFi/braccioOfUnoWiFi.ino b/examples/braccioOfUnoWiFi/braccioOfUnoWiFi.ino new file mode 100644 index 0000000..8adbc56 --- /dev/null +++ b/examples/braccioOfUnoWiFi/braccioOfUnoWiFi.ino @@ -0,0 +1,265 @@ +/* + + braccioOfUnoWifi.ino + + Based on Arduino Uno WiFi Rest Server example + + This example for the Arduino Uno WiFi shows how to + control a TinkerKit Braccio through REST calls. + You can create your how mobile app or your + browser app to control the Braccio in wireless mode + + Note that with the Braccio shield version less than V4 + you need to disconnect the pin A4 from the shield to the board + + Possible commands created in this shetch: + + * "/arduino/custom/base/value:80" -> Moves the base of the Braccio at 80 degrees + * "/arduino/custom/shoulder/value:150" -> Moves the shoulder of the Braccio at 150 degrees + * "/arduino/custom/elbow/value:45" -> Moves the elbow of the Braccio at 45 degrees + * "/arduino/custom/wristv/value:10" -> Moves the wristv of the Braccio at 10 degrees + * "/arduino/custom/wristr/value:120" -> Moves the wristr of the Braccio at 120 degrees + * "/arduino/custom/gripper/value:73" -> Close the gripper + * "/arduino/custom/ledon" -> Turn ON the LED 13 + * "/arduino/custom/ledoff" -> Turn OFF the LED 13 + * "/arduino/custom/servo:3/value:73" -> Moves the servo to the pin 3 at 73 degrees + * "/arduino/custom/sayciao" -> Run the function sayciao(). The Braccio say "Ciao" with the gripper + * "/arduino/custom/takesponge" -> Run the function takesponge(). The Braccio take the big sponge you can find in the its box + * "/arduino/custom/showsponge" -> Run the function showsponge(). The Braccio show the sponge to the user + * "/arduino/custom/throwsponge" -> Run the function throwsponge(). The Braccio throw away the sponge + + This example code is part of the public domain + + http://labs.arduino.org/RestServer+and+RestClient + http://www.arduino.org/products/tinkerkit/arduino-tinkerkit-braccio + +*/ + +#include +#include +#include +#include + +//Initial Value for each Motor +int m1 = 0; +int m2 = 45; +int m3 = 180; +int m4 = 180; +int m5 = 90; +int m6 = 0; + +boolean moveBraccio = false; + +Servo base; +Servo shoulder; +Servo elbow; +Servo wrist_rot; +Servo wrist_ver; +Servo gripper; + + +void setup() { + //Intitialization of Braccio + Braccio.begin(); + //Intitialization of the Uno WiFi + Wifi.begin(); + Wifi.println("REST Server is up"); +} + +void loop() { + //Wait until the board receives HTTP commands + while (Wifi.available()) { + process(Wifi); + } + delay(50); +} + +/** +Parse Command from REST +It parse a command like: /arduino/custom/base/value:45 +@param command: The message to parse +@param type: the key for parsing +@return the value for the key +*/ +int parseCommand(String command, String type) { + int typeIndex = command.indexOf(type); + int dotsIndex = command.indexOf(':', typeIndex + type.length()); + + int idxtmp = dotsIndex + 4; + if ((dotsIndex + 4) > command.length()) idxtmp = command.length(); + String tmp = command.substring(dotsIndex + 1, idxtmp); + + return tmp.toInt(); +} + +/** +It process data from the HTTP protocol +*/ +void process(WifiData client) { + // read the command + String command = client.readString(); + command.toUpperCase(); + + if(command.indexOf("CUSTOM")==-1){ + client.println("Invalid command: " + command + ""); + return; + } + + //The message from sender + String message = command.substring(16); + //client.println(message); //Debug + + /* + For each message perform the proper command + */ + if (message == "LEDON") { + //Turn ON Led 13 + digitalWrite(13, HIGH); + //Return message to the sender (Eg: the browser) + client.println("alert('Led D13 ON');"); + } + else if (message == "LEDOFF") { + digitalWrite(13, LOW); + client.println("alert('Led D13 OFF');"); + } + //This command allow you to move a desired servo motor giving the + //PWM pin where is connected + //eg: http://192.168.240.1/arduino/custom/servo:3/value:45 or http://192.168.240.1/arduino/custom/base/value:45 + else if (message.startsWith("SERVO")) { + //Parse the message to retrive what is the servo to move + int servo = parseCommand(message, "SERVO"); + //Parse the message to retrive what is the value for the servo + int value = parseCommand(message, "VALUE"); + + client.println("Message:" + String(message) + "SERVO: " + String(servo) + " " + String(value)); + + moveBraccio=true; + } + //http://192.168.240.1/arduino/custom/base:45 or http://192.168.240.1/arduino/custom/base/value:45 + //Command for the base of the braccio (M1) + else if (message.startsWith("BASE")) { + m1 = parseCommand(message, "VALUE"); + moveBraccio = true; + client.println("BASE: " + String(m1)); + } + //Command for the shoulder of the braccio (M2) + else if (message.startsWith("SHOULDER")) { + m2 = parseCommand(message, "VALUE"); + moveBraccio = true; + client.println("SHOULDER: " + String(m2)); + } + //Command for the elbow of the braccio (M3) + else if (message.startsWith("ELBOW")) { + m3 = parseCommand(message, "VALUE"); + moveBraccio = true; + client.println("ELBOW: " + String(m3)); + } + //Command for the wrist of the braccio to move it up and down (M4) + else if (message.startsWith("WRISTV")) { + m4 = parseCommand(message, "VALUE"); + moveBraccio = true; + client.println("WRISTV: " + String(m4)); + } + //Command for the wrist of the braccio to rotate it (M5) + else if (message.startsWith("WRISTR")) { + m5 = parseCommand(message, "VALUE"); + moveBraccio = true; + client.println("WRISTR: " + String(m5)); + } + //Command for the GRIPPER of the braccio to open and close it (M6) + else if (message.startsWith("GRIPPER")) { + m6 = parseCommand(message, "VALUE"); + moveBraccio = true; + client.println("GRIPPER: " + String(m6)); + } + //Command to say "Ciao" + else if (message.startsWith("SAYCIAO")) { + sayCiao(); + client.println("SAYCIAO: " + String(m6)); + } + //Command for take the sponge + else if (message.startsWith("TAKESPONGE")) { + takesponge(); + client.println("TAKESPONGE: " + String(m6)); + } + //Command for show the sponge + else if (message.startsWith("SHOWSPONGE")) { + showsponge(); + client.println("SHOWSPONGE: " + String(m6)); + } + //Command for throw away the sponge + else if (message.startsWith("THROWSPONGE")) { + throwsponge(); + client.println("THROWSPONGE: " + String(m6)); + } + else + client.println("command error: " + message); + + //if flag moveBraccio is true fire the movement + if (moveBraccio) { + //client.println("moveBraccio"); + Braccio.ServoMovement(20, m1, m2, m3, m4, m5, m6); + moveBraccio = false; + } + + client.flush(); +} + +/** +The braccio Say 'Ciao' with the GRIPPER +*/ +void sayCiao() { + Braccio.ServoMovement(20, 90, 0, 180, 160, 0, 15); + + for (int i = 0; i < 5; i++) { + Braccio.ServoMovement(10, 90, 0, 180, 160, 0, 15); + delay(500); + + Braccio.ServoMovement(10, 90, 0, 180, 160, 0, 73); + delay(500); + } +} + +/** +Braccio take the Sponge +*/ +void takesponge() { + //starting position + //(step delay M1 , M2 , M3 , M4 , M5 , M6); + Braccio.ServoMovement(20, 0, 45, 180, 180, 90, 0); + + //I move arm towards the sponge + Braccio.ServoMovement(20, 0, 90, 180, 180, 90, 0); + + //the GRIPPER takes the sponge + Braccio.ServoMovement(20, 0, 90, 180, 180, 90, 60 ); + + //up the sponge + Braccio.ServoMovement(20, 0, 45, 180, 45, 0, 60); +} + + +/** +Braccio show the sponge to the user +*/ +void showsponge() { + for (int i = 0; i < 2; i++) { + + //(step delay M1 , M2 , M3 , M4 , M5 , M6 ); + Braccio.ServoMovement(10, 0, 45, 180, 45, 180, 60); + + Braccio.ServoMovement(10, 0, 45, 180, 45, 0, 60); + } +} + +/** +Braccio throw away the sponge +*/ +void throwsponge() { + //(step delay M1 , M2 , M3 , M4 , M5 , M6 ); + Braccio.ServoMovement(20, 0, 45, 90, 45, 90, 60); + + Braccio.ServoMovement(5, 0, 45, 135, 90, 90, 60); + + Braccio.ServoMovement(5, 0, 90, 150, 90, 90, 0); +} diff --git a/keywords.txt b/keywords.txt index c8e923d..39c6dd8 100644 --- a/keywords.txt +++ b/keywords.txt @@ -13,8 +13,6 @@ Braccio KEYWORD3 begin KEYWORD2 ServoMovement KEYWORD2 - - ####################################### # Constants ####################################### @@ -32,6 +30,3 @@ step_elbow LITERAL1 step_wrist_rot LITERAL1 step_wrist_ver LITERAL1 step_gripper LITERAL1 -FAST LITERAL1 -MEDIUM LITERAL1 -SLOW LITERAL1 diff --git a/library.properties b/library.properties index d365483..301f628 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Braccio -version=1.0 +version=2.0 author=Andrea Martino maintainer=Andrea sentence=For Arduino braccio only. diff --git a/src/Braccio.cpp b/src/Braccio.cpp index 3af382c..73e497d 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -1,6 +1,6 @@ /* - Braccio.cpp - board library Version 1.1 - Written by Andrea Martino + Braccio.cpp - board library Version 2.0 + Written by Andrea Martino and Angelo Ferrante This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public @@ -43,10 +43,10 @@ _Braccio::_Braccio() { /** * Braccio initialization and set intial position * Modifing this function you can set up the initial position of all the - * servo motors of the Braccio - * @param soft_start_level: SOFT_START_DISABLED, SOFT_START_SLOW, SOFT_START_MEDIUM, SOFT_START_FAST + * servo motors of Braccio + * @param soft_start_level: from -10 to +10, default value is 0 (SOFT_START_DEFAULT) * You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 - * SOFT_START_DISABLED disable the movement + * SOFT_START_DISABLED disable the Braccio movements */ unsigned int _Braccio::begin(int soft_start_level) { if(soft_start_level!=SOFT_START_DISABLED){ @@ -87,8 +87,8 @@ unsigned int _Braccio::begin(int soft_start_level) { Default implementation */ unsigned int _Braccio::begin() { - //SOFT_START_SLOW is the default value - return begin(SOFT_START_SLOW); + //SOFT_START_DEFAULT is the default value + return begin(SOFT_START_DEFAULT); } /* @@ -104,34 +104,18 @@ void _Braccio::_softwarePWM(int high_time, int low_time){ } /* -This function, used only with the Braccio Shield V4 and greater, -turn ON the Braccio softly and slowly. -The SOFT_START_CONTROL_PIN is used as a software PWM -@parameter soft_start_level: The soft start level: FAST, MEDIUM, SLOW +* This function, used only with the Braccio Shield V4 and greater, +* turn ON the Braccio softly and save it from brokes. +* The SOFT_START_CONTROL_PIN is used as a software PWM +* @param soft_start_level: from -10 to +10, default value is 0 (SOFT_START_DEFAULT) */ void _Braccio::_softStart(int soft_start_level){ long int tmp=millis(); - if (soft_start_level==SOFT_START_FAST){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT) - _softwarePWM(125, 50); + while(millis()-tmp < LOW_LIMIT_TIMEOUT) + _softwarePWM(30+soft_start_level, 500 - soft_start_level); - while(millis()-tmp < HIGH_LIMIT_TIMEOUT) - _softwarePWM(125, 105); - - }else if (soft_start_level==SOFT_START_MEDIUM){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT) - _softwarePWM(15, 125); - - while(millis()-tmp < HIGH_LIMIT_TIMEOUT) - _softwarePWM(15, 145); - - }else if (soft_start_level==SOFT_START_SLOW){ - while(millis()-tmp < LOW_LIMIT_TIMEOUT) - _softwarePWM(15, 155); - - while(millis()-tmp < HIGH_LIMIT_TIMEOUT) - _softwarePWM(5, 165); - } + while(millis()-tmp < HIGH_LIMIT_TIMEOUT) + _softwarePWM(25 + soft_start_level, 480 - soft_start_level); digitalWrite(SOFT_START_CONTROL_PIN,HIGH); } diff --git a/src/Braccio.h b/src/Braccio.h index 16c70e9..ca57b20 100644 --- a/src/Braccio.h +++ b/src/Braccio.h @@ -1,6 +1,6 @@ /* - Braccio.h - board library Version 1.1 - Written by Andrea Martino + Braccio.h - board library Version 2.0 + Written by Andrea Martino and Angelo Ferrante This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public @@ -24,11 +24,10 @@ #include // You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 -#define SOFT_START_DISABLED 0 -//You can set different values for the softstart. SOFT_START_SLOW is the default value -#define SOFT_START_FAST 2 -#define SOFT_START_MEDIUM 3 -#define SOFT_START_SLOW 4 +#define SOFT_START_DISABLED -999 + +//The default value for the soft start +#define SOFT_START_DEFAULT 0 //The software PWM is connected to PIN 12. You cannot use the pin 12 if you are using //a Braccio shield V4 or newer @@ -51,7 +50,7 @@ class _Braccio { unsigned int begin(); /** - * @param soft_start_level: the softstart_level: SOFT_START_DISABLED, SOFT_START_SLOW, SOFT_START_MEDIUM, SOFT_START_FAST + * @param soft_start_level: from -10 to +10, default value is 0 (SOFT_START_DEFAULT) * You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 */ unsigned int begin(int soft_start_level); @@ -64,10 +63,10 @@ class _Braccio { private: /* - * This function, used only with the Braccio Shield V4 and greater, - * turn ON the Braccio softly and slowly. + * This function, used only with the Braccio Shield V4 and greater, + * turn ON the Braccio softly and save Braccio from brokes. * The SOFT_START_CONTROL_PIN is used as a software PWM - * @parameter soft_start_level: The soft start level: FAST, MEDIUM, SLOW + * @param soft_start_level: from -10 to +10, default value is 0 (SOFT_START_DEFAULT) */ void _softStart(int soft_start_level); From 49a536b9f9ec1b45a7b68d08c02ab2c45b2012a1 Mon Sep 17 00:00:00 2001 From: astronomer80 Date: Tue, 13 Sep 2016 16:22:41 +0200 Subject: [PATCH 7/7] minor fix --- library.properties | 2 +- src/Braccio.cpp | 8 -------- src/Braccio.h | 16 ++++++---------- 3 files changed, 7 insertions(+), 19 deletions(-) diff --git a/library.properties b/library.properties index 301f628..95d706d 100644 --- a/library.properties +++ b/library.properties @@ -6,4 +6,4 @@ sentence=For Arduino braccio only. paragraph= Arduino Braccio category=Device Control url=www.arduino.org -architectures=avr +architectures=avr, samd, sam diff --git a/src/Braccio.cpp b/src/Braccio.cpp index 3fc6ff2..343f10c 100644 --- a/src/Braccio.cpp +++ b/src/Braccio.cpp @@ -84,14 +84,6 @@ unsigned int _Braccio::begin(int soft_start_level) { return 1; } -/* -Default implementation -*/ -unsigned int _Braccio::begin() { - //SOFT_START_DEFAULT is the default value - return begin(SOFT_START_DEFAULT); -} - /* Software implementation of the PWM for the SOFT_START_CONTROL_PIN,HIGH @param high_time: the time in the logic level high diff --git a/src/Braccio.h b/src/Braccio.h index 371b0b9..77f9675 100644 --- a/src/Braccio.h +++ b/src/Braccio.h @@ -41,19 +41,15 @@ class _Braccio { public: _Braccio(); - + /** - * Braccio initializations and set intial position - * Modifing this function you can set up the initial position of all the - * servo motors of the Braccio - */ - unsigned int begin(); - - /** - * @param soft_start_level: from -10 to +10, default value is 0 (SOFT_START_DEFAULT) + * Braccio initializations and set intial position + * Modifing this function you can set up the initial position of all the + * servo motors of the Braccio + *@param soft_start_level: from -10 to +10, default value is 0 (SOFT_START_DEFAULT) * You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 */ - unsigned int begin(int soft_start_level); + unsigned int begin(int soft_start_level=SOFT_START_DEFAULT); /** * This function allow the user to control all the servo motors in the Braccio