inicial
This commit is contained in:
4
.gitignore
vendored
Normal file
4
.gitignore
vendored
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
.pioenvs
|
||||||
|
.piolibdeps
|
||||||
|
.clang_complete
|
||||||
|
.gcc-flags.json
|
||||||
67
.travis.yml
Normal file
67
.travis.yml
Normal file
@@ -0,0 +1,67 @@
|
|||||||
|
# Continuous Integration (CI) is the practice, in software
|
||||||
|
# engineering, of merging all developer working copies with a shared mainline
|
||||||
|
# several times a day < http://docs.platformio.org/page/ci/index.html >
|
||||||
|
#
|
||||||
|
# Documentation:
|
||||||
|
#
|
||||||
|
# * Travis CI Embedded Builds with PlatformIO
|
||||||
|
# < https://docs.travis-ci.com/user/integration/platformio/ >
|
||||||
|
#
|
||||||
|
# * PlatformIO integration with Travis CI
|
||||||
|
# < http://docs.platformio.org/page/ci/travis.html >
|
||||||
|
#
|
||||||
|
# * User Guide for `platformio ci` command
|
||||||
|
# < http://docs.platformio.org/page/userguide/cmd_ci.html >
|
||||||
|
#
|
||||||
|
#
|
||||||
|
# Please choice one of the following templates (proposed below) and uncomment
|
||||||
|
# it (remove "# " before each line) or use own configuration according to the
|
||||||
|
# Travis CI documentation (see above).
|
||||||
|
#
|
||||||
|
|
||||||
|
|
||||||
|
#
|
||||||
|
# Template #1: General project. Test it using existing `platformio.ini`.
|
||||||
|
#
|
||||||
|
|
||||||
|
# language: python
|
||||||
|
# python:
|
||||||
|
# - "2.7"
|
||||||
|
#
|
||||||
|
# sudo: false
|
||||||
|
# cache:
|
||||||
|
# directories:
|
||||||
|
# - "~/.platformio"
|
||||||
|
#
|
||||||
|
# install:
|
||||||
|
# - pip install -U platformio
|
||||||
|
# - platformio update
|
||||||
|
#
|
||||||
|
# script:
|
||||||
|
# - platformio run
|
||||||
|
|
||||||
|
|
||||||
|
#
|
||||||
|
# Template #2: The project is intended to by used as a library with examples
|
||||||
|
#
|
||||||
|
|
||||||
|
# language: python
|
||||||
|
# python:
|
||||||
|
# - "2.7"
|
||||||
|
#
|
||||||
|
# sudo: false
|
||||||
|
# cache:
|
||||||
|
# directories:
|
||||||
|
# - "~/.platformio"
|
||||||
|
#
|
||||||
|
# env:
|
||||||
|
# - PLATFORMIO_CI_SRC=path/to/test/file.c
|
||||||
|
# - PLATFORMIO_CI_SRC=examples/file.ino
|
||||||
|
# - PLATFORMIO_CI_SRC=path/to/test/directory
|
||||||
|
#
|
||||||
|
# install:
|
||||||
|
# - pip install -U platformio
|
||||||
|
# - platformio update
|
||||||
|
#
|
||||||
|
# script:
|
||||||
|
# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N
|
||||||
17
README.md
Normal file
17
README.md
Normal file
@@ -0,0 +1,17 @@
|
|||||||
|
Robot balanceador
|
||||||
|
|
||||||
|
|
||||||
|
http://www.brokking.net/yabr_main.html
|
||||||
|
|
||||||
|
http://axelsdiy.brinkeby.se/?page_id=1141
|
||||||
|
http://www.brokking.net/yabr_main.html
|
||||||
|
http://axelsdiy.brinkeby.se/?page_id=1141
|
||||||
|
https://robologs.net/2014/10/15/tutorial-de-arduino-y-mpu-6050/
|
||||||
|
https://www.luisllamas.es/medir-la-inclinacion-imu-arduino-filtro-complementario/
|
||||||
|
|
||||||
|
- nodemcu v1.0 esp8266
|
||||||
|
- 2 motores nema 17
|
||||||
|
- 2 driver motores
|
||||||
|
- MPU6050
|
||||||
|
- 7805
|
||||||
|
- bateria LiPo
|
||||||
BIN
documentos/MPU-6000-Manual.pdf
Normal file
BIN
documentos/MPU-6000-Manual.pdf
Normal file
Binary file not shown.
41
lib/readme.txt
Normal file
41
lib/readme.txt
Normal file
@@ -0,0 +1,41 @@
|
|||||||
|
|
||||||
|
This directory is intended for the project specific (private) libraries.
|
||||||
|
PlatformIO will compile them to static libraries and link to executable file.
|
||||||
|
|
||||||
|
The source code of each library should be placed in separate directory, like
|
||||||
|
"lib/private_lib/[here are source files]".
|
||||||
|
|
||||||
|
For example, see how can be organized `Foo` and `Bar` libraries:
|
||||||
|
|
||||||
|
|--lib
|
||||||
|
| |
|
||||||
|
| |--Bar
|
||||||
|
| | |--docs
|
||||||
|
| | |--examples
|
||||||
|
| | |--src
|
||||||
|
| | |- Bar.c
|
||||||
|
| | |- Bar.h
|
||||||
|
| | |- library.json (optional, custom build options, etc) http://docs.platformio.org/page/librarymanager/config.html
|
||||||
|
| |
|
||||||
|
| |--Foo
|
||||||
|
| | |- Foo.c
|
||||||
|
| | |- Foo.h
|
||||||
|
| |
|
||||||
|
| |- readme.txt --> THIS FILE
|
||||||
|
|
|
||||||
|
|- platformio.ini
|
||||||
|
|--src
|
||||||
|
|- main.c
|
||||||
|
|
||||||
|
Then in `src/main.c` you should use:
|
||||||
|
|
||||||
|
#include <Foo.h>
|
||||||
|
#include <Bar.h>
|
||||||
|
|
||||||
|
// rest H/C/CPP code
|
||||||
|
|
||||||
|
PlatformIO will find your libraries automatically, configure preprocessor's
|
||||||
|
include paths and build them.
|
||||||
|
|
||||||
|
More information about PlatformIO Library Dependency Finder
|
||||||
|
- http://docs.platformio.org/page/librarymanager/ldf.html
|
||||||
16
platformio.ini
Normal file
16
platformio.ini
Normal file
@@ -0,0 +1,16 @@
|
|||||||
|
; PlatformIO Project Configuration File
|
||||||
|
;
|
||||||
|
; Build options: build flags, source filter
|
||||||
|
; Upload options: custom upload port, speed and extra flags
|
||||||
|
; Library options: dependencies, extra library storages
|
||||||
|
; Advanced options: extra scripting
|
||||||
|
;
|
||||||
|
; Please visit documentation for the other options and examples
|
||||||
|
; http://docs.platformio.org/page/projectconf.html
|
||||||
|
|
||||||
|
[env:nodemcuv2]
|
||||||
|
platform = espressif8266
|
||||||
|
board = nodemcuv2
|
||||||
|
framework = arduino
|
||||||
|
upload_speed = 921600
|
||||||
|
build_flags = -Wl,-Teagle.flash.4m1m.ld
|
||||||
83
pruebas/pruebaAccel.cpp
Normal file
83
pruebas/pruebaAccel.cpp
Normal 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
184
pruebas/pruebaMotoresyJostick.cpp
Normal 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;
|
||||||
|
|
||||||
|
}
|
||||||
488
src/data/index.html
Normal file
488
src/data/index.html
Normal file
@@ -0,0 +1,488 @@
|
|||||||
|
<html>
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8">
|
||||||
|
<meta name="viewport" content="width=device-width, user-scalable=no, minimum-scale=1.0, maximum-scale=1.0">
|
||||||
|
|
||||||
|
<style>
|
||||||
|
body {
|
||||||
|
overflow : hidden;
|
||||||
|
padding : 0;
|
||||||
|
margin : 0;
|
||||||
|
background-color: #FFFFFF;
|
||||||
|
}
|
||||||
|
#info {
|
||||||
|
position : absolute;
|
||||||
|
top : 0px;
|
||||||
|
width : 100%;
|
||||||
|
padding : 5px;
|
||||||
|
text-align : center;
|
||||||
|
}
|
||||||
|
#info a {
|
||||||
|
color : #66F;
|
||||||
|
text-decoration : none;
|
||||||
|
}
|
||||||
|
#info a:hover {
|
||||||
|
text-decoration : underline;
|
||||||
|
}
|
||||||
|
#container {
|
||||||
|
width : 100%;
|
||||||
|
height : 100%;
|
||||||
|
overflow : hidden;
|
||||||
|
padding : 0;
|
||||||
|
margin : 0;
|
||||||
|
-webkit-user-select : none;
|
||||||
|
-moz-user-select : none;
|
||||||
|
}
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<div id="container"></div>
|
||||||
|
<div id="info">
|
||||||
|
<b>JOYSTICK VIRTUAL</b><br><br>
|
||||||
|
<span id="result"></span>
|
||||||
|
</div>
|
||||||
|
<script>
|
||||||
|
var VirtualJoystick = function(opts)
|
||||||
|
{
|
||||||
|
opts = opts || {};
|
||||||
|
this._container = opts.container || document.body;
|
||||||
|
this._strokeStyle = opts.strokeStyle || 'cyan';
|
||||||
|
this._stickEl = opts.stickElement || this._buildJoystickStick();
|
||||||
|
this._baseEl = opts.baseElement || this._buildJoystickBase();
|
||||||
|
this._mouseSupport = opts.mouseSupport !== undefined ? opts.mouseSupport : false;
|
||||||
|
this._stationaryBase = opts.stationaryBase || false;
|
||||||
|
this._baseX = this._stickX = opts.baseX || screen.width/2
|
||||||
|
this._baseY = this._stickY = opts.baseY || screen.height/2
|
||||||
|
this._limitStickTravel = opts.limitStickTravel || true;
|
||||||
|
if (screen.width >= screen.height){
|
||||||
|
this._stickRadius = opts.stickRadius !== undefined ? opts.stickRadius : screen.height*0.35;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
this._stickRadius = opts.stickRadius !== undefined ? opts.stickRadius : screen.width*0.35;
|
||||||
|
};
|
||||||
|
this._useCssTransform = opts.useCssTransform !== undefined ? opts.useCssTransform : false
|
||||||
|
this._container.style.position = "relative"
|
||||||
|
this._container.appendChild(this._baseEl)
|
||||||
|
this._baseEl.style.position = "absolute"
|
||||||
|
this._baseEl.style.display = "none"
|
||||||
|
this._container.appendChild(this._stickEl)
|
||||||
|
this._stickEl.style.position = "absolute"
|
||||||
|
this._stickEl.style.display = "none"
|
||||||
|
this._pressed = false;
|
||||||
|
this._touchIdx = null;
|
||||||
|
if(this._stationaryBase === true){
|
||||||
|
this._baseEl.style.display = "";
|
||||||
|
this._baseEl.style.left = (this._baseX - this._baseEl.width /2)+"px";
|
||||||
|
this._baseEl.style.top = (this._baseY - this._baseEl.height/2)+"px";
|
||||||
|
}
|
||||||
|
this._transform = this._useCssTransform ? this._getTransformProperty() : false;
|
||||||
|
this._has3d = this._check3D();
|
||||||
|
var __bind = function(fn, me){ return function(){ return fn.apply(me, arguments); }; };
|
||||||
|
this._$onTouchStart = __bind(this._onTouchStart , this);
|
||||||
|
this._$onTouchEnd = __bind(this._onTouchEnd , this);
|
||||||
|
this._$onTouchMove = __bind(this._onTouchMove , this);
|
||||||
|
this._container.addEventListener( 'touchstart' , this._$onTouchStart , false );
|
||||||
|
this._container.addEventListener( 'touchend' , this._$onTouchEnd , false );
|
||||||
|
this._container.addEventListener( 'touchmove' , this._$onTouchMove , false );
|
||||||
|
if( this._mouseSupport ){
|
||||||
|
this._$onMouseDown = __bind(this._onMouseDown , this);
|
||||||
|
this._$onMouseUp = __bind(this._onMouseUp , this);
|
||||||
|
this._$onMouseMove = __bind(this._onMouseMove , this);
|
||||||
|
this._container.addEventListener( 'mousedown' , this._$onMouseDown , false );
|
||||||
|
this._container.addEventListener( 'mouseup' , this._$onMouseUp , false );
|
||||||
|
this._container.addEventListener( 'mousemove' , this._$onMouseMove , false );
|
||||||
|
}
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype.destroy = function()
|
||||||
|
{
|
||||||
|
this._container.removeChild(this._baseEl);
|
||||||
|
this._container.removeChild(this._stickEl);
|
||||||
|
this._container.removeEventListener( 'touchstart' , this._$onTouchStart , false );
|
||||||
|
this._container.removeEventListener( 'touchend' , this._$onTouchEnd , false );
|
||||||
|
this._container.removeEventListener( 'touchmove' , this._$onTouchMove , false );
|
||||||
|
if( this._mouseSupport ){
|
||||||
|
this._container.removeEventListener( 'mouseup' , this._$onMouseUp , false );
|
||||||
|
this._container.removeEventListener( 'mousedown' , this._$onMouseDown , false );
|
||||||
|
this._container.removeEventListener( 'mousemove' , this._$onMouseMove , false );
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @returns {Boolean} true if touchscreen is currently available, false otherwise
|
||||||
|
*/
|
||||||
|
VirtualJoystick.touchScreenAvailable = function()
|
||||||
|
{
|
||||||
|
return 'createTouch' in document ? true : false;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* microevents.js - https://github.com/jeromeetienne/microevent.js
|
||||||
|
*/
|
||||||
|
;(function(destObj){
|
||||||
|
destObj.addEventListener = function(event, fct){
|
||||||
|
if(this._events === undefined) this._events = {};
|
||||||
|
this._events[event] = this._events[event] || [];
|
||||||
|
this._events[event].push(fct);
|
||||||
|
return fct;
|
||||||
|
};
|
||||||
|
destObj.removeEventListener = function(event, fct){
|
||||||
|
if(this._events === undefined) this._events = {};
|
||||||
|
if( event in this._events === false ) return;
|
||||||
|
this._events[event].splice(this._events[event].indexOf(fct), 1);
|
||||||
|
};
|
||||||
|
destObj.dispatchEvent = function(event /* , args... */){
|
||||||
|
if(this._events === undefined) this._events = {};
|
||||||
|
if( this._events[event] === undefined ) return;
|
||||||
|
var tmpArray = this._events[event].slice();
|
||||||
|
for(var i = 0; i < tmpArray.length; i++){
|
||||||
|
var result = tmpArray[i].apply(this, Array.prototype.slice.call(arguments, 1))
|
||||||
|
if( result !== undefined ) return result;
|
||||||
|
}
|
||||||
|
return undefined
|
||||||
|
};
|
||||||
|
})(VirtualJoystick.prototype);
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// //
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
VirtualJoystick.prototype.deltaX = function(){ return this._stickX - this._baseX; }
|
||||||
|
VirtualJoystick.prototype.deltaY = function(){ return this._stickY - this._baseY; }
|
||||||
|
VirtualJoystick.prototype.up = function(){
|
||||||
|
if( this._pressed === false ) return false;
|
||||||
|
var deltaX = this.deltaX();
|
||||||
|
var deltaY = this.deltaY();
|
||||||
|
if( deltaY >= 0 ) return false;
|
||||||
|
if( Math.abs(deltaX) > 2*Math.abs(deltaY) ) return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype.down = function(){
|
||||||
|
if( this._pressed === false ) return false;
|
||||||
|
var deltaX = this.deltaX();
|
||||||
|
var deltaY = this.deltaY();
|
||||||
|
if( deltaY <= 0 ) return false;
|
||||||
|
if( Math.abs(deltaX) > 2*Math.abs(deltaY) ) return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype.right = function(){
|
||||||
|
if( this._pressed === false ) return false;
|
||||||
|
var deltaX = this.deltaX();
|
||||||
|
var deltaY = this.deltaY();
|
||||||
|
if( deltaX <= 0 ) return false;
|
||||||
|
if( Math.abs(deltaY) > 2*Math.abs(deltaX) ) return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype.left = function(){
|
||||||
|
if( this._pressed === false ) return false;
|
||||||
|
var deltaX = this.deltaX();
|
||||||
|
var deltaY = this.deltaY();
|
||||||
|
if( deltaX >= 0 ) return false;
|
||||||
|
if( Math.abs(deltaY) > 2*Math.abs(deltaX) ) return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// //
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
VirtualJoystick.prototype._onUp = function()
|
||||||
|
{
|
||||||
|
this._pressed = false;
|
||||||
|
this._stickEl.style.display = "none";
|
||||||
|
if(this._stationaryBase == true){
|
||||||
|
this._stickX = this._baseX;
|
||||||
|
this._stickY = this._baseY;
|
||||||
|
}
|
||||||
|
if(this._stationaryBase == false){
|
||||||
|
this._baseEl.style.display = "none";
|
||||||
|
this._stickX = this._baseX = 0;
|
||||||
|
this._stickY = this._baseY = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._onDown = function(x, y)
|
||||||
|
{
|
||||||
|
this._pressed = true;
|
||||||
|
if(this._stationaryBase == false){
|
||||||
|
this._baseX = x;
|
||||||
|
this._baseY = y;
|
||||||
|
this._baseEl.style.display = "";
|
||||||
|
this._move(this._baseEl.style, (this._baseX - this._baseEl.width /2), (this._baseY - this._baseEl.height/2));
|
||||||
|
}
|
||||||
|
this._stickX = x;
|
||||||
|
this._stickY = y;
|
||||||
|
if(this._limitStickTravel === true){
|
||||||
|
var deltaX = this.deltaX();
|
||||||
|
var deltaY = this.deltaY();
|
||||||
|
var stickDistance = Math.sqrt( (deltaX * deltaX) + (deltaY * deltaY) );
|
||||||
|
if(stickDistance > this._stickRadius){
|
||||||
|
var stickNormalizedX = deltaX / stickDistance;
|
||||||
|
var stickNormalizedY = deltaY / stickDistance;
|
||||||
|
this._stickX = stickNormalizedX * this._stickRadius + this._baseX;
|
||||||
|
this._stickY = stickNormalizedY * this._stickRadius + this._baseY;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
this._stickEl.style.display = "";
|
||||||
|
this._move(this._stickEl.style, (this._stickX - this._stickEl.width /2), (this._stickY - this._stickEl.height/2));
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._onMove = function(x, y)
|
||||||
|
{
|
||||||
|
if( this._pressed === true ){
|
||||||
|
this._stickX = x;
|
||||||
|
this._stickY = y;
|
||||||
|
if(this._limitStickTravel === true){
|
||||||
|
var deltaX = this.deltaX();
|
||||||
|
var deltaY = this.deltaY();
|
||||||
|
var stickDistance = Math.sqrt( (deltaX * deltaX) + (deltaY * deltaY) );
|
||||||
|
if(stickDistance > this._stickRadius){
|
||||||
|
var stickNormalizedX = deltaX / stickDistance;
|
||||||
|
var stickNormalizedY = deltaY / stickDistance;
|
||||||
|
this._stickX = stickNormalizedX * this._stickRadius + this._baseX;
|
||||||
|
this._stickY = stickNormalizedY * this._stickRadius + this._baseY;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
this._move(this._stickEl.style, (this._stickX - this._stickEl.width /2), (this._stickY - this._stickEl.height/2));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// bind touch events (and mouse events for debug) //
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
VirtualJoystick.prototype._onMouseUp = function(event)
|
||||||
|
{
|
||||||
|
return this._onUp();
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._onMouseDown = function(event)
|
||||||
|
{
|
||||||
|
event.preventDefault();
|
||||||
|
var x = event.clientX;
|
||||||
|
var y = event.clientY;
|
||||||
|
return this._onDown(x, y);
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._onMouseMove = function(event)
|
||||||
|
{
|
||||||
|
var x = event.clientX;
|
||||||
|
var y = event.clientY;
|
||||||
|
return this._onMove(x, y);
|
||||||
|
}
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// comment //
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
VirtualJoystick.prototype._onTouchStart = function(event)
|
||||||
|
{
|
||||||
|
// if there is already a touch inprogress do nothing
|
||||||
|
if( this._touchIdx !== null ) return;
|
||||||
|
// notify event for validation
|
||||||
|
var isValid = this.dispatchEvent('touchStartValidation', event);
|
||||||
|
if( isValid === false ) return;
|
||||||
|
// dispatch touchStart
|
||||||
|
this.dispatchEvent('touchStart', event);
|
||||||
|
event.preventDefault();
|
||||||
|
// get the first who changed
|
||||||
|
var touch = event.changedTouches[0];
|
||||||
|
// set the touchIdx of this joystick
|
||||||
|
this._touchIdx = touch.identifier;
|
||||||
|
// forward the action
|
||||||
|
var x = touch.pageX;
|
||||||
|
var y = touch.pageY;
|
||||||
|
return this._onDown(x, y)
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._onTouchEnd = function(event)
|
||||||
|
{
|
||||||
|
// if there is no touch in progress, do nothing
|
||||||
|
if( this._touchIdx === null ) return;
|
||||||
|
// dispatch touchEnd
|
||||||
|
this.dispatchEvent('touchEnd', event);
|
||||||
|
// try to find our touch event
|
||||||
|
var touchList = event.changedTouches;
|
||||||
|
for(var i = 0; i < touchList.length && touchList[i].identifier !== this._touchIdx; i++);
|
||||||
|
// if touch event isnt found,
|
||||||
|
if( i === touchList.length) return;
|
||||||
|
// reset touchIdx - mark it as no-touch-in-progress
|
||||||
|
this._touchIdx = null;
|
||||||
|
//??????
|
||||||
|
// no preventDefault to get click event on ios
|
||||||
|
event.preventDefault();
|
||||||
|
return this._onUp()
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._onTouchMove = function(event)
|
||||||
|
{
|
||||||
|
// if there is no touch in progress, do nothing
|
||||||
|
if( this._touchIdx === null ) return;
|
||||||
|
// try to find our touch event
|
||||||
|
var touchList = event.changedTouches;
|
||||||
|
for(var i = 0; i < touchList.length && touchList[i].identifier !== this._touchIdx; i++ );
|
||||||
|
// if touch event with the proper identifier isnt found, do nothing
|
||||||
|
if( i === touchList.length) return;
|
||||||
|
var touch = touchList[i];
|
||||||
|
event.preventDefault();
|
||||||
|
var x = touch.pageX;
|
||||||
|
var y = touch.pageY;
|
||||||
|
return this._onMove(x, y)
|
||||||
|
}
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// build default stickEl and baseEl //
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
/**
|
||||||
|
* build the canvas for joystick base
|
||||||
|
*/
|
||||||
|
VirtualJoystick.prototype._buildJoystickBase = function()
|
||||||
|
{
|
||||||
|
var canvas = document.createElement( 'canvas' );
|
||||||
|
if (screen.width >= screen.height){
|
||||||
|
canvas.width = screen.height*0.7;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
canvas.width = screen.width*0.7;
|
||||||
|
};
|
||||||
|
canvas.height = canvas.width;
|
||||||
|
var ctx = canvas.getContext('2d');
|
||||||
|
ctx.beginPath();
|
||||||
|
ctx.fillStyle = "#053a58";
|
||||||
|
ctx.arc(canvas.width/2, canvas.width/2,canvas.width/2,0,Math.PI*2,true);
|
||||||
|
ctx.closePath();
|
||||||
|
ctx.fill();
|
||||||
|
ctx.beginPath();
|
||||||
|
ctx.strokeStyle = this._strokeStyle;
|
||||||
|
ctx.lineWidth = 6;
|
||||||
|
ctx.arc( canvas.width/2, canvas.width/2, 40, 0, Math.PI*2, true);
|
||||||
|
ctx.stroke();
|
||||||
|
ctx.beginPath();
|
||||||
|
ctx.strokeStyle = "#69C";
|
||||||
|
ctx.lineWidth = 4;
|
||||||
|
ctx.arc( canvas.width/2, canvas.width/2, canvas.width/2-2, 0, Math.PI*2, true);
|
||||||
|
ctx.stroke();
|
||||||
|
ctx.beginPath();
|
||||||
|
ctx.lineWidth = 3;
|
||||||
|
ctx.moveTo(canvas.width/2,0);
|
||||||
|
ctx.lineTo(canvas.width/2,canvas.width/2);
|
||||||
|
ctx.stroke();
|
||||||
|
ctx.lineWidth = 1;
|
||||||
|
ctx.moveTo(canvas.width/2,0);
|
||||||
|
ctx.lineTo(canvas.width/2,canvas.width);
|
||||||
|
ctx.moveTo(0,canvas.width/2);
|
||||||
|
ctx.lineTo(canvas.width,canvas.width/2);
|
||||||
|
ctx.stroke();
|
||||||
|
return canvas;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* build the canvas for joystick stick
|
||||||
|
*/
|
||||||
|
VirtualJoystick.prototype._buildJoystickStick = function()
|
||||||
|
{
|
||||||
|
var canvas = document.createElement( 'canvas' );
|
||||||
|
canvas.width = 86;
|
||||||
|
canvas.height = 86;
|
||||||
|
var ctx = canvas.getContext('2d');
|
||||||
|
ctx.beginPath();
|
||||||
|
ctx.strokeStyle = this._strokeStyle;
|
||||||
|
ctx.lineWidth = 6;
|
||||||
|
ctx.arc( canvas.width/2, canvas.width/2, 40, 0, Math.PI*2, true);
|
||||||
|
ctx.stroke();
|
||||||
|
return canvas;
|
||||||
|
}
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// move using translate3d method with fallback to translate > 'top' and 'left'
|
||||||
|
// modified from https://github.com/component/translate and dependents
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////
|
||||||
|
VirtualJoystick.prototype._move = function(style, x, y)
|
||||||
|
{
|
||||||
|
if (this._transform) {
|
||||||
|
if (this._has3d) {
|
||||||
|
style[this._transform] = 'translate3d(' + x + 'px,' + y + 'px, 0)';
|
||||||
|
} else {
|
||||||
|
style[this._transform] = 'translate(' + x + 'px,' + y + 'px)';
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
style.left = x + 'px';
|
||||||
|
style.top = y + 'px';
|
||||||
|
}
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._getTransformProperty = function()
|
||||||
|
{
|
||||||
|
var styles = [
|
||||||
|
'webkitTransform',
|
||||||
|
'MozTransform',
|
||||||
|
'msTransform',
|
||||||
|
'OTransform',
|
||||||
|
'transform'
|
||||||
|
];
|
||||||
|
var el = document.createElement('p');
|
||||||
|
var style;
|
||||||
|
for (var i = 0; i < styles.length; i++) {
|
||||||
|
style = styles[i];
|
||||||
|
if (null != el.style[style]) {
|
||||||
|
return style;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
VirtualJoystick.prototype._check3D = function()
|
||||||
|
{
|
||||||
|
var prop = this._getTransformProperty();
|
||||||
|
// IE8<= doesn't have `getComputedStyle`
|
||||||
|
if (!prop || !window.getComputedStyle) return module.exports = false;
|
||||||
|
var map = {
|
||||||
|
webkitTransform: '-webkit-transform',
|
||||||
|
OTransform: '-o-transform',
|
||||||
|
msTransform: '-ms-transform',
|
||||||
|
MozTransform: '-moz-transform',
|
||||||
|
transform: 'transform'
|
||||||
|
};
|
||||||
|
// from: https://gist.github.com/lorenzopolidori/3794226
|
||||||
|
var el = document.createElement('div');
|
||||||
|
el.style[prop] = 'translate3d(1px,1px,1px)';
|
||||||
|
document.body.insertBefore(el, null);
|
||||||
|
var val = getComputedStyle(el).getPropertyValue(map[prop]);
|
||||||
|
document.body.removeChild(el);
|
||||||
|
var exports = null != val && val.length && 'none' != val;
|
||||||
|
return exports;
|
||||||
|
}
|
||||||
|
</script>
|
||||||
|
<script>
|
||||||
|
console.log("touchscreen is", VirtualJoystick.touchScreenAvailable() ? "available" : "not available");
|
||||||
|
var joystick = new VirtualJoystick({
|
||||||
|
container : document.getElementById('container'),
|
||||||
|
mouseSupport : true,
|
||||||
|
});
|
||||||
|
joystick.addEventListener('touchStart', function(){
|
||||||
|
console.log('down')
|
||||||
|
})
|
||||||
|
joystick.addEventListener('touchEnd', function(){
|
||||||
|
console.log('up')
|
||||||
|
})
|
||||||
|
var deltaXprevia = 0;
|
||||||
|
var deltaYprevia = 0;
|
||||||
|
var deltaX_corregida = 0;
|
||||||
|
var deltaY_corregida = 0;
|
||||||
|
setInterval(function(){
|
||||||
|
if (screen.width >= screen.height){
|
||||||
|
var stickRadius = screen.height*0.35;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
var stickRadius = screen.width*0.35;
|
||||||
|
};
|
||||||
|
var deltaX_corregida=(joystick.deltaX()/stickRadius*255).toFixed();
|
||||||
|
var deltaY_corregida=(-joystick.deltaY()/stickRadius*255).toFixed();
|
||||||
|
var stick = Math.sqrt((deltaX_corregida*deltaX_corregida)+(deltaY_corregida*deltaY_corregida)).toFixed();
|
||||||
|
if (deltaX_corregida >= 0 & deltaY_corregida >= 0){
|
||||||
|
var angulo = (Math.atan(deltaY_corregida/deltaX_corregida)/(2*Math.PI)*360).toFixed()
|
||||||
|
}
|
||||||
|
if (deltaX_corregida < 0) {
|
||||||
|
var angulo = ((Math.atan(deltaY_corregida/deltaX_corregida)/(2*Math.PI)*360)+180).toFixed()
|
||||||
|
}
|
||||||
|
if (deltaX_corregida >= 0 & deltaY_corregida < 0){
|
||||||
|
var angulo = ((Math.atan(deltaY_corregida/deltaX_corregida)/(2*Math.PI)*360)+360).toFixed()
|
||||||
|
}
|
||||||
|
var outputEl = document.getElementById('result');
|
||||||
|
outputEl.innerHTML = '<b> dx:</b>'+deltaX_corregida
|
||||||
|
+ '<b> dy:</b>'+deltaY_corregida
|
||||||
|
+ '<b> Vector (stick):</b>'+stick
|
||||||
|
+ '<br><b> Ángulo:</b>'+angulo
|
||||||
|
+ '<b> Orientación:</b>'
|
||||||
|
+ (joystick.right() ? ' Derecha' : '')
|
||||||
|
+ (joystick.up() ? ' Arriba' : '')
|
||||||
|
+ (joystick.left() ? ' Izquierda' : '')
|
||||||
|
+ (joystick.down() ? ' Abajo' : '')
|
||||||
|
if (deltaX_corregida != deltaXprevia || deltaY_corregida != deltaYprevia ){
|
||||||
|
var req = new XMLHttpRequest();
|
||||||
|
req.open('GET', '/stick?dx=' + deltaX_corregida + '&dy=' + deltaY_corregida);
|
||||||
|
req.send(null);
|
||||||
|
deltaXprevia = deltaX_corregida;
|
||||||
|
deltaYprevia = deltaY_corregida;
|
||||||
|
}
|
||||||
|
}, 1/30 * 1000);
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
184
src/main.cpp
Normal file
184
src/main.cpp
Normal 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;
|
||||||
|
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user