mdchaparror/balancing_robot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

inicial

Browse Source
This commit is contained in:
Martin Daniel Chaparro Ruiz
2018-07-02 21:56:34 -05:00
parent 5bb74adf96
commit 56581a0a5b
10 changed files with 1084 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
pruebas/pruebaAccel.cpp Normal file
View File

@@ -0,0 +1,83 @@
#include <Arduino.h>
#include <Wire.h>
//Direccion I2C de la IMU
#define MPU 0x68
//Ratios de conversion
#define A_R 16384.0 // 32768/2
#define G_R 131.0 // 32768/250
//Conversion de radianes a grados 180/PI
#define RAD_A_DEG = 57.295779
//MPU-6050 da los valores en enteros de 16 bits
//Valores RAW
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
//Angulos
float Acc[2];
float Gy[3];
float Angle[3];
String valores;
long tiempo_prev;
float dt;
void setup()
{
Wire.begin(D2,D1); // D2(GPIO4)=SDA / D1(GPIO5)=SCL
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(115200);
}
void loop()
{
//Leer los valores del Acelerometro de la IMU
Wire.beginTransmission(MPU);
Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
Wire.endTransmission(false);
Wire.requestFrom(MPU,6); //A partir del 0x3B, se piden 6 registros
AcX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
//A partir de los valores del acelerometro, se calculan los angulos Y, X
//respectivamente, con la formula de la tangente.
Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
//Leer los valores del Giroscopio
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU,6); //A partir del 0x43, se piden 6 registros
GyX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
GyY=Wire.read()<<8|Wire.read();
GyZ=Wire.read()<<8|Wire.read();
//Calculo del angulo del Giroscopio
Gy[0] = GyX/G_R;
Gy[1] = GyY/G_R;
Gy[2] = GyZ/G_R;
dt = (millis() - tiempo_prev) / 1000.0;
tiempo_prev = millis();
//Aplicar el Filtro Complementario
Angle[0] = 0.98 *(Angle[0]+Gy[0]*dt) + 0.02*Acc[0];
Angle[1] = 0.98 *(Angle[1]+Gy[1]*dt) + 0.02*Acc[1];
//Integración respecto del tiempo paras calcular el YAW
Angle[2] = Angle[2]+Gy[2]*dt;
//Mostrar los valores por consola
valores = "90, " +String(Angle[0]) + "," + String(Angle[1]) + "," + String(Angle[2]) + ", -90";
Serial.println(valores);
delay(10);
}

184
pruebas/pruebaMotoresyJostick.cpp Normal file
View File

