Inertial sensor calibrate

Molina · May 2, 2021, 4:21pm

How can I get my inertial sensor to calibrate several times during autonomous and it doesn’t take that long to calibrate, please help.

kevjar · May 2, 2021, 5:33pm

I’m not sure why you would want to calibrate the sensor more than once during the same autonomous run. Could you maybe explain more precisely what you’re trying to do?

Molina · May 2, 2021, 5:38pm

What happens is that the robot has to do two laps in the autonomous, the first one works well because I calibrate it in pre-auton, but the second round does not give it, it starts to spin like crazy, that’s why I ask what What can I do so that it does not pass and it is recalibrated in the second round.

kevjar · May 2, 2021, 5:43pm

If the robot starts spinning like crazy, that sounds like an error in the code. I’ve found the inertial sensor to be very accurate, even over a full 1 minute auton skills run. The sensor itself is definitely not the issue here. Maybe you could post the code you use for turning?

PS: What kind of program are you making? Is it a 15 second auton for competition or is this a coding project for a class?

Molina · May 2, 2021, 5:57pm

This is the code that I use my friend, and I calibrate it in the pre-auton part, and as I mentioned in the first round it works well without problems but in the second it no longer gives it, it skips that function and continues with the next one or it it starts to spin, and I think it is because it is not calibrated, and I use this program for a 15-second autonomous friend.

Void rightTurn(int degrees) {

While(inertial1.rotation(deg) < degrees) {
RightMotor.spin(directionType::rev, 20,
velocityUnits::pct)
LeftMotor.spin(directionType::fwd, 20,
velocityUnits::pct)
}
RightMotor.stop(bakeType::hold)
RightMotor.stop(brakeType::hold)
}

kevjar · May 2, 2021, 6:09pm

there’s nothing wrong that I can see in that function (except that you should add a 20 msec wait inside the while loop). I would need to see your whole main.cpp code to figure out what’s wrong. you can post the code with:

[code]
…code goes in here
[/code]

for proper formatting

DrewWHOOP · May 2, 2021, 6:13pm

the inertial will return a value within the bounds [0,360). This means that by just comparing what it returns, you will get the wrong answer after a full revolution.

kevjar · May 2, 2021, 6:17pm

Actually, Inertial.rotation() doesn’t wrap around, so it can go above 360 on a full spin. @Molina You should use Inertial.heading() instead of Inertial.rotation. Heading values are limited between 0-360 degrees.

Molina · May 2, 2021, 6:23pm

I leave you the link of the friend programming

Program

Molina · May 2, 2021, 6:32pm

So would it look like this friend?

Void rightTurn (grados int) {

While (inercial1.heading (grados) <grados) {
RightMotor.spin (directionType :: rev, 20,
velocityUnits :: pct)
LeftMotor.spin (directionType :: fwd, 20,
velocityUnits :: pct)
wait(20,msec);
}
RightMotor.stop (bakeType :: mantener)
RightMotor.stop (brakeType :: hold)
}

kevjar · May 2, 2021, 6:38pm

That link isn’t working for me. can you just copy/paste the code?

Molina · May 2, 2021, 6:40pm

[code]
/----------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: VEX /
/ Created: Thu Sep 26 2019 /
/ Description: Competition Template /
/ /
/----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Controller1 controller
// rightmotor1 motor 1
// rightmotor2 motor 2
// leftmotor1 motor 3
// leftmotor2 motor 4
// roller1 motor 5
// roller2 motor 6
// rodillo1 motor 7
// rodillo2 motor 8
// EncoderA encoder A, B
// Inertial9 inertial 9
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

// Una instancia global de competencia
competition Competition;
//creamos grupos de motores para que el programa sea mas compacto, para poder declarar solo un motor por cada dos motores.
motor_group rightmotor = motor_group(rightmotor1,rightmotor2);
motor_group leftmotor = motor_group(leftmotor1, leftmotor2);
motor_group roller = motor_group(roller1, roller2);
motor_group rodillo = motor_group(rodillo1, rodillo2);

