Sensor Kit For BRACCIO
1. Introduction
Hi! This will be my entry for the Sensors Project14. For this project I wanted to design a sensor kit for the BRACCIO robot arm. What do I mean by a sensor kit? Braccio is a robot arm kit made by Arduino (https://export.farnell.com/arduino/t050000/tinkerkit-braccio-arduino-robotic/dp/2831002?ost=braccio ), for movement is uses 6 pretty powerful hobby servo motors which can be easily controlled using PWM. Since the arm is using standard servos we don't have any feedback about the position of the motors. Why would we want this? There are 2 main reasons, to check during operation if the motors are actually at the desired position, and we can also teach the robot what to do by showing it, which is what I plan on doing in the end. To accomplish this we need to track the position of all of the motors, record it, save it, and play it back later.
2. Idea
There is a couple of different ways we can get the angle information for all of the joints, we can use:
- Encoders
- Internal potentiometers
- Potentiometers
1. Encoder
This is the way it is done on real industrial robots, they would have attached encoders to the joints or motors. Since the motors are geared down to move the joints usually, we can get a better resolution by attaching the encoder to the motor directly. Encoders work by having a fixed amount of, let's call them steps/clicks. The more steps the encoder has the greater the resolution. A great thing about the encoders is that they work the same no matter the circumstances and really reliable, we are working with a digital signal.
2. Internal Potentiometer
Servo motors work by having an internal potentiometer which it uses as a feedback to control the position of the motor. There are servo motors which have an additional wire pulled out of them which is connected to the potentiometer, so we can get the data out of the motor. This is a really clean way to the thing I'm trying to do, but this would mean trying to hack all of the servos. which I wasn't keen on doing, since there's always a chance of ruining something, and the motors are pretty good so I didn't want to ruin them.
3. Potentiometers
The option I went is this one, it's not as robust as the first option, and requires much more design work to do than the second option, but when we put it all together, it can be made to work pretty good. To make the sensors, I plan on using the simplest thing, the ordinary one turn potentiometer. Since the servos can only do 180 degrees or so, the one turn potentiometers are sufficient for most of the joints. The bad thing about potentiometers is their repeatability, linearity and temperature dependence.
- Repeatability
- This means that we might not get the exact same value when returning to a certain point
- Linearity
- If, let's say, the potentiometer can turn 300 degrees, we would expect that at 30 degrees we have 1/10 of the resistance compared to 300 degrees. When we buy a potentiometer there are different kinds like linear, logarithmic, but there is always a small error
- Temperature dependence
- The resistance can change depending on the ambient temperature and ruin having consistent measurements, this isn't a huge difference but it is still present.
Potentiometers might not sound like the best option after looking at these things, but the truth is, the servos aren't that accurate either and the whole construction of course has backlash and stuff like that, so, considering that we can measure the voltage at around 1000 points of the potentiometers really easily with analog pins on the Arduino, this will be a more than a sufficient option.
3. Plan
The whole project is around using potentiometers as sensors, so one of the main things we have to do is design mounts for potentiometers for all of the joints. I will be 3D printing all of the mounts, but there will be a lot of work around designing and fitting everything together, specially for the first and last joint where I had to use multi turn potentiometers as I will show later in this blog. We need electronics to gather all of the data and send the controls to the motors, we need a pretty good power supply and we need some kind of a interface to control the robot. I wanted to go with the design where I use 2 Arduinos, a Nano near the robot to read all of the potentiometer values and send all of the commands to the motors, as well as an Arduino Mega2560 which will be used to drive a touchscreen shield as a HMI. Looking at this, we can split the design and build into 4 different categories
- Mounting for the potentiometers
- Electronics for the robot
- Power supply
- Human Machine Interface
4. Design & Build
I've already categorizes all of the things that need to be done, and here, I'll start with the simplest one, but one that is really needed for the project, a power supply unit.
1. PSU
The robot comes with a pretty decent 5V 5A power supply, but there's some things that I would like to change/add:
- I want an on/off switch
- Fuse
- LED indicator
- Multiple Connections
- Safety Switch
- Special connectors
To begin this build I needed an enclosure for everything, so I used a standard electronics box which is 150x120x80mm. They are sturdy, cheap and can be easily found at electronics shops. As for the PSU itself, I went with a Meanwell 5V 7A power supply, each of the 6 motors can come close to 1A, so the power supply is sufficient, and the Meanwell PSU on my 3D printer works flawlessly for many months now. Another thing on the list that seems different is the special connectors. Around 2 months ago since writing this blog, I designed my own connector based on the 3.5mm jack (3D Printed Locking Connector - Type 1 ), so I redesigned them a bit into their second, waterproof version which I will be using for this project. I will write a separate blog that will go around this connector. One thing about the connector is that it works with the barrel jack connectors as well.
PSU box
This part of the build was pretty straightforward, but it was pretty tight to pack everything in, but in the end, it works without any problems!
The black plastic part at the back consists of a standard 240V connector, an integrated fuse slot, and a small switch, which ticks a lot of the boxer from my list, besides that I added a safety switch on top as well as a power LED to know when the PSU is on, which proved to be important, because if there is a short, the PSU will cut out on it's own and kill the LED. As I've already said, I'll go into detail in a separate blog about the connectors, there will be a picture or two of them here through the blog.
Cables
There are 2 types of cables I'll need with 3 cables in total, 2 power cables and one data cable. For the power cables I'll use cables with 2 2mm wires, and I'll use a smaller 3 wire cable for the data cable. The communication between the Arduinos will be serial, so generally, 2 wires would be enough for Rx and Tx since they are both on the same power supply, but as I found out during testing, a common ground is needed if they are not on the same power supply, so that's why I'm running 3 wires between them.
On the above pictures are the things needed for the connectors, they consist of 4 3D printed parts each, the main body which houses the connector itself as well as the cable, the mushroom which is like an adapter for the cable, a cable nut which holds down the mushroom and the locking nut which locks the connector, to make it more water resistant, I added a couple of O rings. Just to mention now, the connector design works with the barrel jack connector, but the O ring part is designed for the 3.5mm connector. Here are the finished cables:
2. Mounting for the Potentiometers
This will require 5 different mounts, I've labeled the potentiometers from down going up, the first and fifth potentiometer are a bit tricky and needed geared mechanisms, as I will show through the build, the other 3 were pretty straight forward. One thing I should note is that this is an invasive method for mounting the potentiometers, since I drilled into the plastic of the robot, a lot... All of the parts will be attached to each other, or to the robot using machine screws and nuts. Let's get started with the best looking mount for the potentiometer one.
Before I get into each potentiometer I want to cover that I call a universal mount:
These mounts are designed to snuggly fit on the middle of the sections of the braccio, they are attached with 4 screws, and every one of them has embedded nuts for both attaching to the robot itself and for attaching other parts to it. They will be used for a lot of sensors for mounting the connector arms to the robot.
1. Potentiometer
For this potentiometer, my idea was to mount the potentiometer to the body of the robot and to mount a giant gear to the base of the robots and in that way, make that gear the new strong base of the robot. For modelling the gears I used the integrated spur gear generator in Fusion360, here is how that turned out:
Big Gear
| {gallery} Big Gear |
|---|
|
|
|
|
|
|
|
|
|
|
|
|
The gear turned out great, to make sure that it would work as a gear good, I printed it out on the raft, it turned out great on the first try, which is great, since it took almost 7 hours to print. To mount the big gear to the base of the robot I used machine screws and small nuts inserted into the spur gear, besides those nuts, the gear also has 8 M5 nuts which are to be used to mount the whole robot to whatever surface from underneath. Here is how the gear mounted to the base looks like:
Besides the big gear, we need 2 more parts, a potentiometer holder and a small gear which will go on the shaft of the potentiometer. Unfortunately, I somehow managed to lose the core 3D models for these 2, but I managed to find their STLs:
Potentiometer Mount
Small Gear
Here is how the finished mount looks like on the robot with the potentiometer attached as well as wires soldered to it, one thing I should have looked at, which gave me a little delay is, that for this type of potentiometer, the center pin isn't the variable one, that's just a simple thing to keep in mind.
2. Potentiometer
This was much more straight forward than the first potentiometer, the pot was on one side mounted to the body of the robot, and on the other side there was a connecting arm connected to the knob of the potentiometer. Here is my design for this potentiometer:
| {gallery} Second Potentiometer Mount |
|---|
|
|
|
|
|
|
|
|
|
|
|
|
Here is the completed mount on the robot:
3. Potentiometer
I mounted this potentiometer in the same way I mounted the last one pretty much, it needed 3 parts in total not counting the universal mount. One thing to mention is that it's critical to accurately position the pot holders so that the shaft of the potentiometer is in line with the axis of the motor, so that we only have rotation on the potentiometer and no other forces
| {gallery} Third Potentiomter |
|---|
|
|
|
|
|
|
4. Potentiometer
| {gallery} Fourth Potentiometer Mount |
|---|
|
|
|
|
|
|
5. Potentiometer
For this potentiometer, it was a similar situation as for the first potentiometer, so I made another gear which I mounted to the top with a multi turn potentiometer with a smaller gear mounted to the side. We could go to a gear ratio of 20:1 since the gear at the top can turn 180 degrees, while the multi turn potentiometer can do 10 turns.
| {gallery} My Gallery Title |
|---|
|
|
|
|
|
|
And with that, we have all of the mounts designed and printed, here are some pictures of how the whole robot looks like with all of the potentiometers mounted. I took another electronics box, this one 150x150mm, and mounted the big gear to the lid with 8 M5 screws. This box will act as an easy way to connect everything to the robot and to enclose some of the electronics needed for this project:
3. Electronics for the robot
In this part of the blog I'll cover the electronics which are housed underneath the robot as well as the box I already mentioned. Here is my requirements list that I wanted to fulfill for this part of the project, I needed a:
- Big enough box to fit the gear on top
- Safety switch
- Fuse
- Indicator LED-s
- Connector Panel
- Connectors for the potentiometers
- Connectors for the motors
- Connector for power
- Connector for communication with the other Arduino
- Electronics inside which would gather sensor data and send data to motors and the other Arduino via serial communication
Electrical
Now we come to the electronics underneath the robot, the main for this will be a small Arduino Nano, I've chosen it because it's extremely easy to work with, has 6 PWM pins, has over 6 analog pins, and is really small. My goal was to design a small board that will act like a shield for the Nano board. Let's first look at the requirements for this board:
Arduino Nano Board
- Connections for the 6 servo motors
- Connections for the 6 potentiometers
- Integrated relay
- Integrated buzzer
- Connection for LED indicators
- Connection for serial communication
- Power connection
Relay circuit
The board is pretty straight forward for the most part, the only thing I looked a bit more into is the relay activating circuit. I got myself a small 5V relay which can handle 10A which is plenty, but the problem is that when turning off the relay we can easily damage or kill the Arduino if we don't have a proper circuit for activating it, that's why additional components can be seen on the small relay modules, here is the circuit I went with:
Buzzer Circuit
To activate the buzzer I just went with a small transistor next to it:
Here is a short video where I test out both of the circuits:
Soldering
The rest of the board is just simple connections from pins to the connectors, as connectors on the board I went with the small screw terminals, one thing I tried out which honestly worked out great in my opinion is making tiny labels for these connectors. To make them, I used a piece of paper and some double sided sticky tape, and just put small marks.
Here is how the boards turned out after a couple of hours of soldering:
This side of the board is dedicated for the connectors for the motors and potentiometers, you can see a few additional components on the board that I haven't mentioned, I added a small fuse for the Arduino as well as a couple of capacitors since I have some bad experience when working with servos, they can really drain a lot of current fast, specially when there are 6 of them, but these caps proved to work great.
This side of the board has connectors for the 2 LED-s, a danger LED and safe for work LED, which indicate if the robot is currently running it's motors or if it's in safe mode when you can move it around by hand. Besides that we have the power connector on the right as well as connection for the serial communication on the left. One thing I switched to is using boards with lines when making these kinds of boards. They make my life so much easier, you just have to be careful when breaking one of the lines to not short it out with the line next to it. I use a small hand drill for breaking the lines.
The board worked without any problems from the beginning, one thing I should have added was a small jumper between the 5v pin of the Arduino and the fuse, because I need to take out the Arduino every time I want to program it if everything is connected to the board. Besides that I added a few more wires from the power supply to the common pin of the relay and had to switch the A2 pin for the second potentiometer to the A7 pin since I was experiencing some bugs. This solved the problem.
Mechanical
As already mentioned, I will be using a 150x150mm box for this, I designed and 3D printed a connector panel, which has 12 connections for the 6 servos and 6 potentiometers and space for 2 of my locking connectors, where one will be used for data and the other one for power, here is how the connector panel looks like:
For the small connectors themselves I used the small nylon connectors, since they are already used on the servos of the robot so I went with them as well, they are really easy to crimp and I didn't have too many problems with them, just a pin or two coming out because I didn't crimp them properly. I soldered them to a small board and attached it to the panel with a bit of super glue and soldered some wires to the back.
As I did for the small screw terminals, I made small labels for the wires as well, this proved to be a great move since there are 14 wires in total here, I used crimp ferrules for all of the wires so that they can be securely attached to the screw terminals.
On the other side of the box I added the safety switch, fuse and the indicator LED-s, I 3D printed some LED holders as well as the panel for the safety switch and fuse since I don't have a step drill bit.
All that's left not is to do all of the wiring inside. This is where I had a problem which baffled me the most. After completing the board I double checked all of the connections that there aren't any shorts and stuff like that, but when I connected the power cable the power supply would just turn off. After troubleshooting a bit, checking the power supply, the cables, the faulty thing in the end was the barrel jack which I put into the panel. It had an internal short when you pushed the connector in. I was out of these connectors to replace it, so unfortunately, at least for now, I had to add a standard barrel jack connector on the other side.
This was the spaghetti at the beginning, but after some wire cutting, crimping and a few zip ties, I ended with something that in my opinion looks much nicer than the picture above, here is the finished result:
With that, the electronics that go underneath the robot are complete, how I programmed the Arduino, I'll show after I go over the build of the interface for controlling the robot, which I will show now.
4. Human Machine Interface
Sticking with the theme of my build, I used the same type of box I used for the PSU here. My main idea was to use an Adafruit touchscreen shield for the Arduino that I had, but when trying to put it into a mount I designed I managed to brake the top layer killing the touch screen functionality.... Fortunately, the screen underneath worked without any problems, so I continued with using it as it is, I just added a couple of buttons at the bottom. The box had 2 connectors on the side, one for data, and the other one for power.
On the picture above, it can't be seen that much, but the shattered top layer can clearly be seen on the picture underneath. I designed a 3 button holder and put a small potentiometer for controlling speed as well as a power on switch to the left.
And here is how the 2 boxes, the PSU and the HMI box look one next to the other:
With that, the mechanical build is complete, beside this, the only thing I did was screwing down the robot to a big and heavy piece of wood, since it loves moving a lot, I then clamped down the robot during testing. With the hardware out of the way, it's time to tackle the software for this robot and then do some testing. But before that, here is how everything assembled looks like:
5. Software
The software part of this project will of course be split into 2 sections, one regarding the Arduino Mega and the other one for the Arduino Nano. Unfortunately, this is the part of the project that I didn't manage to complete to the end, but the sensor portion of this project is fully functional. What is left is to program procedures for recording and storing the data from the robot, though I've already scratched the surface of that as you will see through the code. First off, I'll start with the Arduino Mega.
Code for the Mega
I split the code for the Arduino Mega into 5 different tabs, the first one being the main tab, with all of the variables, setup and loop, one being the buttons tab, SerialData tab and last the TextFields tab. Each tab of course covered one of the areas as the name implies. One thing to note again, the code is still work in progress, this was enough for me to play around with the robot a bit by manually writing down all of the coordinates, but I got the coordinates themselves by using this system. Let's begin with the main tab:
Main Tab
I made this code starting from a few examples, for the graphics for the display using the ILI9341 library as well as the library for the touch screen, and just went from there, this part of the code takes care of all of the variables and calls the needed functions which are located in the other tabs. The background for the display was a picture to make things simpler which I designed, and then just populated the needed text fields, here is the blank template and the code:
#include <SPI.h>
#include <SD.h>
#include <Adafruit_GFX.h> // Core graphics library
#include <Adafruit_ILI9341.h> // Hardware-specific library
#include <Adafruit_ImageReader.h> // Image-reading functions
#include <Adafruit_STMPE610.h>
#include "SdFat.h"
// This is calibration data for the raw touch data to the screen coordinates
#define TS_MINX 150
#define TS_MINY 130
#define TS_MAXX 3800
#define TS_MAXY 4000
#define SD_CS 4 // SD card select pin
#define TFT_CS 10 // TFT select pin
#define TFT_DC 9 // TFT display/command pin
#define STMPE_CS 8
Adafruit_STMPE610 ts = Adafruit_STMPE610(STMPE_CS);
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
File myFile;
void DrawRecordButton(int color);
void DrawPlayButton(int color);
void DrawPauseButton(int color);
void DrawStopButton(int color);
bool recordPressed = false;
bool playPressed = false;
bool pausePressed = true;
bool stopPressed = true;
int speedData = 100;
double speedMultiplier = 1;
Adafruit_ImageReader reader; // Class w/image-reading functions
Adafruit_Image img; // An image loaded into RAM
int32_t width = 0, // BMP image dimensions
height = 0;
String inputString = ""; // a String to hold incoming data
boolean stringComplete = false; // whether the string is complete
int m1, m2, m3, m4, m5, m6;
void setup() {
Serial.begin(9600);
ImageReturnCode stat; // Status from image-reading functions
if(!SD.begin(SD_CS)) {
for(;;); // Loop here forever
}
ts.begin();
tft.begin();
tft.setRotation(3);
stat = reader.drawBMP("/Test.bmp", tft, 0, 0);
DrawRecordButton(1);
DrawPlayButton(1);
DrawPauseButton(0);
DrawStopButton(0);
String x;
/*
Serial.println("Trying to open the file");
myFile = SD.open("TestFile.txt");
if(myFile){
Serial.println("Uspesno otvoren file");
while(myFile.available()){
x = myFile.readStringUntil('\r');
getMotorValues(x);
Serial.println(x);
UpdateTextFields(m1, m2, m3, m4, m5, m6, speedData);
x = myFile.read();
x = "";
delay(20);
}
}
Serial.println("Done with the file");
*/
}
String readLine(File file){
String s,s1;
/*while(s = file.read() != "\n"){
s1 += s;
}*/
/*for(int i = 0; i < 18; i++){
s1 += file.read();
}*/
s1 = file.readStringUntil('\r');
return s1;
}
void loop() {
// put your main code here, to run repeatedly:
UpdateData();
SpeedChangeListener();
delay(100);
}
Buttons Tab
This tab was dedicated to the buttons which were on the touchscreen, they worked great, until I broke the touchscreen... I'll look into getting another one soon, and I like how they look, so for now, I'm leaving this part of the code in. The buttons were just simple geometric shape, so I made them using built in functions for drawing rectangles and circles to make it work faster rather than loading small images. Here is the code and how it worked in the end.
//These are the functions that draw all of the buttons, as the input argument they take one int which determines the color
//If the int is 0 the color is gray, and if it's 1 they are drawn with their dedicated color
void DrawRecordButton(int color){
if(color == 0){
tft.fillCircle(197, 101, 20, 0xA510);
tft.drawCircle(197, 101, 20, ILI9341_BLACK);
}
else if(color == 1){
tft.fillCircle(197, 101, 20, ILI9341_RED);
tft.drawCircle(197, 101, 20, ILI9341_BLACK);
}
}
void DrawPlayButton(int color){
if(color == 0){
tft.fillTriangle(258, 80, 258, 120, 298, 100, 0xA510);
tft.drawTriangle(258, 80, 258, 120, 298, 100, ILI9341_BLACK);
}
else if(color == 1){
tft.fillTriangle(258, 80, 258, 120, 298, 100, ILI9341_GREEN);
tft.drawTriangle(258, 80, 258, 120, 298, 100, ILI9341_BLACK);
}
}
void DrawPauseButton(int color){
if(color == 0){
tft.fillRect(182,154,10,36,0xA510);
tft.fillRect(202,154,10,36,0xA510);
tft.drawRect(182,154,10,36,ILI9341_BLACK);
tft.drawRect(202,154,10,36,ILI9341_BLACK);
}
else if(color == 1){
tft.fillRect(182,154,10,36,0x5D1C);
tft.fillRect(202,154,10,36,0x5D1C);
tft.drawRect(182,154,10,36,ILI9341_BLACK);
tft.drawRect(202,154,10,36,ILI9341_BLACK);
}
}
void DrawStopButton(int color){
if(color == 0){
tft.fillRoundRect(258, 154, 36, 36, 5, 0xA510);
tft.drawRoundRect(258, 154, 36, 36, 5, ILI9341_BLACK);
}
else if(color == 1){
tft.fillRoundRect(258, 154, 36, 36, 5, ILI9341_RED);
tft.drawRoundRect(258, 154, 36, 36, 5, ILI9341_BLACK);
}
}
void ButtonListener(int recordEN, int playEN, int pauseEN, int stopEN){
if(!ts.bufferEmpty()){
TS_Point p = ts.getPoint();
// Scale using the calibration #'s
// and rotate coordinate system
p.x = map(p.x, TS_MINY, TS_MAXY, 0, tft.height());
p.y = map(p.y, TS_MINX, TS_MAXX, 0, tft.width());
int y = tft.height() - p.x;
int x = p.y;
//Record Button Listener - Works if recordEN is 1
if(recordEN == 1){
int dist = (x - 197) * (x - 197) + (y - 101) * (y - 101);
if(dist < 400){
if(recordPressed == false){
DrawRecordButton(0);
recordPressed = true;
delay(250);
}
else{
DrawRecordButton(1);
recordPressed = false;
delay(250);
}
}
}
//Play Button Listener - Works if playEN is
if(playEN == 1){
if(x >= 258 && x <= 298 && y >= 80 && y <= 120){
if(playPressed == false){
DrawPlayButton(0);
playPressed = true;
delay(250);
}
else{
DrawPlayButton(1);
playPressed = false;
delay(250);
}
}
}
//Pause Button Listener - Works if pauseEN is 1
if(pauseEN == 1){
if(x >= 182 && x <= 212 && y >= 154 && y <= 190){
if(pausePressed == false){
DrawPauseButton(0);
pausePressed = true;
delay(250);
}
else{
DrawPauseButton(1);
pausePressed = false;
delay(250);
}
}
}
//Stop Button Listener - Works if stopEN is 1
if(stopEN == 1){
if(x >= 258 && x <= 294 && y >= 154 && y <= 190){
if(stopPressed == false){
DrawStopButton(0);
stopPressed = true;
delay(250);
}
else{
DrawStopButton(1);
stopPressed = false;
delay(250);
}
}
}
}
}
SerialData Tab
This tab tackled the serial data input and playing with those values. It's a pretty simple tab where I used serial event to detect when there is any new data on the serial port and it extracted the motor values from that string, I used a format '111222333444555666' where every three digits were dedicated to one motor/sensor. And during testing I had to put a string length check since there were some bugs there, but this solved all of the issues.
/*
SerialEvent occurs whenever a new data comes in the hardware serial RX. This
routine is run between each time loop() runs, so using delay inside loop can
delay response. Multiple bytes of data may be available.
*/
void serialEvent() {
while (Serial.available()) {
// get the new byte:
char inChar = (char)Serial.read();
// add it to the inputString:
inputString += inChar;
// if the incoming character is a newline, set a flag so the main loop can
// do something about it:
if (inChar == '\n') {
stringComplete = true;
}
}
}
void getMotorValues(String s){
m1 = s.substring(0,3).toInt();
m2 = s.substring(3,6).toInt();
m3 = s.substring(6,9).toInt();
m4 = s.substring(9,12).toInt();
m5 = s.substring(12,15).toInt();
m6 = s.substring(15,18).toInt();
}
void UpdateData(){
if(stringComplete){
if(inputString.length() == 21){
Serial.println("Good string");
getMotorValues(inputString);
UpdateTextFields(m1, m2, m3, m4, m5, m6, speedData);
}
Serial.println(inputString);
Serial.println(inputString.length());
inputString = "";
stringComplete = false;
}
}
TextFields Tab
As the name suggests, this tab was dedicated to the text fields that can be seen through the video. This required a bit of playing around to position all of the text boxes properly, and to find a way to update them in a good way, the problem is, if we write let's say a value of 100, and want to update it to a value of 150, we can't just write 150 over 100, the 100 underneath won't be erased, so as the easiest solution, I would just draw a white rectangle underneath the text box, every time I wanted to update it.
void UpdateTextFields(int mv1, int mv2, int mv3, int mv4, int mv5, int mv6, int spdv){
//Motor1
tft.fillRect(90,35,65,30,ILI9341_WHITE);
tft.setCursor(95,40);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
tft.print(mv1);
//Motor2
tft.fillRect(90,70,65,30,ILI9341_WHITE);
tft.setCursor(95,75);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
tft.print(mv2);
//Motor3
tft.fillRect(90,105,65,30,ILI9341_WHITE);
tft.setCursor(95,110);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
tft.print(mv3);
//Motor4
tft.fillRect(90,140,65,30,ILI9341_WHITE);
tft.setCursor(95,145);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
tft.print(mv4);
//Motor5
tft.fillRect(90,175,65,30,ILI9341_WHITE);
tft.setCursor(95,180);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
tft.print(mv5);
//Motor6
tft.fillRect(90,210,65,26,ILI9341_WHITE);
tft.setCursor(95,213);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
tft.print(mv6);
//Speed
tft.setCursor(242,213);
tft.fillRect(240,210,75,26,ILI9341_WHITE);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
String spdtxt = String(spdv) + "%";
tft.print(spdtxt);
}
void UpdateSpeedTxtField(int data){
tft.setCursor(242,213);
tft.fillRect(240,210,75,26,ILI9341_WHITE);
tft.setTextColor(ILI9341_BLACK);
tft.setTextSize(3);
String spdtxt = String(data) + "%";
tft.print(spdtxt);
}
void SpeedChangeListener(){
speedData = map(analogRead(A8), 0, 1000, 1, 100);
speedData = speedData * 5;
speedMultiplier = double(speedData) / 100;
speedMultiplier = 1 / speedMultiplier;
UpdateSpeedTxtField(speedData);
}
In the video below is a small test I did, where I wrote a sequence of many numbers in a txt file and put it on a SD card and just let it write through the fields as if were data that was recorded from the sensors. This is something I plan on doing in the near future with this build.
That would wrap the code for the Arduino Mega for now. This is something that requires more work, not too much, but I didn't manage to finish it in time for this Project14, though the part that was important regarding the sensor data itself is working great. With that done, it's time to go over to the programming of the Arduino Nano.
Code for the Nano
Code for the Nano is just a one tab code with not a lot of going on in it. I did some tests where I would teach the arm to do certain movements, so I needed to get the data from the Nano to the Mega which wasn't a big issue, but, one thing is just getting raw data, I needed the angles of the motors. Since I'm using potentiometers and something is a bit different every time I turn on the robot I decided on going with a short calibration sequence at the start, that means that, the robot will go through the range of movements for every joint, get those values and save them, and then map those values to angles going from 0 to 180 degrees, since this is how I will be controlling the robot. In the code you can also see a couple of long sequences, these are the sequences that I manually wrote down using the HMI screen, pretty much like a stop motion puppet, to program the robot.
#include "Servo.h"
#define buzzerPin 2
#define DNGLEDPin 7
#define SFWLEDPin 8
Servo m1, m2, m3, m4, m5, m6;
int min1, max1, min2, max2, min3, max3, min4, max4, min5, max5;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(12, OUTPUT);
digitalWrite(12,HIGH);
pinMode(SFWLEDPin, OUTPUT);
pinMode(DNGLEDPin, OUTPUT);
PowerUpSound();
digitalWrite(12, LOW);
delay(10000);
DangerSound();
min1 = 0;
max1 = 0;
min2 = 0;
max2 = 0;
min3 = 0;
max3 = 0;
min4 = 0;
max4 = 0;
min5 = 0;
max5 = 0;
m1.attach(3);
m2.attach(5);
m3.attach(6);
m4.attach(9);
m5.attach(10);
m6.attach(11);
delay(10000);
m1.write(90);
m2.write(90);
m3.write(90);
m4.write(90);
m5.write(90);
digitalWrite(DNGLEDPin, HIGH);
delay(3000);
//Calibration();
//Sequence1();
//Sequence2();
//Sequence3();
//Sequence4();
Sequence5();
m1.write(90);
m2.write(90);
m3.write(90);
m4.write(90);
m5.write(90);
digitalWrite(12, HIGH);
SafeSound();
}
void Calibration(){
m1.write(90);
m2.write(90);
m3.write(90);
m4.write(90);
m5.write(90);
//Calibrating Motor1
m1.write(0);
delay(2500);
min1 += analogRead(A6);
delay(500);
min1 += analogRead(A6);
delay(500);
min1 += analogRead(A6);
delay(500);
min1 = min1 / 3;
BlinkGreen();
m1.write(180);
delay(2500);
max1 += analogRead(A6);
delay(500);
max1 += analogRead(A6);
delay(500);
max1 += analogRead(A6);
delay(500);
max1 = max1 / 3;
BlinkRed();
m1.write(90);
m2.write(90);
m3.write(180);
m4.write(180);
m5.write(90);
delay(3000);
//Calibrating Motor2
m2.write(45);
delay(5000);
min2 += analogRead(A7);
delay(500);
min2 += analogRead(A7);
delay(500);
min2 += analogRead(A7);
delay(500);
min2 = min2 / 3;
BlinkGreen();
m2.write(135);
delay(5000);
max2 += analogRead(A7);
delay(500);
max2 += analogRead(A7);
delay(500);
max2 += analogRead(A7);
delay(500);
max2 = max2 / 3;
BlinkRed();
m1.write(90);
m2.write(90);
m3.write(90);
m4.write(180);
m5.write(90);
delay(3000);
//Calibrating Motor3
m3.write(45);
delay(5000);
min3 += analogRead(A3);
delay(500);
min3 += analogRead(A3);
delay(500);
min3 += analogRead(A3);
delay(500);
min3 = min3 / 3;
BlinkGreen();
m3.write(135);
delay(5000);
max3 += analogRead(A3);
delay(500);
max3 += analogRead(A3);
delay(500);
max3 += analogRead(A3);
delay(500);
max3 = max3 / 3;
BlinkRed();
m1.write(90);
m2.write(90);
m3.write(90);
m4.write(90);
m5.write(90);
delay(3000);
//Calibrating Motor4
m4.write(0);
delay(5000);
min4 += analogRead(A4);
delay(500);
min4 += analogRead(A4);
delay(500);
min4 += analogRead(A4);
delay(500);
min4 = min4 / 3;
BlinkGreen();
m4.write(180);
delay(5000);
max4 += analogRead(A4);
delay(500);
max4 += analogRead(A4);
delay(500);
max4 += analogRead(A4);
delay(500);
max4 = max4 / 3;
BlinkRed();
m1.write(90);
m2.write(90);
m3.write(90);
m4.write(90);
m5.write(90);
delay(3000);
//Calibrating Motor5
m5.write(0);
delay(5000);
min5 += analogRead(A5);
delay(500);
min5 += analogRead(A5);
delay(500);
min5 += analogRead(A5);
delay(500);
min5 = min5 / 3;
BlinkGreen();
m5.write(180);
delay(5000);
max5 += analogRead(A5);
delay(500);
max5 += analogRead(A5);
delay(500);
max5 += analogRead(A5);
delay(500);
max5 = max5 / 3;
BlinkRed();
m1.write(90);
m2.write(90);
m3.write(90);
m4.write(90);
m5.write(90);
delay(3000);
BlinkGreen();
BlinkRed();
}
void MoveMotors(int v1, int v2, int v3, int v4, int v5, int v6){
m1.write(v1);
m2.write(v2);
m3.write(v3);
m4.write(v4);
m5.write(v5);
m6.write(v6);
}
void Sequence1(){
int t = 1500;
MoveMotors(90, 90, 90, 90, 90, 0);//1
delay(t);
MoveMotors(43, 90, 90, 90, 90, 0);//2
delay(t);
MoveMotors(43, 77, 134, 36, 73, 0);//3
delay(t);
MoveMotors(43, 93, 141, 22, 73, 0);//4
delay(t);
MoveMotors(43, 100, 147, 21, 74, 0);//5
delay(t);
MoveMotors(43, 110, 136, 15, 86, 180);//6
delay(t);
MoveMotors(43, 90, 90, 90, 90, 180);//7
delay(t);
MoveMotors(84, 90, 90, 20, 90, 180);//8
delay(t);
MoveMotors(84, 82, 129, 9, 90, 180);//9
delay(t);
MoveMotors(78, 100, 143, 9, 90, 180);//10
delay(t);
MoveMotors(81, 120, 142, 20, 89, 180);//11
delay(t);
MoveMotors(81, 120, 142, 9, 90, 0);//12
delay(t);
MoveMotors(90, 90, 90, 90, 90, 0);//13
delay(t);
}
void Sequence2(){
int t = 1000;
MoveMotors(90, 90, 90, 90, 90, 90);//1
delay(t);
MoveMotors(90, 85, 100, 80, 90, 90);//2
delay(t);
MoveMotors(90, 85, 115, 70, 90, 90);//3
delay(t);
MoveMotors(90, 100, 125, 60, 90, 90);//4
delay(t);
MoveMotors(90, 110, 125, 60, 90, 90);//5
delay(t);
MoveMotors(90, 115, 125, 60, 90, 90);//6
delay(t);
MoveMotors(90, 125, 125, 60, 90, 90);//7
delay(t);
MoveMotors(90, 135, 125, 70, 90, 90);//8
delay(t);
MoveMotors(90, 140, 125, 80, 100, 90);//9
delay(t);
MoveMotors(65, 140, 125, 80, 100, 90);//10
delay(t);
MoveMotors(67, 135, 135, 81, 101, 90);//11
delay(t);
MoveMotors(66, 128, 144, 81, 101, 90);//12
delay(t);
MoveMotors(65, 121, 151, 73, 101, 90);//13
delay(t);
MoveMotors(77, 122, 152, 72, 63, 90);//14
delay(t);
MoveMotors(87, 122, 152, 72, 63, 90);//15
delay(t);
MoveMotors(97, 122, 152, 72, 63, 90);//16
delay(t);
MoveMotors(107, 122, 152, 72, 63, 90);//17
delay(t);
MoveMotors(105, 122, 152, 73, 99, 90);//18
delay(t);
MoveMotors(105, 130, 140, 73, 99, 92);//19
delay(t);
MoveMotors(105, 130, 140, 81, 98, 90);//20
delay(t);
MoveMotors(105, 125, 140, 81, 98, 90);//21
delay(t);
MoveMotors(90, 125, 140, 81, 98, 90);//22
delay(t);
MoveMotors(90, 106, 175, 111, 98, 90);//23
delay(t);
MoveMotors(90, 90, 180, 130, 98, 90);//24
delay(t);
MoveMotors(90, 83, 180, 180, 90, 90);//25
delay(t);
MoveMotors(90, 35, 180, 180, 90, 90);//26
delay(t);
}
void Sequence3(){
int t = 500;
MoveMotors(90, 90, 90, 90, 90, 90);//1
delay(t);
MoveMotors(90, 100, 100, 75, 90, 90);//2
delay(t);
MoveMotors(90, 110, 110, 75, 90, 90);//3
delay(t);
MoveMotors(90, 125, 115, 75, 90, 90);//4
delay(t);
MoveMotors(90, 120, 115, 90, 90, 90);//5
delay(t);
MoveMotors(70, 120, 120, 90, 120, 90);//6
delay(t);
MoveMotors(70, 150, 120, 90, 120, 90);//7
delay(t);
MoveMotors(60, 150, 120, 90, 120, 90);//8
delay(t);
MoveMotors(59, 142, 130, 93, 117, 90);//9
delay(t);
MoveMotors(58, 137, 139, 93, 117, 90);//10
delay(t);
MoveMotors(70, 137, 139, 93, 118, 90);//11
delay(t);
MoveMotors(70, 130, 140, 91, 118, 90);//12
delay(t);
MoveMotors(70, 117, 160, 91, 118, 90);//13
delay(t);
MoveMotors(70, 110, 170, 91, 113, 90);//14
delay(t);
MoveMotors(70, 115, 170, 92, 113, 90);//15
delay(t);
MoveMotors(52, 115, 170, 92, 115, 90);//16
delay(t);
MoveMotors(52, 117, 170, 115, 115, 90);//17
delay(t);
MoveMotors(53, 120, 170, 109, 115, 90);//18
delay(t);
MoveMotors(54, 125, 163, 110, 115, 90);//19
delay(t);
MoveMotors(55, 129, 157, 110, 115, 90);//20
delay(t);
MoveMotors(61, 123, 158, 110, 90, 90);//21
delay(t);
MoveMotors(81, 123, 158, 110, 90, 90);//22
delay(t);
MoveMotors(81, 133, 142, 110, 90, 90);//23
delay(t);
MoveMotors(81, 147, 124, 110, 90, 90);//24
delay(t);
MoveMotors(81, 150, 124, 110, 90, 90);//25
delay(t);
MoveMotors(81, 144, 132, 110, 90, 90);//26
delay(t);
MoveMotors(81, 136, 144, 110, 90, 90);//27
delay(t);
MoveMotors(81, 128, 156, 110, 90, 90);//28
delay(t);
MoveMotors(81, 121, 167, 110, 90, 90);//29
delay(t);
MoveMotors(81, 116, 173, 109, 90, 90);//30
delay(t);
MoveMotors(81, 112, 176, 104, 90, 90);//31
delay(t);
MoveMotors(81, 105, 176, 103, 90, 90);//32
delay(t);
MoveMotors(81, 94, 175, 103, 90, 90);//33
delay(t);
MoveMotors(85, 84, 167, 77, 90, 90);//34
delay(t);
MoveMotors(90, 84, 167, 77, 90, 90);//35
delay(t);
MoveMotors(93, 103, 150, 77, 90, 90);//36
delay(t);
MoveMotors(94, 114, 138, 77, 90, 90);//37
delay(t);
MoveMotors(94, 123, 136, 92, 90, 90);//38
delay(t);
MoveMotors(94, 137, 128, 103, 90, 90);//39
delay(t);
MoveMotors(95, 148, 117, 102, 90, 90);//40
delay(t);
MoveMotors(94, 155, 117, 111, 90, 90);//41
delay(t);
MoveMotors(94, 148, 129, 111, 90, 90);//42
delay(t);
MoveMotors(95, 136, 145, 111, 90, 90);//43
delay(t);
MoveMotors(96, 128, 165, 125, 90, 90);//44
delay(t);
MoveMotors(97, 121, 174, 125, 90, 90);//45
delay(t);
MoveMotors(98, 115, 180, 125, 90, 90);//46
delay(t);
MoveMotors(98, 93, 180, 97, 90, 90);//47
delay(t);
MoveMotors(98, 107, 160, 98, 105, 90);//48
delay(t);
MoveMotors(94, 116, 150, 98, 105, 90);//49
delay(t);
MoveMotors(95, 125, 138, 98, 105, 90);//50
delay(t);
MoveMotors(94, 134, 133, 101, 105, 90);//51
delay(t);
MoveMotors(94, 141, 132, 109, 105, 90);//52
delay(t);
MoveMotors(94, 146, 133, 117, 105, 90);//53
delay(t);
MoveMotors(89, 146, 133, 117, 105, 90);//54
delay(t);
MoveMotors(84, 146, 133, 117, 105, 90);//55
delay(t);
MoveMotors(87, 150, 124, 118, 107, 90);//56
delay(t);
MoveMotors(88, 160, 118, 118, 105, 90);//57
delay(t);
MoveMotors(91, 160, 115, 118, 112, 90);//58
delay(t);
MoveMotors(91, 111, 180, 140, 110, 90);//59
delay(t);
MoveMotors(82, 110, 180, 140, 90, 90);//60
delay(t);
MoveMotors(82, 93, 180, 140, 90, 90);//61
delay(t);
MoveMotors(82, 86, 180, 155, 90, 90);//62
delay(t);
MoveMotors(82, 82, 160, 180, 90, 90);//63
delay(t);
MoveMotors(90, 90, 90, 90, 90, 90);//64
delay(t);
}
void Sequence4(){
int t = 1000;
MoveMotors(90, 90, 90, 90, 90, 90);//1
delay(t);
MoveMotors(78, 94, 133, 62, 90, 90);//2
delay(t);
MoveMotors(76, 130, 130, 96, 90, 90);//3
delay(t);
MoveMotors(76, 134, 142, 110, 86, 90);//4
delay(t);
MoveMotors(76, 138, 145, 118, 85, 90);//5
delay(t);
MoveMotors(76, 141, 145, 119, 90, 90);//6
delay(t);
MoveMotors(66, 142, 145, 121, 90, 90);//7
delay(t);
MoveMotors(66, 138, 145, 120, 88, 90);//8
delay(t);
MoveMotors(65, 133, 145, 121, 88, 90);//9
delay(t);
MoveMotors(72, 114, 176, 121, 88, 90);//10
delay(t);
MoveMotors(73, 123, 176, 130, 90, 90);//11
delay(t);
MoveMotors(61, 127, 172, 130, 90, 90);//12
delay(t);
MoveMotors(61, 118, 172, 128, 90, 90);//13
delay(t);
MoveMotors(71, 102, 180, 103, 90, 90);//14
delay(t);
MoveMotors(73, 112, 172, 86, 89, 90);//15
delay(t);
MoveMotors(58, 116, 167, 86, 89, 90);//16
delay(t);
MoveMotors(58, 110, 167, 86, 89, 90);//17
delay(t);
MoveMotors(62, 131, 138, 110, 90, 90);//18
delay(t);
MoveMotors(64, 139, 137, 110, 90, 90);//19
delay(t);
MoveMotors(66, 142, 145, 121, 90, 90);//20
delay(t);
MoveMotors(62, 135, 144, 101, 88, 90);//21
delay(t);
MoveMotors(62, 130, 145, 84, 87, 90);//22
delay(t);
MoveMotors(60, 128, 145, 74, 87, 90);//23
delay(t);
MoveMotors(59, 126, 145, 63, 90, 90);//24
delay(t);
MoveMotors(71, 111, 145, 98, 90, 90);//25
delay(t);
MoveMotors(78, 134, 145, 111, 90, 90);//26
delay(t);
MoveMotors(85, 141, 145, 122, 88, 90);//27
delay(t);
MoveMotors(85, 131, 159, 121, 88, 90);//28
delay(t);
MoveMotors(85, 123, 172, 121, 90, 90);//29
delay(t);
MoveMotors(79, 113, 180, 121, 90, 90);//30
delay(t);
MoveMotors(79, 108, 180, 102, 90, 90);//31
delay(t);
MoveMotors(79, 85, 180, 102, 90, 90);//32
delay(t);
MoveMotors(89, 129, 140, 109, 90, 90);//33
delay(t);
MoveMotors(88, 138, 139, 111, 90, 90);//34
delay(t);
MoveMotors(89, 144, 138, 116, 90, 90);//35
delay(t);
MoveMotors(89, 137, 150, 116, 90, 90);//36
delay(t);
MoveMotors(89, 124, 169, 116, 90, 90);//37
delay(t);
MoveMotors(90, 117, 168, 95, 90, 90);//38
delay(t);
MoveMotors(91, 115, 168, 83, 90, 90);//39
delay(t);
MoveMotors(91, 105, 168, 111, 90, 90);//40
delay(t);
MoveMotors(89, 138, 135, 111, 90, 90);//41
delay(t);
MoveMotors(89, 140, 139, 113, 90, 90);//42
delay(t);
MoveMotors(89, 144, 138, 116, 90, 90);//43
delay(t);
MoveMotors(87, 135, 151, 113, 90, 90);//44
delay(t);
MoveMotors(86, 130, 158, 113, 90, 90);//45
delay(t);
MoveMotors(84, 125, 166, 112, 90, 90);//46
delay(t);
MoveMotors(95, 127, 163, 113, 90, 90);//47
delay(t);
MoveMotors(95, 111, 163, 111, 90, 90);//48
delay(t);
MoveMotors(78, 64, 170, 67, 90, 90);//49
delay(t);
MoveMotors(80, 125, 116, 109, 165, 90);//50
delay(t);
MoveMotors(80, 85, 180, 180, 180, 90);//51
delay(t);
MoveMotors(90, 90, 60, 20, 90, 90);//52
delay(t);
}
void Sequence5(){
int t = 1000;
MoveMotors(90, 90, 90, 90, 90, 90);//1
delay(t);
MoveMotors(75, 97, 130, 86, 90, 90);//2
delay(t);
MoveMotors(76, 126, 105, 86, 90, 90);//3
delay(t);
MoveMotors(78, 150, 95, 90, 86, 90);//4
delay(t);
MoveMotors(77, 157, 97, 91, 90, 90);//5
delay(t);
MoveMotors(77, 120, 123, 91, 90, 90);//6
delay(t);
MoveMotors(77, 118, 137, 92, 90, 90);//7
delay(t);
MoveMotors(77, 103, 154, 91, 90, 90);//8
delay(t);
MoveMotors(76, 97, 168, 90, 90, 90);//9
delay(t);
MoveMotors(75, 88, 182, 90, 90, 90);//10
delay(t);
MoveMotors(75, 84, 180, 70, 90, 90);//11
delay(t);
MoveMotors(80, 84, 180, 70, 90, 90);//12
delay(t);
MoveMotors(80, 91, 173, 78, 90, 90);//13
delay(t);
MoveMotors(82, 91, 172, 78, 90, 90);//14
delay(t);
MoveMotors(83, 91, 170, 70, 90, 90);//15
delay(t);
MoveMotors(85, 92, 168, 77, 90, 90);//16
delay(t);
MoveMotors(90, 93, 169, 77, 90, 90);//17
delay(t);
MoveMotors(90, 93, 169, 88, 90, 90);//18
delay(t);
MoveMotors(96, 93, 169, 88, 90, 90);//19
delay(t);
MoveMotors(93, 102, 158, 79, 90, 90);//20
delay(t);
MoveMotors(89, 102, 152, 80, 90, 90);//21
delay(t);
MoveMotors(93, 105, 152, 87, 90, 90);//22
delay(t);
MoveMotors(89, 108, 152, 95, 90, 90);//23
delay(t);
MoveMotors(91, 118, 152, 111, 90, 90);//24
delay(t);
MoveMotors(87, 118, 152, 111, 90, 90);//25
delay(t);
MoveMotors(90, 122, 148, 125, 90, 90);//26
delay(t);
MoveMotors(87, 122, 148, 125, 90, 90);//27
delay(t);
MoveMotors(88, 130, 136, 124, 90, 90);//28
delay(t);
MoveMotors(85, 130, 136, 124, 90, 90);//29
delay(t);
MoveMotors(86, 140, 128, 124, 90, 90);//30
delay(t);
MoveMotors(83, 140, 128, 124, 90, 90);//31
delay(t);
MoveMotors(82, 143, 123, 124, 90, 90);//32
delay(t);
MoveMotors(80, 145, 123, 124, 90, 90);//33
delay(t);
MoveMotors(83, 103, 125, 102, 90, 90);//34
delay(t);
MoveMotors(88, 73, 175, 151, 90, 90);//35
delay(t);
MoveMotors(88, 68, 83, 85, 90, 90);//36
delay(t);
MoveMotors(88, 6, 160, 4, 180, 90);//37
delay(t);
MoveMotors(87, 87, 141, 115, 37, 90);//38
delay(t);
MoveMotors(0, 76, 141, 180, 180, 90);//39
delay(t);
MoveMotors(0, 76, 141, 180, 180, 90);//40
delay(t);
MoveMotors(81, 80, 130, 90, 180, 90);//41
delay(t);
MoveMotors(120, 40, 130, 0, 180, 90);//42
delay(t);
MoveMotors(170, 60, 180, 180, 180, 90);//43
delay(t);
MoveMotors(84, 70, 135, 133, 45, 90);//44
delay(t);
MoveMotors(84, 85, 120, 90, 180, 90);//45
delay(t);
MoveMotors(84, 17, 180, 90, 0, 90);//46
delay(t);
delay(5000);
}
void BlinkGreen(){
digitalWrite(SFWLEDPin, HIGH);
delay(200);
digitalWrite(SFWLEDPin, LOW);
delay(200);
digitalWrite(SFWLEDPin, HIGH);
delay(200);
digitalWrite(SFWLEDPin, LOW);
delay(200);
digitalWrite(SFWLEDPin, HIGH);
delay(200);
digitalWrite(SFWLEDPin, LOW);
delay(200);
}
void BlinkRed(){
digitalWrite(DNGLEDPin, HIGH);
delay(200);
digitalWrite(DNGLEDPin, LOW);
delay(200);
digitalWrite(DNGLEDPin, HIGH);
delay(200);
digitalWrite(DNGLEDPin, LOW);
delay(200);
digitalWrite(DNGLEDPin, HIGH);
delay(200);
digitalWrite(DNGLEDPin, LOW);
delay(200);
}
void SendData(int w1, int w2, int w3, int w4, int w5, int w6){
String q1 = FormatNumber(w1);
String q2 = FormatNumber(w2);
String q3 = FormatNumber(w3);
String q4 = FormatNumber(w4);
String q5 = FormatNumber(w5);
String q6 = FormatNumber(w6);
String data = q1 + q2 + q3 + q4 + q5 + q6 + "\n";
Serial.println(data);
}
String FormatNumber(int a){
String sa;
if(a >= 100) sa = String(a);
if(a >= 10 && a <= 99) sa = "0" + String(a);
if(a <= 9) sa = "00" + String(a);
return sa;
}
void loop() {
//Sequence2();
Serial.println(MakeString());
delay(100);
}
String MakeString1(){
int x1 = analogRead(A1);
int x2 = analogRead(A2);
int x3 = analogRead(A3);
int x4 = analogRead(A4);
int x5 = analogRead(A5);
if(x1 < 100) x1 = 100;
if(x1 > 999) x1 = 999;
if(x2 < 100) x2 = 100;
if(x2 > 999) x2 = 999;
if(x3 < 100) x3 = 100;
if(x3 > 999) x3 = 999;
if(x4 < 100) x4 = 100;
if(x4 > 999) x4 = 999;
if(x5 < 100) x5 = 100;
if(x5 > 999) x5 = 999;
String s = String(x1) + String(x2) + String(x3) + String(x4) + String(x5) + "000" + "\n";
return s;
}
String MakeString(){
int x1 = analogRead(A6);
int x2 = analogRead(A7);
int x3 = analogRead(A3);
int x4 = analogRead(A4);
int x5 = analogRead(A5);
int a1 = map(x1, min1, max1, 0, 180);
int a2 = map(x2, min2, max2, 45, 135);
int a3 = map(x3, min3, max3, 45, 135);
int a4 = map(x4, min4, max4, 0, 180);
int a5 = map(x5, min5, max5, 0, 180);
String s = FormatNumber(a1) + FormatNumber(a2) + FormatNumber(a3) + FormatNumber(a4) + FormatNumber(a5) + "000" + "\n";
return s;
}
void PowerUpSound(){
digitalWrite(buzzerPin, HIGH);
delay(100);
digitalWrite(buzzerPin, LOW);
delay(100);
digitalWrite(buzzerPin, HIGH);
delay(100);
digitalWrite(buzzerPin, LOW);
delay(100);
digitalWrite(buzzerPin, HIGH);
delay(100);
digitalWrite(buzzerPin, LOW);
delay(100);
}
void DangerSound(){
int tx = 400;
int ty = 60;
for(int i = 0; i < 3; i++){
digitalWrite(buzzerPin, HIGH);
delay(tx);
digitalWrite(buzzerPin, LOW);
delay(ty);
}
}
void SafeSound(){
int tx = 150;
int ty = 100;
for(int i = 0; i < 2; i++){
digitalWrite(buzzerPin, HIGH);
delay(tx);
digitalWrite(buzzerPin, LOW);
delay(ty);
}
}
6. Playing around and testing
After all of that, we finally get to the fun part, testing and playing with the robot. Before we can teach the robot any kind of movement we first need to do the calibration procedure since this will give us proper angle values which we can write down, and later on, which the Mega can save to the microSD card.
Calibration Sequence
Sequences
These are the sequences I tried writing down by hand, of course, my steps aren't the smallest thing and I would need to sit for hours to write down that small increments, but it still gave some cool looking results, here is how one of the sequences looked on paper.
In this first video I just made a simple mount for the marker and wanted to make a few lines on a piece of paper:
Here is the arm practicing some moves in mid air:
And here is the arm trying to write down E14, I went to fast for some sections, so it slammed into the paper a bit as you can see, in the beginning you can see a promising beginning of the letter E, as well as some pretty good straight lines:
7. Future Plans
This was a really fun build but I'm obviously not done with it yet. First of all I want to get the Record/Pause/Play functionality going with the buttons on the HMI and also be able to alter the speed at which the robot is moving. The braccio itself is really a great display to get an understanding of how industrial robot arms perform, but of course, specially considering the price, the motors aren't that powerful, we have limited movement, and there is a lot of backlash in the system in general, but never the less, it's one seriously amazing product. This is something I plan on using in my 1 Meter of Pie design challenge, since I can record different sequences and just use the Raspberry to tell the robot what to do, I'll post updates as I go on!
8. Summary
This was a fun project I've been planning to do for a while, the main enabler for this project was definitely the 3D printer. I wanted to do a similar thing last year, but I couldn't find a way to mount these sensors reliably. I will be tackling the software and finish up that soon, and then it will be ready for a lot of projects and experimentation that I have planned for it. Thanks for reading the blog, hope you liked it!
Milos
Top Comments