Hardware components | ||||||
_ztBMuBhMHo.jpg?auto=compress%2Cformat&w=48&h=48&fit=fill&bg=ffffff) |
| × | 3 | |||
| × | 1 | ||||
| × | 2 | ||||
| × | 1 | ||||
 |
| × | 2 | |||
 |
| × | 4 | |||
Software apps and online services | ||||||
 |
| |||||
Hand tools and fabrication machines | ||||||
 |
| |||||
| ||||||
See a video of the self-balancing robot:
https://drive.google.com/open?id=0B3LMiO6FQEExYzNTcVVsMEp4TEE
.........................................
If you did like this project you can donate some cents of Crypto currency
Bitcoins SV in the following address.
1sAZQMy5Ci1G88CmbucFJDsF7TxeXAcko
or bitcoin cash in the address:
qzmjpeqrlgd3flltpavm9t2xh0nz8y97mggajvvqnv
or Stellar XLM in the address:
GBAWDPQ4FTRXWE2ZUWVYYZ7XSCQGBYZGOWQMAHSA2FPGJ5QTLF4IJ3NX
thanks
#include "Wire.h"
#include "SPI.h"
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset -1
#define Gyr_Gain 0.00763358
//#define Gyr_Gain 0.003358
#define Angle_offset 1 //0
#define RMotor_offset 15
#define LMotor_offset 15
#define pi 3.14159
int sensorpin = A0;
int sensorValue=0;
int luz,luz2;
long data;
int x, y,i;
float kp, ki, kd;
float r_angle, r_angle1,r_angle2, f_angle, omega;
float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0;
float LOutput,ROutput;
//const int ledPin13 = 13;
//const int ledPin12 = 12;
unsigned long preTime = 1;
float SampleTime = 0.08; // RAFA antes 0.08
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;
////////////////// ENTRADAS Y SALIDAS USADAS ////////////////////////////////////////////
// D2,D3,D4,D5,D6,D9,D10
//
// PARA EL LCD SE REQUIEREN 6 LIBRES , a continuacin , entre parntesis su uso con el 24L01
// D1,D7(SCN),D8(CE),D11(MOSI),D12(MISO),D13(SCK)
//
// EL MIRF 24L01 usa los pines D7(SCN),D8(CE),D11(MOSI),D12(MISO),D13(SCK)
//
int TN1=2;
int TN2=4;
int ENA=9;
int TN3=5;
int TN4=6;
int ENB=10;
int LUCES=3;
int muestreo_envio_aTFT;
void setup() {
muestreo_envio_aTFT=0;
//Para dar tiempo a conectar
pinMode(LUCES, OUTPUT);
for (i=0;i<10;i++)
{
analogWrite(LUCES,128);
delay(15);
analogWrite(LUCES,0);
delay(250);
}
Wire.begin();
accelgyro.initialize();
pinMode(TN1,OUTPUT);
pinMode(TN2,OUTPUT);
pinMode(TN3,OUTPUT);
pinMode(TN4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
Mirf.spi = &MirfHardwareSpi;
Mirf.init();
Mirf.setRADDR((byte *)"serv1");
Mirf.payload = sizeof(long);
Mirf.config();
Serial.begin(9600);
Serial.println("Fichero: balancing_robot090217");
}
void loop() {
Recive();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ay, az) * 180 / pi + Angle_offset); //ngulo del robot sobre el eje de sus ruedas, o perpendicularidad
r_angle1=r_angle;
omega = Gyr_Gain * (gx + Gry_offset);
if (abs(r_angle)<45){ //<45
myPID();
PWMControl();
}
else{
analogWrite(ENA, 0);
analogWrite(ENB, 0);
analogWrite(LUCES,0);
}
r_angle2=(r_angle1+r_angle)/2; //filtro de media dinmica
//Con este IF enviando 1 valor de cada 150, evitamos que el receptor se atragante.
if (muestreo_envio_aTFT==150) {
Serial.println(r_angle2);
muestreo_envio_aTFT=0;
}
else {
muestreo_envio_aTFT++;
}
}
void Recive(){
if(!Mirf.isSending() && Mirf.dataReady()){
Mirf.getData((byte *)&data);
Mirf.rxFifoEmpty();
x = data >> 16;
y = data & 0x0000ffff;
if(x >= 520){
Run_Speed_K = map(x, 520, 1023, 0, 50);
Run_Speed_K = Run_Speed_K / 30;
Run_Speed = Run_Speed + Run_Speed_K;
}
else if(x <= 480){
Run_Speed_K = map(x, 480, 0, 0, -100);
Run_Speed_K = Run_Speed_K / 30;
Run_Speed = Run_Speed + Run_Speed_K;
}
else{
Run_Speed_K = 0;
}
if(y >= 520){
Turn_Speed = map(y, 520, 1023, 0, 30);
}
else if(y <= 480){
Turn_Speed = map(y,480,0,0,-30);
}
else{
Turn_Speed = 0;
}
}
else{
x = y = 500;
}
}
void myPID(){
// original this line not commented --- > kp = analogRead(A0)*0.1; Serial.print(" kp=");Serial.print(kp);
// original this line not commented --- > kd = analogRead(A2)*1.0; Serial.print(" kd=");Serial.print(kd);
//ki = analogRead(A3)*0.001; Serial.print(" ki=");Serial.print(ki);
kp = 40; //Serial.print(" kp=");Serial.print(kp); //30
kd = 650; //Serial.print(" kd=");Serial.print(kd); //630
ki = 0.1; //Serial.print(" ki=");Serial.print(ki); //0.07
unsigned long now = millis();
float dt = (now - preTime) / 900.0;
preTime = now;
float K = 0.9;
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle;
timeChange = (now - lastTime);
if(timeChange >= SampleTime){
Input = f_angle;
error = Input;
errSum += error * timeChange;
dErr = (error - lastErr) / timeChange;
Output = kp * error + ki * errSum + kd * dErr; //Serial.print(" Output=");Serial.print(Output);
LOutput = Output + Run_Speed + Turn_Speed; //Serial.print(" LOutput=");Serial.print(LOutput);
ROutput = Output + Run_Speed - Turn_Speed; //Serial.print(" ROutput=");Serial.println(ROutput);
//Serial.print(" f_angle=");Serial.print(f_angle);Serial.print(" Run_Speed=");Serial.print(Run_Speed);Serial.print(" Turn_Speed=");Serial.print(Turn_Speed);
lastErr = error;
lastTime = now;
analogWrite(LUCES,abs(Output/10));//
//Serial.println(r_angle);
}
}
void PWMControl(){
if(LOutput > 0){
digitalWrite(TN1, HIGH);
digitalWrite(TN2, LOW);
}
else if(LOutput < 0){
digitalWrite(TN1, LOW);
digitalWrite(TN2, HIGH);
}
else{
digitalWrite(TN1, HIGH);
digitalWrite(TN2, HIGH);
}
if(ROutput > 0){
digitalWrite(TN3, HIGH);
digitalWrite(TN4, LOW);
}
else if(ROutput < 0){
digitalWrite(TN3, LOW);
digitalWrite(TN4, HIGH);
}
else{
digitalWrite(TN3, HIGH);
digitalWrite(TN4, HIGH);
}
analogWrite(ENA, min(255, abs(LOutput) + LMotor_offset));
analogWrite(ENB, min(255, abs(ROutput) + RMotor_offset));
}
#include "SPI.h"
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.h"
#include "Wire.h"
long Joystick_1_X;
long Joystick_1_Y;
long Joystick_2_X;
long Joystick_2_Y;
long Joystick_1;
long Joystick_2;
long Joystick_2_XORIG;
int IN4 = 4;
int IN7 = 7; //SW included in the joystick 2. Due to the Pull up internal resistor of the Microcontroller is selected when is not pressed the input will have 5V
int LED2 =2; // OFF when SWJoystick2 is pressed
void setup(){
pinMode(IN4, INPUT_PULLUP); // The input will have 5V (logic value of HIGH) due to the internal pull up resistor of 20Kohms of the ATmega is selected. If SW of the Joystick is pressed the input will have 0 Vols and a logic value of LOW.
pinMode(IN7, INPUT_PULLUP); // The input will have 5V (logic value of HIGH) due to the internal pull up resistor of 20Kohms of the ATmega is selected. If SW of the Joystick is pressed the input will have 0 Vols and a logic value of LOW.
pinMode(LED2, OUTPUT);
Serial.begin(115200);
Mirf.spi = &MirfHardwareSpi;
Mirf.init();
Mirf.setTADDR((byte *)"serv1");
Mirf.payload = sizeof(long);
Mirf.config();
}
void loop(){
Joystick_1_X = analogRead(A0);
Joystick_1_Y = analogRead(A1);
Joystick_2_X = analogRead(A2);
Joystick_2_Y = analogRead(A3);
Joystick_1_X = Joystick_1_X << 16; // Here the value from the A0 is shifted 16 positions to the left
Joystick_1 = Joystick_1_X | Joystick_1_Y; // Here it makes the OR logic of both A0(16 << bit shifted) and A1 from the joystick
//delay(1000); // RAFA Just for debugging with serial monitor, to avoid hung windows.
Joystick_2_XORIG = Joystick_2_X; // Not used, just for trials
Joystick_2_X = Joystick_2_X << 16; // DESPLAZA TODOS LOS BITS 16 POSICIONES A LA IZQ. Not used, just for trials
Joystick_2 = (Joystick_2_X | Joystick_2_Y);// AQU HACE EL OR LGICO BIT A BIT Not used, just for trials
if (digitalRead(IN7)==HIGH) {
Mirf.send((byte *)&Joystick_1);
//digitalWrite(LED13,HIGH);Serial.print(" SW joystick2=");Serial.print(digitalRead(IN7));
//delay(10);
//Mirf.send((byte *)&Joystick_2);
} else {
Joystick_1=33620479; //Value composed when Joystick1 is in repose, center position. This value is sent when the SWitch of the Joystick2 is pressed.
Mirf.send((byte *)&Joystick_1);
}
while(Mirf.isSending()){
/// Nothing
}
digitalWrite(LED2,digitalRead(IN7)) ; // OFF when SWJoystick2 is pressed
Serial.print(" Joystick_1=");Serial.print(Joystick_1);Serial.print(" SWJoystick2=");Serial.println(digitalRead(IN7));
}
#include <gfxfont.h>
#include <Adafruit_GFX.h> // Hardware-specific library
#include <Adafruit_TFTLCD.h> // Hardware-specific library
#include <SPI.h>
#include <SD.h>
#define SD_CS 10 // CHIP SELECT CARTA SD DEL TFT
#define LCD_CS A3 // Chip Select goes to Analog 3
#define LCD_CD A2 // Command/Data goes to Analog 2
#define LCD_WR A1 // LCD Write goes to Analog 1
#define LCD_RD A0 // LCD Read goes to Analog 0
#define LCD_RESET A4 // Can alternately just connect to Arduino's reset pin
// Assign human-readable names to some common 16-bit color values:
#define BLACK 0x0000
#define BLUE 0x001F
#define BLUE2 0x00FF
#define RED 0xF800
#define GREEN 0x07E0
#define CYAN 0x07FF
#define MAGENTA 0xF81F
#define YELLOW 0xFFE0
#define WHITE 0xFFFF
int n=0;
float i;
Adafruit_TFTLCD tft(LCD_CS, LCD_CD, LCD_WR, LCD_RD, LCD_RESET);
String inString = "";
void setup()
{
uint16_t tmp;
int m;
Serial.begin(9600);
tft.reset();
delay(500);
tft.begin(9600);
delay(500);
uint16_t identifier = tft.readID();
tft.begin(identifier);
tft.fillScreen(BLACK); //Borra pantalla
//Escribe en la TFT los nombres de los ficheros del arduino del TFT y del del robot
tft.setRotation(1);
tft.setCursor(10, 10);
tft.setTextSize(2);
tft.setTextColor(BLUE);
tft.print("ID de la tft = 0x");
tft.println(identifier, HEX);
tft.println(" ");
tft.println(" ");
tft.println(" ");
tft.println("Ficheros:");
tft.println(" ");
tft.println("TFT_dibujar_pendulo3.INO");
tft.println(" ");
tft.println("balancing_robot090217.INO");
Serial.println("Ficheros:");
Serial.println(" ");
Serial.println("TFT_dibujar_pendulo3.INO");
Serial.println(" ");
Serial.println("balancing_robot090217.INO");
delay(1000);
tft.fillScreen(BLACK);
//Muestra una cuenta atrs
for (m=3;m>=0;m--){
tft.setCursor(125, 70);
tft.setTextSize(15);
tft.setTextColor(YELLOW);
tft.println(m);
delay(500);
tft.fillScreen(BLACK);
}
//Dibuja la rueda
tft.drawCircle(160, 190, 25,WHITE);
tft.drawCircle(160, 190, 24,WHITE);
tft.drawCircle(160, 190, 23,WHITE);
tft.drawCircle(160, 190, 22,WHITE);
tft.drawCircle(160, 190, 21,WHITE);
tft.drawCircle(160, 190, 20,WHITE);
tft.drawCircle(160, 190, 19,WHITE);
tft.fillCircle(160, 190, 5,BLUE);
//Dibuja el suelo verde
tft.fillRect(0,212,340,340,GREEN);
}
void loop()
{
// inputString = "";
tft.setCursor(67, 10);
tft.drawRect(60, 5, 185,25,BLUE);
tft.setTextSize(2);
tft.setTextColor(RED);
tft.println("ROBOT_PENDULAR");
dibuja_pendulo();
}
void dibuja_pendulo()
{
float r,rad,rad1;
int x1,y1,i1;
x1=0;
y1=0;
r=0.0;
int analogpin=A0;
tft.setTextSize(1);
tft.setRotation(1);
tft.drawLine(160,190,160+x1,190-y1,BLUE);
while(Serial.available()>0) {
int inChar=Serial.read();
if(inChar != '\n') {
inString += (char)inChar; // Si no es fin de string, append del caracter
tft.drawLine(160,190,160+x1,190-y1,BLACK);
}
else {
//Si ve fin string, coge la cadena, la convierte en float y dibuja la lnea variando parte de arriba segnangulo recibido.
tft.drawLine(160,190,160+x1,190-y1,BLACK);
r=125;
i=inString.toFloat();
Serial.println(i+90);
rad=(i+90)*2*PI/360; //pasa de grados a rad
x1=r*cos((PI/2)+(PI/2)-rad);
y1=r*sin((PI/2)+(PI/2)-rad);
tft.drawLine(160,190,160+x1,190-y1,BLUE);
tft.setCursor(240, 120);
tft.fillRect(242,120,80,20,BLACK);
tft.print("x1= ");
tft.println(x1);
tft.setCursor(240, 160);
tft.fillRect(242,160,80,20,BLACK);
tft.print("y1= ");
tft.println(y1);
tft.setCursor(85, 220);
tft.fillRect(87,220,80,70,GREEN);
tft.setTextColor(BLUE);
tft.setTextSize(2);
tft.print("Angulo= ");
tft.println(i+90);
tft.setTextSize(1);
tft.setTextColor(RED);
tft.drawCircle(160, 190, 25,WHITE);
tft.drawCircle(160, 190, 24,WHITE);
tft.drawCircle(160, 190, 23,WHITE);
tft.drawCircle(160, 190, 22,WHITE);
tft.drawCircle(160, 190, 21,WHITE);
tft.drawCircle(160, 190, 20,WHITE);
tft.drawCircle(160, 190, 19,WHITE);
tft.fillRect(0,212,340,340,GREEN);
tft.fillCircle(160, 190, 5,BLUE);
inString = "";
}
tft.drawLine(160,190,160+x1,190-y1,BLACK);
}
}
Rafa Juárez
18 projects • 39 followers
Very interested in prototyping of new ideas. 30 years experience in electronics.
_3u05Tpwasz.png?auto=compress%2Cformat&w=40&h=40&fit=fillmax&bg=fff&dpr=2)
Comments