void avanza(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(forward);
rightmotor.setVelocity(50, percent);
leftmotor.spin(forward);
leftmotor.setVelocity(50, percent);
/*mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
*/
while (!(EncoderA.position(degrees) > distancia)) {
Brain.Screen.print(“.%2f”, EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void izquierda1(int distancia){
//Declaramos a que velocidad van a ir los motores.
rightmotor1.setVelocity(50,percent);
rightmotor2.setVelocity(50,percent);
leftmotor1.setVelocity(50,percent);
leftmotor2.setVelocity(50,percent);
//Declaramos los motores con startRotateFor e insertaos los grados que queremos que gire el robot.
rightmotor1.startRotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);
rightmotor2.startRotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);
leftmotor1.startRotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);
leftmotor2.rotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);

//los motores se detienen cuando se cumplen los grados deseados.
rightmotor1.stop(brakeType::hold);
rightmotor2.stop(brakeType::hold);
leftmotor1.stop(brakeType::hold);
leftmotor2.stop(brakeType::hold);

}
void derecha1(int distancia){
//Declaramos a que velocidad van a ir los motores.
rightmotor1.setVelocity(50,percent);
rightmotor2.setVelocity(50,percent);
leftmotor1.setVelocity(50,percent);
leftmotor2.setVelocity(50,percent);
//Declaramos los motores con startRotateFor e insertaos los grados que queremos que gire el robot.
rightmotor1.startRotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);
rightmotor2.startRotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);
leftmotor1.startRotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);
leftmotor2.rotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);

