├── library.properties ├── .gitignore ├── keywords.txt ├── examples ├── openGripper │ └── openGripper.ino ├── simpleMoves │ └── simpleMoves.ino ├── positions │ └── positions.ino └── serialBraccio │ └── serialBraccio.ino ├── README.md ├── src ├── Position.h ├── BraccioRobot.h ├── Position.cpp └── BraccioRobot.cpp ├── moveRobot.py └── LICENSE /library.properties: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stefangs/arduino-library-braccio-robot/HEAD/library.properties -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | *.idea 4 | venv 5 | 6 | # Compiled Object files 7 | *.slo 8 | *.lo 9 | *.o 10 | *.obj 11 | 12 | # Precompiled Headers 13 | *.gch 14 | *.pch 15 | 16 | # Compiled Dynamic libraries 17 | *.so 18 | *.dylib 19 | *.dll 20 | 21 | # Fortran module files 22 | *.mod 23 | *.smod 24 | 25 | # Compiled Static libraries 26 | *.lai 27 | *.la 28 | *.a 29 | *.lib 30 | 31 | # Executables 32 | *.exe 33 | *.out 34 | *.app 35 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For BraccioRobot 3 | ####################################### 4 | # Class 5 | ####################################### 6 | 7 | BraccioRobot KEYWORD3 8 | Position KEYWORD3 9 | 10 | ####################################### 11 | # Methods and Functions 12 | ####################################### 13 | 14 | init KEYWORD2 15 | moveToPosition KEYWORD2 16 | powerOff KEYWORD2 17 | powerOn KEYWORD2 18 | setStartSpeed KEYWORD2 19 | getStartSpeed KEYWORD2 20 | setBase KEYWORD2 21 | setShoulder KEYWORD2 22 | setElbow KEYWORD2 23 | setWristRotation KEYWORD2 24 | setWrist KEYWORD2 25 | setGripper KEYWORD2 26 | set KEYWORD2 27 | getBase KEYWORD2 28 | getShoulder KEYWORD2 29 | getElbow KEYWORD2 30 | getWristRotation KEYWORD2 31 | getWrist KEYWORD2 32 | getGripper KEYWORD2 33 | Position KEYWORD2 34 | maxPositionDiff KEYWORD2 35 | setFromString KEYWORD2 36 | 37 | 38 | 39 | 40 | ####################################### 41 | # Constants 42 | ####################################### 43 | 44 | MAXIMUM_SPEED LITERAL1 45 | MINIMUM_SPEED LITERAL1 46 | 47 | -------------------------------------------------------------------------------- /examples/openGripper/openGripper.ino: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | openGripper.ino 4 | 5 | This sketch shows how to open gripper 6 | 7 | Created 01012019 8 | by Stefan Strömberg 9 | 10 | This example is in the public domain. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | void setup() { 17 | //init() will start the robot and move to the initial position, which is: 18 | //Base (M1): 90 degrees 19 | //Shoulder (M2): 90 degrees 20 | //Elbow (M3): 90 degrees 21 | //Wrist (M4): 90 degrees 22 | //Wrist rot(M5): 90 degrees 23 | //gripper (M6): 72 degrees 24 | //The initial position can be changed by supplying a Position as a parameter 25 | //to init() 26 | BraccioRobot.init(); 27 | } 28 | 29 | Position pos; 30 | 31 | void loop() { 32 | // Set the position 33 | // M1=base degrees. Allowed values from 0 to 180 degrees 34 | // M2=shoulder degrees. Allowed values from 15 to 165 degrees 35 | // M3=elbow degrees. Allowed values from 0 to 180 degrees 36 | // M4=wrist degrees. Allowed values from 0 to 180 degrees 37 | // M5=wrist rotation degrees. Allowed values from 0 to 180 degrees 38 | // M6=gripper degrees. Allowed values from 10 to 73 degrees. 10: the toungue is open, 73: the gripper is closed. 39 | // (M1, M2, M3, M4, M5, M6) 40 | pos.set(90, 90, 90, 90, 90, GRIPPER_OPEN); 41 | 42 | // Move the robot to the position with a specified speed between 20-200 degrees per second 43 | BraccioRobot.moveToPosition(pos, 100); 44 | 45 | //Wait 1 second 46 | delay(1000); 47 | } 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # BraccioRobot 2 | This is a library to control the Tinkerkit Braccio robot arm. The library supports the following features: 3 | * Configurable acceleration and retardation for the movements 4 | * Speeds are adjusted per joint, so all joints reach a new position simultaneously regradless of travel distance 5 | * Can power the arm on and off 6 | * Separate class for handling arm positions 7 | * Configurable start position 8 | * Supports soft start 9 | * Includes example which allows control of the robot via the serial port 10 | * Configurable start and max speed for movements 11 | * Automatically limits speed for the large joints 12 | * Speed specified in degrees per second 13 | * Includes a simple Python program (moveRobot.py) for interactive robot movements 14 | 15 | When the robot is moved from one position to another, it starts at a 16 | specified start speed and accelerates to the specified travel speed. 17 | Before reaching the end of the movement it decelerates back to the start speed. 18 | 19 | `BraccioRobot.moveToPosition(pos, 100);` 20 | 21 | The travel speed is specified in degrees per second, for example 100 22 | degrees per second as in the example above. The speed can be between 23 | 20 and 200 degrees per second. 24 | 25 | The start speed is set by the command: 26 | 27 | `BraccioRobot.setStartSpeed(speed);` 28 | 29 | If the specified start speed is higher than the move speed, then the start 30 | speed is ignored. This means that the acceleration can be turned off by 31 | setting start speed to MAXIMUM_SPEED: 32 | 33 | `BraccioRobot.setStartSpeed(MAXIMUM_SPEED)` 34 | 35 | You can also turn the power to the robot servos on or off by the commands: 36 | 37 | `BraccioRobot.powerOn();` 38 | 39 | `BraccioRobot.powerOff();` 40 | -------------------------------------------------------------------------------- /examples/simpleMoves/simpleMoves.ino: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | simpleMoves.ino 4 | 5 | This sketch shows how to move the Braccio to different positions 6 | 7 | Created 01012019 8 | by Stefan Strömberg 9 | 10 | This example is in the public domain. 11 | */ 12 | 13 | #include 14 | #include 15 | 16 | void setup() { 17 | //init() will start the robot and move to the initial position, which is: 18 | //Base (M1): 90 degrees 19 | //Shoulder (M2): 90 degrees 20 | //Elbow (M3): 90 degrees 21 | //Wrist (M4): 90 degrees 22 | //Wrist rot(M5): 90 degrees 23 | //gripper (M6): 72 degrees 24 | //The initial position can be changed by supplying a Position as a parameter 25 | //to init() 26 | BraccioRobot.init(); 27 | } 28 | 29 | Position pos; 30 | 31 | void loop() { 32 | // Set the position 33 | // M1=base degrees. Allowed values from 0 to 180 degrees 34 | // M2=shoulder degrees. Allowed values from 15 to 165 degrees 35 | // M3=elbow degrees. Allowed values from 0 to 180 degrees 36 | // M4=wrist degrees. Allowed values from 0 to 180 degrees 37 | // M5=wrist rotation degrees. Allowed values from 0 to 180 degrees 38 | // M6=gripper degrees. Allowed values from 10 to 73 degrees. 10: the toungue is open, 73: the gripper is closed. 39 | // (M1, M2, M3, M4, M5, M6) 40 | pos.set( 0, 15, 180, 170, 0, 73); 41 | 42 | // Move the robot to the position with a specified speed between 20-200 degrees per second 43 | BraccioRobot.moveToPosition(pos, 100); 44 | 45 | //Wait 1 second 46 | delay(1000); 47 | 48 | // Move the robot to a new position with speed 50 degrees per second. 49 | // Note that you can set the position at the same time as you use it 50 | BraccioRobot.moveToPosition(pos.set(180, 165, 0, 0, 180, 10), 50); 51 | 52 | //Wait 1 second 53 | delay(1000); 54 | } 55 | -------------------------------------------------------------------------------- /src/Position.h: -------------------------------------------------------------------------------- 1 | /* 2 | File: Position.h 3 | 4 | Copyright 2018 Stefan Strömberg, stefangs@nethome.nu 5 | 6 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except 7 | in compliance with the License. You may obtain a copy of the License at 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | Unless required by applicable law or agreed to in writing, software distributed under the License 10 | is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 11 | either express or implied. See the License for the specific language governing permissions and 12 | limitations under the License. 13 | */ 14 | 15 | #ifndef POSITION_H_ 16 | #define POSITION_H_ 17 | 18 | #define GRIPPER_OPEN 10 19 | #define GRIPPER_CLOSED 73 20 | 21 | class Position { 22 | public: 23 | Position& setBase(int b); 24 | Position& setShoulder(int s); 25 | Position& setElbow(int e); 26 | Position& setWristRotation(int w); 27 | Position& setWrist(int v); 28 | Position& setGripper(int g); 29 | Position& set(int basePos, int shoulderPos, int elbowPos, int wristPos, int wristRotationPos, int gripperPos); 30 | 31 | inline int getBase() const {return base;} 32 | inline int getShoulder() const {return shoulder;} 33 | inline int getElbow() const {return elbow;} 34 | inline int getWristRotation() const {return wristRotation;} 35 | inline int getWrist() const {return wrist;} 36 | inline int getGripper() const {return gripper;} 37 | 38 | Position(); 39 | Position(int basePos, int ShoulderPos, int elbowPos, int wristPos, int wristRotationPos, int gripperPos); 40 | int maxPositionDiff(const Position& p) const; 41 | int setFromString(char* string); 42 | 43 | Position& operator=(const Position& p); 44 | 45 | private: 46 | int limit(int value, int minv, int maxv); 47 | int parseInt(char *&in, bool &isSuccess); 48 | int base; 49 | int shoulder; 50 | int elbow; 51 | int wrist; 52 | int wristRotation; 53 | int gripper; 54 | }; 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/BraccioRobot.h: -------------------------------------------------------------------------------- 1 | /* 2 | File: BraccioRobot.h 3 | 4 | Copyright 2018 Stefan Strömberg, stefangs@nethome.nu 5 | 6 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except 7 | in compliance with the License. You may obtain a copy of the License at 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | Unless required by applicable law or agreed to in writing, software distributed under the License 10 | is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 11 | either express or implied. See the License for the specific language governing permissions and 12 | limitations under the License. 13 | */ 14 | 15 | #ifndef BRACCIO_ROBOT_H_ 16 | #define BRACCIO_ROBOT_H_ 17 | 18 | #include 19 | #ifndef Servo_h 20 | #error Sketch must include Servo.h 21 | #endif 22 | 23 | #include "Position.h" 24 | 25 | #define MAXIMUM_SPEED 200 26 | #define MINIMUM_SPEED 20 27 | 28 | class _BraccioRobot { 29 | 30 | public: 31 | static Position initialPosition; 32 | void init(Position& startPosition=_BraccioRobot::initialPosition, bool doSoftStart=true); 33 | 34 | /* Moves the arm to the specified position with the specified speed. The movement will start at startSpeed and accelerate 35 | towards specified speed. At the end of the movement it will decelerate back to startSpeed before reaching 36 | end position. Speeds are adjusted so that all joints will start and finish at the same time regardless 37 | of varying travel distance. Speed is in degrees/second and can be between 20 and 200 */ 38 | void moveToPosition(const Position& newPosition, int speed); 39 | 40 | /* Turns off power to the servo motors. This only work if you are using a robot shield later than V1.6. 41 | Note that after a call to init() the power is on */ 42 | void powerOff(); 43 | 44 | /* Turns on power to the servo motors. This only work if you are using a robot shield later than V1.6 */ 45 | void powerOn(); 46 | 47 | /* Set start speed of movement in degrees/second. Between 20 and 200 */ 48 | void setStartSpeed(int speed); 49 | inline int getStartSpeed() const {return startSpeed;} 50 | 51 | 52 | private: 53 | Servo base; 54 | Servo shoulder; 55 | Servo elbow; 56 | Servo wristRotation; 57 | Servo wrist; 58 | Servo gripper; 59 | Position currentPosition; 60 | int startSpeed; 61 | 62 | void softStart(); 63 | }; 64 | 65 | extern _BraccioRobot BraccioRobot; 66 | 67 | #endif // BRACCIO_ROBOT_H_ 68 | -------------------------------------------------------------------------------- /examples/positions/positions.ino: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | positions.ino 4 | 5 | This sketch shows how to move the Braccio to different positions 6 | with different options. The example picks up an object at a pickup 7 | position and puts it down at the put down position and then moves it 8 | back again. 9 | 10 | Created 01012019 11 | by Stefan Strömberg 12 | 13 | This example is in the public domain. 14 | */ 15 | 16 | #include 17 | #include 18 | #define GRIPPER_HALF_CLOSED 64 19 | 20 | // Create a custom initial position 21 | // M1=base degrees. Allowed values from 0 to 180 degrees 22 | // M2=shoulder degrees. Allowed values from 15 to 165 degrees 23 | // M3=elbow degrees. Allowed values from 0 to 180 degrees 24 | // M4=wrist degrees. Allowed values from 0 to 180 degrees 25 | // M5=wrist rotation degrees. Allowed values from 0 to 180 degrees 26 | // M6=gripper degrees. Allowed values from 10 to 73 degrees. 10: the toungue is open, 73: the gripper is closed. 27 | // (M1, M2, M3, M4, M5, M6) 28 | Position myInitialPosition(90, 90, 90, 90, 45, GRIPPER_CLOSED); 29 | 30 | Position readyPosition(87, 135, 0, 57, 90, 10); 31 | Position pickUpPosition(120, 84, 0, 42, 90, 10); 32 | Position putDownPosition; 33 | 34 | void setup() { 35 | //init() will start the robot and move to the supplied initial position. 36 | // If no initial position is supplied, it will go to the default initial position 37 | BraccioRobot.init(myInitialPosition); 38 | 39 | // The put down position is just 60 degrees rotated from the pick up position 40 | putDownPosition = pickUpPosition; 41 | putDownPosition.setBase(60); 42 | } 43 | 44 | // Moves an object between two specified positions 45 | void moveBetweenPositions(Position& fromPosition, Position& toPosition) { 46 | 47 | // Move to the ready position with open gripper 48 | BraccioRobot.moveToPosition(readyPosition.setGripper(GRIPPER_OPEN), 100); 49 | 50 | // Move to the fromPosition with open gripper 51 | BraccioRobot.moveToPosition(fromPosition.setGripper(GRIPPER_OPEN), 100); 52 | 53 | // Close the gripper slowly at fromPosition 54 | BraccioRobot.moveToPosition(fromPosition.setGripper(GRIPPER_HALF_CLOSED), 20); 55 | 56 | // Move at moderate speed to the ready position with closed gripper 57 | BraccioRobot.moveToPosition(readyPosition.setGripper(GRIPPER_HALF_CLOSED), 50); 58 | 59 | // Move at moderate speed to the toPosition with closed gripper 60 | BraccioRobot.moveToPosition(toPosition.setGripper(GRIPPER_HALF_CLOSED), 50); 61 | 62 | // Open the gripper slowly at the toPosition 63 | BraccioRobot.moveToPosition(toPosition.setGripper(GRIPPER_OPEN), 20); 64 | 65 | // Move to the ready position with open gripper 66 | BraccioRobot.moveToPosition(readyPosition.setGripper(GRIPPER_OPEN), 100); 67 | } 68 | 69 | void loop() { 70 | 71 | // Move from pickUpPosition to putDownPosition 72 | moveBetweenPositions(pickUpPosition, putDownPosition); 73 | 74 | // Move back from putDownPosition to pickUpPosition 75 | moveBetweenPositions(putDownPosition, pickUpPosition); 76 | 77 | //Wait 5 seconds 78 | delay(5000); 79 | } 80 | -------------------------------------------------------------------------------- /examples/serialBraccio/serialBraccio.ino: -------------------------------------------------------------------------------- 1 | /* 2 | File: serialBraccio.ino 3 | 4 | Copyright 2018 Stefan Strömberg, stefangs@nethome.nu 5 | 6 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except 7 | in compliance with the License. You may obtain a copy of the License at 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | Unless required by applicable law or agreed to in writing, software distributed under the License 10 | is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 11 | either express or implied. See the License for the specific language governing permissions and 12 | limitations under the License. 13 | */ 14 | 15 | /* 16 | This example reads robot positions from the serial port and puts 17 | the Braccio in those positions. A position is formatted as: 18 | 19 | P90,90,90,90,90,73,100 20 | 21 | Where the numbers are the joint angels in the same order as in the Position class and the last 22 | number is the speed of the movement in degrees per second. 23 | When the arm has reached the specified position it will answer with the string: 24 | 25 | OK 26 | 27 | You can also issue the Home command (H), to put the robot in the home position, the Off command (0) 28 | to turn off the robot or the On command (1) to turn on the robot: 29 | 30 | H 31 | 32 | Note, that when opening the serial port to Arduino, the Arduino restarts, which causes 33 | it to become non responsive for serial input for a couple of seconds. Therefore a 34 | controller program should wait at least 3 seconds after opening the port before sending 35 | any commands. An example in Python: 36 | 37 | s = serial.Serial('COM4', 115200, timeout=5) 38 | time.sleep(3) 39 | s.write(b'P0,90,20,90,90,73,100\n') 40 | print(s.readline()) 41 | s.write(b'P90,90,20,90,90,73,100\n') 42 | print(s.readline()) 43 | 44 | You should also wait until you get an "OK" (or "E0") from the Arduino before sending the 45 | next position, like in the above example using readline(). 46 | */ 47 | 48 | #include 49 | #include "BraccioRobot.h" 50 | #define INPUT_BUFFER_SIZE 50 51 | 52 | static char inputBuffer[INPUT_BUFFER_SIZE]; 53 | Position armPosition; 54 | 55 | void setup() { 56 | Serial.begin(115200); 57 | BraccioRobot.init(); 58 | } 59 | 60 | void loop() { 61 | handleInput(); 62 | } 63 | 64 | void handleInput() { 65 | if (Serial.available() > 0) { 66 | byte result = Serial.readBytesUntil('\n', inputBuffer, INPUT_BUFFER_SIZE); 67 | inputBuffer[result] = 0; 68 | interpretCommand(inputBuffer, result); 69 | } 70 | } 71 | 72 | void interpretCommand(char* inputBuffer, byte commandLength) { 73 | if (inputBuffer[0] == 'P') { 74 | positionArm(&inputBuffer[0]); 75 | } else if (inputBuffer[0] == 'H') { 76 | homePositionArm(); 77 | } else if (inputBuffer[0] == '0') { 78 | BraccioRobot.powerOff(); 79 | Serial.println("OK"); 80 | } else if (inputBuffer[0] == '1') { 81 | BraccioRobot.powerOn(); 82 | Serial.println("OK"); 83 | } else { 84 | Serial.println("E0"); 85 | } 86 | Serial.flush(); 87 | } 88 | 89 | void 90 | positionArm(char *in) { 91 | int speed = armPosition.setFromString(in); 92 | if (speed > 0) { 93 | BraccioRobot.moveToPosition(armPosition, speed); 94 | Serial.println("OK"); 95 | } else { 96 | Serial.println("E1"); 97 | } 98 | } 99 | 100 | void 101 | homePositionArm() { 102 | BraccioRobot.moveToPosition(armPosition.set(90, 90, 90, 90, 90, 73), 100); 103 | Serial.println("OK"); 104 | } 105 | -------------------------------------------------------------------------------- /src/Position.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | File: Position.cpp 3 | 4 | Copyright 2018 Stefan Strömberg, stefangs@nethome.nu 5 | 6 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except 7 | in compliance with the License. You may obtain a copy of the License at 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | Unless required by applicable law or agreed to in writing, software distributed under the License 10 | is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 11 | either express or implied. See the License for the specific language governing permissions and 12 | limitations under the License. 13 | */ 14 | 15 | #include "Position.h" 16 | #include "Arduino.h" 17 | 18 | Position& 19 | Position::setBase(int b) { 20 | base = limit(b, 0, 180); 21 | return *this; 22 | } 23 | 24 | Position& 25 | Position::setShoulder(int s) { 26 | shoulder = limit(s, 15, 165); 27 | return *this; 28 | } 29 | 30 | Position& 31 | Position::setElbow(int e){ 32 | elbow = limit(e, 0, 180); 33 | return *this; 34 | } 35 | 36 | Position& 37 | Position::setWristRotation(int w){ 38 | wristRotation = limit(w, 0, 180); 39 | return *this; 40 | } 41 | 42 | Position& 43 | Position::setWrist(int v){ 44 | wrist = limit(v, 0, 180); 45 | return *this; 46 | } 47 | 48 | Position& 49 | Position::setGripper(int g){ 50 | gripper = limit(g, 10, 73); 51 | return *this; 52 | } 53 | 54 | int 55 | Position::limit(int value, int minv, int maxv) { 56 | int result; 57 | if (value < minv) { 58 | result = minv; 59 | } else if (value > maxv) { 60 | result = maxv; 61 | } else { 62 | result = value; 63 | } 64 | return result; 65 | } 66 | 67 | Position::Position() { 68 | Position(90, 90, 90, 90, 90, 73); 69 | } 70 | 71 | Position::Position(int basePos, int shoulderPos, int elbowPos, int wristRotationPos, int wristPos, int gripperPos) { 72 | set(basePos, shoulderPos, elbowPos, wristRotationPos, wristPos, gripperPos); 73 | } 74 | 75 | Position& 76 | Position::set(int basePos, int shoulderPos, int elbowPos, int wristPos, int wristRotationPos, int gripperPos) { 77 | setBase(basePos); 78 | setShoulder(shoulderPos); 79 | setElbow(elbowPos); 80 | setWrist(wristPos); 81 | setWristRotation(wristRotationPos); 82 | setGripper(gripperPos); 83 | return *this; 84 | } 85 | 86 | Position& 87 | Position::operator=(const Position& p) { 88 | base = p.base; 89 | shoulder = p.shoulder; 90 | elbow = p.elbow; 91 | wristRotation = p.wristRotation; 92 | wrist = p.wrist; 93 | gripper = p.gripper; 94 | return *this; 95 | } 96 | 97 | int 98 | Position::maxPositionDiff(const Position& p) const { 99 | int maxDiff = 0; 100 | maxDiff = max(maxDiff, abs(base - p.base)); 101 | maxDiff = max(maxDiff, abs(shoulder - p.shoulder)); 102 | maxDiff = max(maxDiff, abs(elbow - p.elbow)); 103 | maxDiff = max(maxDiff, abs(wrist - p.wrist)); 104 | maxDiff = max(maxDiff, abs(wristRotation - p.wristRotation)); 105 | maxDiff = max(maxDiff, abs(gripper - p.gripper)); 106 | 107 | return maxDiff; 108 | } 109 | 110 | int 111 | Position::setFromString(char* string) { 112 | bool isSuccess = true; 113 | int speed; 114 | setBase(parseInt(string, isSuccess)); 115 | setShoulder(parseInt(string, isSuccess)); 116 | setElbow(parseInt(string, isSuccess)); 117 | setWrist(parseInt(string, isSuccess)); 118 | setWristRotation(parseInt(string, isSuccess)); 119 | setGripper(parseInt(string, isSuccess)); 120 | speed = parseInt(string, isSuccess); 121 | return isSuccess ? speed : -1; 122 | } 123 | 124 | int 125 | Position::parseInt(char *&in, bool &isSuccess) { 126 | bool success = false; 127 | int result = 0; 128 | // Read past separator(s) 129 | while(((*in > '9') || (*in < '0')) && (*in != 0)) { 130 | in++; 131 | } 132 | // Read integer 133 | while((*in <= '9') && (*in >= '0')) { 134 | result *= 10; 135 | result += (*in - '0'); 136 | in++; 137 | success = true; 138 | } 139 | isSuccess &= success; 140 | return result; 141 | } 142 | -------------------------------------------------------------------------------- /moveRobot.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import time 3 | import copy 4 | from tkinter import * 5 | 6 | 7 | class Position: 8 | def __init__(self, base, shoulder, elbow, wrist, wrist_rotation, gripper): 9 | self.angels = [0, 0, 0, 0, 0, 0] 10 | self.angels[0] = base 11 | self.angels[1] = shoulder 12 | self.angels[2] = elbow 13 | self.angels[3] = wrist 14 | self.angels[4] = wrist_rotation 15 | self.angels[5] = gripper 16 | 17 | def set(self, joint_number, value): 18 | self.angels[joint_number] = min(max(value, 0), 180) 19 | 20 | def get(self, joint_number): 21 | return self.angels[joint_number] 22 | 23 | def add(self, joint_number, value): 24 | self.set(joint_number, self.angels[joint_number] + value) 25 | 26 | def to_string(self): 27 | result = "" 28 | separator = "" 29 | for angel in self.angels: 30 | result = result + separator + str(angel) 31 | separator = "," 32 | return result 33 | 34 | 35 | class Braccio: 36 | def __init__(self, serial_port): 37 | self.port = serial.Serial(serial_port, 115200, timeout=5) 38 | time.sleep(3) 39 | 40 | def write(self, string): 41 | self.port.write(string.encode()) 42 | self.port.readline() 43 | 44 | def move_to_position(self, position, speed): 45 | self.write('P' + position.to_string() + ',' + str(speed) + '\n') 46 | 47 | def power_off(self): 48 | self.write('0\n') 49 | 50 | def power_on(self): 51 | self.write('0\n') 52 | 53 | 54 | moves = {'q': (0, 1), 'a': (0, -1), 55 | 'w': (1, 1), 's': (1, -1), 56 | 'e': (2, 1), 'd': (2, -1), 57 | 'r': (3, 1), 'f': (3, -1), 58 | 't': (4, 1), 'g': (4, -1), 59 | 'y': (5, 1), 'h': (5, -1)} 60 | 61 | 62 | position_names = {'0': 0, '1': 1, '2': 2, '3': 3, '4': 4, '5': 5, '6': 6, '7': 7, '8': 8, '9': 9} 63 | 64 | 65 | def move_joint(joint_number, angle): 66 | positions[current_position].add(joint_number, angle) 67 | robot.move_to_position(positions[current_position], move_speed) 68 | joint_labels[joint_number].config(text=str(positions[current_position].get(joint_number))) 69 | joint_labels[joint_number].update_idletasks() 70 | 71 | 72 | def change_position(position): 73 | global current_position, positions, last_position 74 | if positions[position] is None: 75 | positions[position] = copy.deepcopy(positions[current_position]) 76 | last_position = current_position 77 | current_position = position 78 | robot.move_to_position(positions[current_position], move_speed) 79 | position_label.config(text=str(position)) 80 | position_label.update_idletasks() 81 | for j in range(0, len(joints)): 82 | joint_labels[j].config(text=str(positions[current_position].get(j))) 83 | joint_labels[j].update_idletasks() 84 | 85 | 86 | def key_pressed(event): 87 | global positions 88 | if event.char in moves: 89 | move = moves[event.char] 90 | move_joint(move[0], move[1]) 91 | elif event.char in position_names: 92 | change_position(position_names[event.char]) 93 | elif event.char == "c": 94 | positions[current_position] = copy.deepcopy(positions[last_position]) 95 | change_position(current_position) 96 | 97 | 98 | def setup_gui(): 99 | global position_label 100 | root = Tk() 101 | root.title('Braccio Robot') 102 | root.bind('', key_pressed) 103 | Label(text='Move robot joints with keyboard keys\nChange positions with numerical keys') \ 104 | .grid(row=0, column=0, columnspan=2, padx=(30, 30)) 105 | Label(text='Current position:').grid(row=1, column=0) 106 | position_label = Label(text=str(current_position)) 107 | position_label.grid(row=1, column=1) 108 | 109 | i = 0 110 | for joint in joints: 111 | Label(text=joint).grid(row=2 + i, column=0) 112 | label = Label(text=str(positions[current_position].get(i))) 113 | label.grid(row=2 + i, column=1) 114 | joint_labels.append(label) 115 | i += 1 116 | root.mainloop() 117 | 118 | 119 | joints = ['Base [q,a]', 'Shoulder [w,s]', 'Elbow [e,d]', 'Wrist [r,f]', 'Wrist rotation [t,g]', 'Gripper [y,h]'] 120 | home = Position(90, 90, 90, 90, 90, 72) 121 | positions = [copy.deepcopy(home), None, None, None, None, None, None, None, None, None] 122 | current_position = 0 123 | last_position = 0 124 | move_speed = 100 125 | joint_labels = [] 126 | position_label = None 127 | 128 | port = input("Enter serial port: ") 129 | robot = Braccio(port) 130 | robot.move_to_position(home, 100) 131 | setup_gui() 132 | robot.move_to_position(home, 100) 133 | robot.power_off() 134 | -------------------------------------------------------------------------------- /src/BraccioRobot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | File: BraccioRobot.cpp 3 | 4 | Copyright 2018 Stefan Strömberg, stefangs@nethome.nu 5 | 6 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except 7 | in compliance with the License. You may obtain a copy of the License at 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | Unless required by applicable law or agreed to in writing, software distributed under the License 10 | is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, 11 | either express or implied. See the License for the specific language governing permissions and 12 | limitations under the License. 13 | */ 14 | 15 | #include "BraccioRobot.h" 16 | #include 17 | 18 | #define SOFT_START_CONTROL_PIN 12 19 | #define BIG_JOINT_MAXIMUM_SPEED 140 20 | #define DEFAULT_START_SPEED 40 21 | #define MS_PER_S 1000 22 | #define SOFT_START_TIME 1000 23 | 24 | Position _BraccioRobot::initialPosition(90, 90, 90, 90, 90, 73); 25 | _BraccioRobot BraccioRobot; 26 | 27 | void _BraccioRobot::init(Position& startPosition, bool doSoftStart) { 28 | if (doSoftStart) { 29 | pinMode(SOFT_START_CONTROL_PIN,OUTPUT); 30 | digitalWrite(SOFT_START_CONTROL_PIN,LOW); 31 | } 32 | startSpeed = DEFAULT_START_SPEED; 33 | base.attach(11); 34 | shoulder.attach(10); 35 | elbow.attach(9); 36 | wristRotation.attach(5); 37 | wrist.attach(6); 38 | gripper.attach(3); 39 | 40 | base.write(startPosition.getBase()); 41 | shoulder.write(startPosition.getShoulder()); 42 | elbow.write(startPosition.getElbow()); 43 | wrist.write(startPosition.getWrist()); 44 | wristRotation.write(startPosition.getWristRotation()); 45 | gripper.write(startPosition.getGripper()); 46 | if (doSoftStart) { 47 | softStart(); 48 | } 49 | currentPosition = startPosition; 50 | } 51 | 52 | void _BraccioRobot::softStart() { 53 | long int startTime=millis(); 54 | while(millis()-startTime < SOFT_START_TIME) { 55 | digitalWrite(SOFT_START_CONTROL_PIN,LOW); 56 | delayMicroseconds(450); 57 | digitalWrite(SOFT_START_CONTROL_PIN,HIGH); 58 | delayMicroseconds(20); 59 | } 60 | } 61 | 62 | void 63 | _BraccioRobot::powerOff() { 64 | digitalWrite(SOFT_START_CONTROL_PIN,LOW); 65 | } 66 | 67 | void 68 | _BraccioRobot::powerOn(){ 69 | digitalWrite(SOFT_START_CONTROL_PIN,HIGH); 70 | } 71 | 72 | 73 | void 74 | _BraccioRobot::setStartSpeed(int speed){ 75 | startSpeed = max(MINIMUM_SPEED, speed); 76 | startSpeed = min(MAXIMUM_SPEED, startSpeed); 77 | } 78 | 79 | void 80 | _BraccioRobot::moveToPosition(const Position& newPosition, int speed) { 81 | speed = max(MINIMUM_SPEED, speed); 82 | speed = min(MAXIMUM_SPEED, speed); 83 | int maxStepDelay = MS_PER_S/min(startSpeed, speed); 84 | int minStepDelay = MS_PER_S/speed; 85 | int stepDelay = maxStepDelay; 86 | int accellerationGap = maxStepDelay - minStepDelay; 87 | int accelleration = 1; 88 | double baseMove = currentPosition.getBase(); 89 | double shoulderMove = currentPosition.getShoulder(); 90 | double elbowMove = currentPosition.getElbow(); 91 | double wristRotationMove = currentPosition.getWristRotation(); 92 | double wristMove = currentPosition.getWrist(); 93 | double gripperMove = currentPosition.getGripper(); 94 | 95 | int maxMove = currentPosition.maxPositionDiff(newPosition); 96 | 97 | double baseStep = (newPosition.getBase() - currentPosition.getBase() + 0.0) / maxMove; 98 | double shoulderStep = (newPosition.getShoulder() - currentPosition.getShoulder() + 0.0) / maxMove; 99 | double elbowStep = (newPosition.getElbow() - currentPosition.getElbow() + 0.0) / maxMove; 100 | double wristRotationStep = (newPosition.getWristRotation() - currentPosition.getWristRotation() + 0.0) / maxMove; 101 | double wristStep = (newPosition.getWrist() - currentPosition.getWrist() + 0.0) / maxMove; 102 | double gripperStep = (newPosition.getGripper() - currentPosition.getGripper() + 0.0) / maxMove; 103 | 104 | /* Limit the speed of the big joints */ 105 | if ((abs(baseStep) > 0.6) || (abs(shoulderStep) > 0.6)) { 106 | minStepDelay = max(MS_PER_S/BIG_JOINT_MAXIMUM_SPEED, minStepDelay); 107 | } 108 | 109 | int retardationPoint = maxMove / 2; 110 | if (maxMove > accellerationGap * 2) { 111 | retardationPoint = maxMove - accellerationGap; 112 | } 113 | 114 | for (int i = 0; i < maxMove; i++) { 115 | baseMove += baseStep; 116 | base.write(int(baseMove)); 117 | shoulderMove += shoulderStep; 118 | shoulder.write(int(shoulderMove)); 119 | elbowMove += elbowStep; 120 | elbow.write(int(elbowMove)); 121 | wristRotationMove += wristRotationStep; 122 | wristRotation.write(int(wristRotationMove)); 123 | wristMove += wristStep; 124 | wrist.write(int(wristMove)); 125 | gripperMove += gripperStep; 126 | gripper.write(int(gripperMove)); 127 | 128 | delay(stepDelay); 129 | 130 | if (i == retardationPoint) { 131 | accelleration = -accelleration; 132 | } 133 | stepDelay = max(minStepDelay, stepDelay - accelleration); 134 | stepDelay = min(maxStepDelay, stepDelay); 135 | } 136 | currentPosition = newPosition; 137 | base.write(currentPosition.getBase()); 138 | shoulder.write(currentPosition.getShoulder()); 139 | elbow.write(currentPosition.getElbow()); 140 | wristRotation.write(currentPosition.getWristRotation()); 141 | wrist.write(currentPosition.getWrist()); 142 | gripper.write(currentPosition.getGripper()); 143 | } 144 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright 2018 Stefan Strömberg, stefangs@nethome.nu 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | --------------------------------------------------------------------------------