@@ -0,0 +1,184 @@
#include <ESP8266WiFi.h>
#include <WebSocketsServer.h>
#include <ESP8266WebServer.h>
#include <ESP8266mDNS.h>
#include "FS.h"
volatile int estado = HIGH;
volatile int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory;
volatile int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory;
unsigned long loop_timer;
unsigned char Dir_m_left=D5;
unsigned char Step_m_left=D6;
unsigned char Dir_m_right=D3;
unsigned char Step_m_right=D4;
bool FWD=HIGH;
bool BWD=LOW;
volatile int valor=0;
int descI=0;
int descR=0;
extern "C" {
#include "user_interface.h"
}
os_timer_t myTimer;
bool tickOccured;
// start of timerCallback
void timerCallback(void *pArg) {
throttle_counter_left_motor ++; //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
if(throttle_counter_left_motor > throttle_left_motor_memory){ //If the number of loops is larger then the throttle_left_motor_memory variable
throttle_counter_left_motor = 0; //Reset the throttle_counter_left_motor variable
throttle_left_motor_memory = throttle_left_motor; //Load the next throttle_left_motor variable
if(throttle_left_motor_memory < 0){ //If the throttle_left_motor_memory is negative
digitalWrite(Dir_m_left,BWD); //Set output 3 low to reverse the direction of the stepper controller
throttle_left_motor_memory *= -1; //Invert the throttle_left_motor_memory variable
}
else digitalWrite(Dir_m_left,FWD); //Set output 3 high for a forward direction of the stepper motor
}
else if(throttle_counter_left_motor == 1)digitalWrite(Step_m_left,HIGH); //Set output 2 high to create a pulse for the stepper controller
else if(throttle_counter_left_motor == 2)digitalWrite(Step_m_left,LOW); //Set output 2 low because the pulse only has to last for 20us
//right motor pulse calculations
throttle_counter_right_motor ++; //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
if(throttle_counter_right_motor > throttle_right_motor_memory){ //If the number of loops is larger then the throttle_right_motor_memory variable
throttle_counter_right_motor = 0; //Reset the throttle_counter_right_motor variable
throttle_right_motor_memory = throttle_right_motor; //Load the next throttle_right_motor variable
if(throttle_right_motor_memory < 0){ //If the throttle_right_motor_memory is negative
digitalWrite(Dir_m_right,BWD); //Set output 5 low to reverse the direction of the stepper controller
throttle_right_motor_memory *= -1; //Invert the throttle_right_motor_memory variable
}
else digitalWrite(Dir_m_right,FWD);; //Set output 5 high for a forward direction of the stepper motor
}
else if(throttle_counter_right_motor == 1)digitalWrite(Step_m_right,HIGH); //Set output 4 high to create a pulse for the stepper controller
else if(throttle_counter_right_motor == 2)digitalWrite(Step_m_right,LOW);
} // End of timerCallback
void user_init(void) {
os_timer_setfn(&myTimer, timerCallback, NULL);
os_timer_arm(&myTimer, 1, true);
} // End of user_init
const char* ssid = "mdchaparror";
const char* password = "un260874";
int contconexion = 0;
ESP8266WebServer server = ESP8266WebServer(80);
void setup() {
pinMode(Dir_m_left,OUTPUT);
pinMode(Step_m_left,OUTPUT);
pinMode(Dir_m_right,OUTPUT);
pinMode(Step_m_right,OUTPUT);
//digitalWrite(Dir_m_left,HIGH);
Serial.begin(115200);
SPIFFS.begin();
Serial.println("");
Serial.println("--------------------------");
Serial.println("ESP8266 Motor Test");
Serial.println("--------------------------");
user_init();
WiFi.mode(WIFI_STA); //para que no inicie el SoftAP en el modo normal
WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED and contconexion <50) { //Cuenta hasta 50 si no se puede conectar lo cancela
++contconexion;
delay(500);
Serial.print(".");
}
if (contconexion <50) {
Serial.println("");
Serial.println("WiFi conectado");
Serial.println(WiFi.localIP());
}
else {
Serial.println("");
Serial.println("Error de conexion");
}
if(MDNS.begin("motores")) {
Serial.println("MDNS responder started");
}
// handle index
server.on("/", []() {
File index = SPIFFS.open("/index.html", "r");
server.streamFile(index, "text/html");
index.close();
});
server.on("/stick", []() { // Recibe los datos (dx y dy)
String dx=server.arg("dx");
String dy=server.arg("dy");
server.send(200);
int x = dx.toInt(); // Convierte los datos de tipo String a Int
int y = dy.toInt();
valor = map(abs(y),0,255,20,3);
if (y==0)valor=0;
if(y<0) valor*=-1;
if(x>100){
descI =3;
descR =-3;
}
if(x<-100) {
descI =-3;
descR =3;
}
if(x<=100 && x>=-100) {
descI =0;
descR =0;
}
throttle_left_motor = valor+descI;
throttle_right_motor = valor+descR;
Serial.println(valor);
// Serial.print(x); Serial.print(" "); Serial.println(y);
});
server.begin();
// Add service to MDNS
MDNS.addService("http", "tcp", 80);
MDNS.addService("ws", "tcp", 81);
}
void loop() {
server.handleClient();
// throttle_left_motor = 10;
//throttle_right_motor = 10;
while(loop_timer > micros());
loop_timer += 4000;
}