//los motores se detienen cuando se cumplen los grados deseados.
rightmotor1.stop(brakeType::hold);
rightmotor2.stop(brakeType::hold);
leftmotor1.stop(brakeType::hold);
leftmotor2.stop(brakeType::hold);

}
void avanza2(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(forward);
rightmotor.setVelocity(80, percent);
leftmotor.spin(forward);
leftmotor.setVelocity(80, percent);
roller.spin(reverse);
roller.setVelocity(95, percent);
/*mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
*/
while (!(EncoderA.position(degrees) > distancia)) {
Brain.Screen.print(“.%2f”, EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
roller.stop();

}
void avanza3(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(forward);
rightmotor.setVelocity(100, percent);
leftmotor.spin(forward);
leftmotor.setVelocity(100, percent);
roller.spin(reverse);
roller.setVelocity(95, percent);
/*mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
*/
while (!(EncoderA.position(degrees) > distancia)) {
Brain.Screen.print(“.%2f”, EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
roller.stop();

}
void atras(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(reverse);
rightmotor.setVelocity(-80, percent);
leftmotor.spin(reverse);
leftmotor.setVelocity(-80, percent);
/*mientras la posicion del encoder sea menor a la distancia introducida, los motores
iran en reversa.
*/
while (!(EncoderA.position(degrees) < distancia)) {
Brain.Screen.print(“.%2f”, EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void lanzador(int grados){
//insertamos tipo de freno, velocidad y direccion.
roller.setStopping(coast);
rodillo.setStopping(coast);
roller.setVelocity(100,percent);
rodillo.setVelocity(100,percent);
roller.spin(reverse);
rodillo.spin(reverse);
//insertamos los grados que deseamos, que giren los motores.
roller.spinFor(grados,degrees);
//los motores se detienen cuando se cumplen los grados deseados.
roller.stop();
rodillo.stop();
wait(1,msec);
}
void lanzador2(int grados){
//insertamos tipo de freno, velocidad y direccion.
rodillo.setStopping(coast);
rodillo.setVelocity(100,percent);
rodillo.spin(reverse);
//insertamos los grados que deseamos, que giren los motores.
rodillo.spinFor(grados,degrees);
//los motores se detienen cuando se cumplen los grados deseados.
rodillo.stop();
wait(1,msec);
}
void roller3(int grados){
//insertamos tipo de freno, velocidad y direccion.
roller.setStopping(hold);
roller.setVelocity(80,percent);
roller.spin(reverse);
//insertamos los grados que deseamos, que giren los motores.
roller.spinFor(grados,degrees);
//los motores se detienen cuando se cumplen los grados deseados.
roller.stop();
wait(1,seconds);
}
void izquierda(int grados){
while(Inertial9.rotation(degrees) < grados){

rightmotor.spin(directionType::fwd,20, velocityUnits::pct);
leftmotor.spin(directionType::rev,20, velocityUnits::pct);

}
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void derecha(int grados){

while(Inertial9.rotation(degrees) < grados){

rightmotor.spin(directionType::rev,20, velocityUnits::pct);
leftmotor.spin(directionType::fwd,20, velocityUnits::pct);

}
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void derecha3(int grados){
wait(2000,msec);
Inertial9.calibrate();
// waits for the Inertial Sensor to calibrate
while (Inertial9.isCalibrating()) {
wait(25, msec);
}
while(Inertial9.rotation(degrees) < grados){

rightmotor.spin(directionType::rev,20, velocityUnits::pct);
leftmotor.spin(directionType::fwd,20, velocityUnits::pct);

}
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void pre_auton(void) {
// Inicializando la configuración del robot. ¡NO QUITAR!
vexcodeInit();
Brain.Screen.print(“Device initialization…”);
Brain.Screen.setCursor(2, 1);
// calibrate the drivetrain gyro
wait(2000, msec);
Inertial9.calibrate();
Brain.Screen.print(“Calibrating Gyro for Drivetrain”);
// wait for the gyro calibration process to finish
while (Inertial9.isCalibrating()) {
wait(25, msec);
}
// reset the screen now that the calibration is complete
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1,1);
wait(50, msec);
Brain.Screen.clearScreen();
}
void autonomous(void) {

roller3(800);
wait(1,msec);
avanza2(700);
wait(1,msec);
lanzador(1400);
wait(1,msec);
atras(-1500);
derecha(60);
avanza(500);
lanzador2(1600);
atras(-350);
derecha(60);
//derecha3(60);
avanza3(1550);
lanzador(1600);
atras(-200);

}
[\code]

kevjar · May 2, 2021, 6:42pm

That function will work better, but it will still have the issue where if the robot overshoots the target, it will have to go all the way around again to reach the target heading. Ideally, you would have one function that controls all turning (right and left turns). I would recommend a simple P-loop for this

kevjar · May 2, 2021, 6:43pm

*reposting with code formatting

/*----------------------------------------------------------------------------*/
/* <em>/
/</em> Module: main.cpp <em>/
/</em> Author: VEX <em>/
/</em> Created: Thu Sep 26 2019 <em>/
/</em> Description: Competition Template <em>/
/</em> <em>/
/</em>----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Controller1 controller
// rightmotor1 motor 1
// rightmotor2 motor 2
// leftmotor1 motor 3
// leftmotor2 motor 4
// roller1 motor 5
// roller2 motor 6
// rodillo1 motor 7
// rodillo2 motor 8
// EncoderA encoder A, B
// Inertial9 inertial 9
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

// Una instancia global de competencia
competition Competition;
//creamos grupos de motores para que el programa sea mas compacto, para poder declarar solo un motor por cada dos motores.
motor_group rightmotor = motor_group(rightmotor1,rightmotor2);
motor_group leftmotor = motor_group(leftmotor1, leftmotor2);
motor_group roller = motor_group(roller1, roller2);
motor_group rodillo = motor_group(rodillo1, rodillo2);

void avanza(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(forward);
rightmotor.setVelocity(50, percent);
leftmotor.spin(forward);
leftmotor.setVelocity(50, percent);
/*mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
*/
while (!(EncoderA.position(degrees) > distancia)) {
Brain.Screen.print(".%2f", EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void izquierda1(int distancia){
//Declaramos a que velocidad van a ir los motores.
rightmotor1.setVelocity(50,percent);
rightmotor2.setVelocity(50,percent);
leftmotor1.setVelocity(50,percent);
leftmotor2.setVelocity(50,percent);
//Declaramos los motores con startRotateFor e insertaos los grados que queremos que gire el robot.
rightmotor1.startRotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);
rightmotor2.startRotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);
leftmotor1.startRotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);
leftmotor2.rotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);

//los motores se detienen cuando se cumplen los grados deseados.
rightmotor1.stop(brakeType::hold);
rightmotor2.stop(brakeType::hold);
leftmotor1.stop(brakeType::hold);
leftmotor2.stop(brakeType::hold);

}
void derecha1(int distancia){
//Declaramos a que velocidad van a ir los motores.
rightmotor1.setVelocity(50,percent);
rightmotor2.setVelocity(50,percent);
leftmotor1.setVelocity(50,percent);
leftmotor2.setVelocity(50,percent);
//Declaramos los motores con startRotateFor e insertaos los grados que queremos que gire el robot.
rightmotor1.startRotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);
rightmotor2.startRotateFor(vex::directionType::rev , distancia, vex::rotationUnits::deg);
leftmotor1.startRotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);
leftmotor2.rotateFor(vex::directionType::fwd , distancia, vex::rotationUnits::deg);

//los motores se detienen cuando se cumplen los grados deseados.
rightmotor1.stop(brakeType::hold);
rightmotor2.stop(brakeType::hold);
leftmotor1.stop(brakeType::hold);
leftmotor2.stop(brakeType::hold);

}
void avanza2(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(forward);
rightmotor.setVelocity(80, percent);
leftmotor.spin(forward);
leftmotor.setVelocity(80, percent);
roller.spin(reverse);
roller.setVelocity(95, percent);
/*mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
*/
while (!(EncoderA.position(degrees) > distancia)) {
Brain.Screen.print(".%2f", EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
roller.stop();

}
void avanza3(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(forward);
rightmotor.setVelocity(100, percent);
leftmotor.spin(forward);
leftmotor.setVelocity(100, percent);
roller.spin(reverse);
roller.setVelocity(95, percent);
/*mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
*/
while (!(EncoderA.position(degrees) > distancia)) {
Brain.Screen.print(".%2f", EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
roller.stop();

}
void atras(int distancia){
//insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition(0, degrees);
rightmotor.spin(reverse);
rightmotor.setVelocity(-80, percent);
leftmotor.spin(reverse);
leftmotor.setVelocity(-80, percent);
/*mientras la posicion del encoder sea menor a la distancia introducida, los motores
iran en reversa.
*/
while (!(EncoderA.position(degrees) < distancia)) {
Brain.Screen.print(".%2f", EncoderA.position(degrees));
Brain.Screen.setCursor(1, 1);//brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
wait(100, msec);
Brain.Screen.clearScreen();//la pantalla se limpia cuando se cumple la condicion.
wait(5, msec);
}
// los motores se detienen cuando se cumple la condicion.
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void lanzador(int grados){
//insertamos tipo de freno, velocidad y direccion.
roller.setStopping(coast);
rodillo.setStopping(coast);
roller.setVelocity(100,percent);
rodillo.setVelocity(100,percent);
roller.spin(reverse);
rodillo.spin(reverse);
//insertamos los grados que deseamos, que giren los motores.
roller.spinFor(grados,degrees);
//los motores se detienen cuando se cumplen los grados deseados.
roller.stop();
rodillo.stop();
wait(1,msec);
}
void lanzador2(int grados){
//insertamos tipo de freno, velocidad y direccion.
rodillo.setStopping(coast);
rodillo.setVelocity(100,percent);
rodillo.spin(reverse);
//insertamos los grados que deseamos, que giren los motores.
rodillo.spinFor(grados,degrees);
//los motores se detienen cuando se cumplen los grados deseados.
rodillo.stop();
wait(1,msec);
}
void roller3(int grados){
//insertamos tipo de freno, velocidad y direccion.
roller.setStopping(hold);
roller.setVelocity(80,percent);
roller.spin(reverse);
//insertamos los grados que deseamos, que giren los motores.
roller.spinFor(grados,degrees);
//los motores se detienen cuando se cumplen los grados deseados.
roller.stop();
wait(1,seconds);
}
void izquierda(int grados){
while(Inertial9.rotation(degrees) < grados){

rightmotor.spin(directionType::fwd,20, velocityUnits::pct);
leftmotor.spin(directionType::rev,20, velocityUnits::pct);

}
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void derecha(int grados){

while(Inertial9.rotation(degrees) < grados){

rightmotor.spin(directionType::rev,20, velocityUnits::pct);
leftmotor.spin(directionType::fwd,20, velocityUnits::pct);

}
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void derecha3(int grados){
wait(2000,msec);
Inertial9.calibrate();
// waits for the Inertial Sensor to calibrate
while (Inertial9.isCalibrating()) {
wait(25, msec);
}
while(Inertial9.rotation(degrees) < grados){

rightmotor.spin(directionType::rev,20, velocityUnits::pct);
leftmotor.spin(directionType::fwd,20, velocityUnits::pct);

}
rightmotor.stop(brakeType::hold);
leftmotor.stop(brakeType::hold);
}
void pre_auton(void) {
// Inicializando la configuración del robot. ¡NO QUITAR!
vexcodeInit();
Brain.Screen.print(“Device initialization…”);
Brain.Screen.setCursor(2, 1);
// calibrate the drivetrain gyro
wait(2000, msec);
Inertial9.calibrate();
Brain.Screen.print(“Calibrating Gyro for Drivetrain”);
// wait for the gyro calibration process to finish
while (Inertial9.isCalibrating()) {
wait(25, msec);
}
// reset the screen now that the calibration is complete
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1,1);
wait(50, msec);
Brain.Screen.clearScreen();
}
void autonomous(void) {

roller3(800);
wait(1,msec);
avanza2(700);
wait(1,msec);
lanzador(1400);
wait(1,msec);
atras(-1500);
derecha(60);
avanza(500);
lanzador2(1600);
atras(-350);
derecha(60);
//derecha3(60);
avanza3(1550);
lanzador(1600);
atras(-200);

}

Molina · May 2, 2021, 6:50pm

I would like to know how to do it friend, the truth is that I have not been in this long and I do not understand the simple p-loop very well

Molina · May 2, 2021, 6:55pm


/ *---------------------------------------------- ------------------------------* /
/ * <em>/
/</em> Módulo: main.cpp <em>/
/</em> Autor: VEX <em>/
/</em> Creado: Jue. 26 de septiembre de 2019 <em>/
/</em> Descripción: Plantilla de competición <em>/
/</em> <em>/
/</em> ------------------------------------- --------------------------------------- * /

// ---- INICIAR DISPOSITIVOS CONFIGURADOS CON VEXCODE ----
// Configuración del robot:
// [Nombre] [Tipo] [Puerto (s)]
// Controlador Controlador1
// Motor derecho1 motor 1
// Motor derecho2 motor 2
// Motor izquierdo1 motor 3
// motor izquierdo2 motor 4
// rodillo1 motor 5
// rodillo2 motor 6
// rodillo1 motor 7
// rodillo2 motor 8
// EncoderA encoder A, B
// Inercial9 inercial 9
// ---- FIN VEXCODE CONFIGURED DEVICES - ---

#include "vex.h"

usando el espacio de nombres vex;

//
Concurso Una instancia global de competencia competition.
// creamos grupos de motores para que el programa sea mas compacto, para poder declarar solo un motor por cada dos motores.
motor_group rightmotor = motor_group (rightmotor1, rightmotor2);
motor_group leftmotor = motor_group (leftmotor1, leftmotor2);
grupo_motor rodillo = grupo_motor (rodillo1, rodillo2);
grupo_motor rodillo = grupo_motor (rodillo1, rodillo2);

void avanza (int distancia) {
// insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition (0, grados);
rightmotor.spin (adelante);
rightmotor.setVelocity (50, por ciento);
leftmotor.spin (adelante);
leftmotor.setVelocity (50, por ciento);
/ * mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
* /
while (! (EncoderA.position (grados)> distancia)) {
Brain.Screen.print (".% 2f", EncoderA.position (grados));
Brain.Screen.setCursor (1, 1); // brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
esperar (100, ms);
Brain.Screen.clearScreen (); // la pantalla se limpia cuando se cumple la condicion.
esperar (5, ms);
}
// los motores se administran cuando se cumple la condición.
rightmotor.stop (brakeType :: hold);
leftmotor.stop (brakeType :: hold);
}
void izquierda1 (int distancia) {
// Declaramos a que velocidad van a ir los motores.
rightmotor1.setVelocity (50, por ciento);
rightmotor2.setVelocity (50, por ciento);
leftmotor1.setVelocity (50, por ciento);
leftmotor2.setVelocity (50, por ciento);
// Declaramos los motores con startRotateFor e insertaos los grados que queremos que gire el robot.
rightmotor1.startRotateFor (vex :: directionType :: fwd, distancia, vex :: rotacionUnits :: deg);
rightmotor2.startRotateFor (vex :: directionType :: fwd, distancia, vex :: rotacionUnits :: deg);
leftmotor1.startRotateFor (vex :: directionType :: rev, distancia, vex :: rotacionUnits :: deg);
leftmotor2.rotateFor (vex :: directionType :: rev, distancia, vex :: rotacionUnits :: deg);

// los motores se administran cuando se cumplen los grados deseados.
rightmotor1.stop (brakeType :: hold);
rightmotor2.stop (brakeType :: hold);
leftmotor1.stop (brakeType :: hold);
leftmotor2.stop (brakeType :: hold);

}
void derecha1 (int distancia) {
// Declaramos a que velocidad van a ir los motores.
rightmotor1.setVelocity (50, por ciento);
rightmotor2.setVelocity (50, por ciento);
leftmotor1.setVelocity (50, por ciento);
leftmotor2.setVelocity (50, por ciento);
// Declaramos los motores con startRotateFor e insertaos los grados que queremos que gire el robot.
rightmotor1.startRotateFor (vex :: directionType :: rev, distancia, vex :: rotacionUnits :: deg);
rightmotor2.startRotateFor (vex :: directionType :: rev, distancia, vex :: rotacionUnits :: deg);
leftmotor1.startRotateFor (vex :: directionType :: fwd, distancia, vex :: rotacionUnits :: deg);
leftmotor2.rotateFor (vex :: directionType :: fwd, distancia, vex :: rotacionUnits :: deg);

// los motores se administran cuando se cumplen los grados deseados.
rightmotor1.stop (brakeType :: hold);
rightmotor2.stop (brakeType :: hold);
leftmotor1.stop (brakeType :: hold);
leftmotor2.stop (brakeType :: hold);

}
void avanza2 (int distancia) {
// insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition (0, grados);
rightmotor.spin (adelante);
rightmotor.setVelocity (80, por ciento);
leftmotor.spin (adelante);
leftmotor.setVelocity (80, por ciento);
roller.spin (marcha atrás);
roller.setVelocity (95, por ciento);
/ * mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
* /
while (! (EncoderA.position (grados)> distancia)) {
Brain.Screen.print (".% 2f", EncoderA.position (grados));
Brain.Screen.setCursor (1, 1); // brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
esperar (100, ms);
Brain.Screen.clearScreen (); // la pantalla se limpia cuando se cumple la condicion.
esperar (5, ms);
}
// los motores se administran cuando se cumple la condición.
rightmotor.stop (brakeType :: hold);
leftmotor.stop (brakeType :: hold);
roller.stop ();

}
void avanza3 (int distancia) {
// insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition (0, grados);
rightmotor.spin (adelante);
rightmotor.setVelocity (100, por ciento);
leftmotor.spin (adelante);
leftmotor.setVelocity (100, por ciento);
roller.spin (marcha atrás);
roller.setVelocity (95, por ciento);
/ * mientras la posicion del encoder sea mayor a la distancia introducida, los motores
iran hacia adelante.
* /
while (! (EncoderA.position (grados)> distancia)) {
Brain.Screen.print (".% 2f", EncoderA.position (grados));
Brain.Screen.setCursor (1, 1); // brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
esperar (100, ms);
Brain.Screen.clearScreen (); // la pantalla se limpia cuando se cumple la condicion.
esperar (5, ms);
}
// los motores se administran cuando se cumple la condición.
rightmotor.stop (brakeType :: hold);
leftmotor.stop (brakeType :: hold);
roller.stop ();

}
void atras (int distancia) {
// insertamos la posicion, velocidad y direccion que deseamos que tengan los motores.
EncoderA.setPosition (0, grados);
rightmotor.spin (marcha atrás);
rightmotor.setVelocity (-80, porcentaje);
leftmotor.spin (marcha atrás);
leftmotor.setVelocity (-80, porcentaje);
/ * mientras la posicion del encoder sea menor a la distancia introducida, los motores
iran en reversa.
* /
while (! (EncoderA.position (grados) <distancia)) {
Brain.Screen.print (".% 2f", EncoderA.position (grados));
Brain.Screen.setCursor (1, 1); // brain screen sirve para imprimir los grados que avanza el enconder, en la pantalla del brain.
esperar (100, ms);
Brain.Screen.clearScreen (); // la pantalla se limpia cuando se cumple la condicion.
esperar (5, ms);
}
// los motores se administran cuando se cumple la condición.
rightmotor.stop (brakeType :: hold);
leftmotor.stop (brakeType :: hold);
}
void lanzador (int grados) {
// insertamos tipo de freno, velocidad y direccion.
roller.setStopping (costa);
rodillo.setStopping (costa);
roller.setVelocity (100, por ciento);
rodillo.setVelocity (100, por ciento);
roller.spin (marcha atrás);
rodillo.spin (reverso);
// insertamos los grados que deseamos, que giren los motores.
roller.spinFor (grados, grados);
// los motores se administran cuando se cumplen los grados deseados.
roller.stop ();
rodillo.stop ();
esperar (1, ms);
}
void lanzador2 (int grados) {
// insertamos tipo de freno, velocidad y direccion.
rodillo.setStopping (costa);
rodillo.setVelocity (100, por ciento);
rodillo.spin (reverso);
// insertamos los grados que deseamos, que giren los motores.
rodillo.spinFor (grados, grados);
// los motores se administran cuando se cumplen los grados deseados.
rodillo.stop ();
esperar (1, ms);
}
void roller3 (int grados) {
// insertamos tipo de freno, velocidad y direccion.
roller.setStopping (mantener);
roller.setVelocity (80, por ciento);
roller.spin (marcha atrás);
// insertamos los grados que deseamos, que giren los motores.
roller.spinFor (grados, grados);
// los motores se administran cuando se cumplen los grados deseados.
roller.stop ();
esperar (1, segundos);
}
void izquierda (int grados) {
while (Inertial9.rotation (grados) <grados) {

rightmotor.spin (directionType :: fwd, 20, velocityUnits :: pct);
leftmotor.spin (directionType :: rev, 20, velocityUnits :: pct);

}
rightmotor.stop (brakeType :: hold);
leftmotor.stop (brakeType :: hold);
}
void derecha (int grados) {

while (Inertial9.rotation (grados) <grados) {

rightmotor.spin (directionType :: rev, 20, velocityUnits :: pct);
leftmotor.spin (directionType :: fwd, 20, velocityUnits :: pct);

}
rightmotor.stop (brakeType :: hold);
leftmotor.stop (brakeType :: hold);
}
void derecha3 (int grados) {
espera (2000, mseg);
Inercial9.calibrate ();
// espera a que el sensor de inercia se calibre
while (Inertial9.isCalibrating ()) {
espera (25, mseg);
}
while (Inertial9.rotation (grados) <grados) {

rightmotor.spin (directionType :: rev, 20, velocityUnits :: pct);
leftmotor.spin (directionType :: fwd, 20, velocityUnits :: pct);

}
rightmotor.stop (brakeType :: hold);
leftmotor.stop (brakeType :: hold);
}
void pre_auton (void) {
// Inicializando la configuración del robot. ¡NO QUITAR!
vexcodeInit ();
Brain.Screen.print ("Inicialización del dispositivo ...");
Brain.Screen.setCursor (2, 1);
// calibrar la
espera del giróscopo del tren motriz (2000, mseg);
Inercial9.calibrate ();
Brain.Screen.print ("Calibración del giróscopo para el tren motriz");
// espera a que termine el proceso de calibración del giróscopo
while (Inertial9.isCalibrating ()) {
espera (25, mseg);
}
// reinicia la pantalla ahora que la calibración está completa
Brain.Screen.clearScreen ();
Brain.Screen.setCursor (1,1);
esperar (50, ms);
Brain.Screen.clearScreen ();
}
vacío autónomo (vacío) {

roller3 (800);
esperar (1, ms);
avanza2 (700);
esperar (1, ms);
lanzador (1400);
esperar (1, ms);
atras (-1500);
derecha (60);
avanza (500);
lanzador2 (1600);
atras (-350);
derecha (60);
// derecha3 (60);
avanza3 (1550);
lanzador (1600);
atras (-200);

}

kevjar · May 2, 2021, 7:05pm

A P-loop (or Proportional Loop) uses the difference between your current position and the target position to determine what power to give to the motors. Since this is a turning function, we’ll call the difference between our current heading and the target turnError

your function would look something like this:

void turnFunction(double targetValue) {

  double turnError = targetValue - Inertial.heading();

  //run the loop while the error is bigger than 1 degree
  while(turnError > 1) {

    //calculate the difference between the current heading and the target heading
    turnError = targetValue - Inertial.heading();

    leftDrive.spin(fwd, turnError * kP, volts);
    rightDrive.spin(rev, turnError * kP, volts);

    task::sleep(20);
  }
}

With a P-loop, the error is recalculated every loop. the motors are given more power when the error is large (far from the target) and less power when closer to thet target. By using an Error value rather than comparing values with “<” or “>”, this also allows the robot to turn both ways, since a negative error will make the motors spin in the opposite direction.

The kP value is a constant that you will have to fien tune through testing to get the correct speed.

Molina · May 2, 2021, 7:26pm

Ok friend, this function will help the robot to decide based on its position, wow, I had never understood that friend, thank you very much, but I have a question about how I would define the right or the left, it would be through the positive and the negative, that is, the negative is left and the right is positive, is that correct?

Molina · May 2, 2021, 7:32pm

And finally I can put a number that will be constant in the kp part and that will remain there depending on the voltage I want my motors to have, right, and I will do that depending on my tests that I do, right?

kevjar · May 2, 2021, 7:33pm

Just so I’m absolutely clear, the robot’s position on the field does not affect the turning. Whenever I say 'position" in my explanation, I mean the robot’s heading (rotational position).

As for how the left and right are defined, the error will be positive when the target is to the right and negative when the target requires a left turn. For example, if the robot is facing toward 90 degrees and the target is 45 degrees, the turnError would be 45 - 90 = -45. Since this value is negative, the power sent to the motors will be negative. the left side will go in reverse, and the right side wil spin forward, resulting in a left turn.