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

control on off

Browse Source
This commit is contained in:
Martin Daniel Chaparro Ruiz
2020-08-29 18:41:28 -05:00
parent ecb494c4b1
commit 82ebb95678
1 changed files with 147 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

224
src/main.cpp
View File

@@ -1,24 +1,45 @@
#include <Arduino.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
TaskHandle_t TareaProcesador1;
void loop1(void *pvParameters);
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;
volatile int dir = 0;
bool FWD = HIGH;
bool BWD = LOW;
//TaskHandle_t TareaProcesador1;
//void loop1(void *pvParameters);
/*
=========================================
definiciones de los motores paso a paso
=========================================
*/
#define STEP_1 13
#define DIR_1 25
#define STEP_2 33
#define DIR_2 14
MPU6050 mpu6050(Wire);
#define max_speed 100
void IRAM_ATTR onTimer(); //declaración callback interrupciones tick Motores
void motores_calc();
void control_on_off(float angulo);
hw_timer_t *timer_motores = NULL; //timer para controlar tick de los motores paso a paso
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
volatile int tick_motores;
float ax, ay, az;
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;
volatile float setPoint = 0.0;
volatile int dir = 0;
bool FWD = HIGH;
bool BWD = LOW;
//==========================================
/*
============================================
Definiciones MPU6050
============================================
*/
MPU6050 mpu6050(Wire);
float ay;
long timer = 0;
//==========================================
void setup()
{
//Configuracion motores
pinMode(23, OUTPUT);
pinMode(STEP_1, OUTPUT);
pinMode(STEP_2, OUTPUT);
@@ -26,92 +47,147 @@ void setup()
pinMode(DIR_2, OUTPUT);
digitalWrite(DIR_1, LOW);
digitalWrite(DIR_2, LOW);
timer_motores = timerBegin(0, 80, true);
timerAttachInterrupt(timer_motores, &onTimer, true);
timerAlarmWrite(timer_motores, 50, true);
//I2C
Wire.begin();
Serial.begin(115200);
Wire.begin();
xTaskCreatePinnedToCore(loop1, "Tarea1", 10000, NULL, 1, &TareaProcesador1, 0);
// xTaskCreatePinnedToCore(loop1, "Tarea1", 10000, NULL, 1, &TareaProcesador1, 0);
delay(2000); // Pause for 2 seconds
delay(1000); // Pause for 1 seconds
//Configuracion MPU6050
mpu6050.begin();
mpu6050.calcGyroOffsets(false);
delay(2000);
//Calculamos el setPoint inicial
Serial.print("\n[");
for (int i = 0; i < 100; i++)
{
mpu6050.update();
setPoint += mpu6050.getAngleY();
//Serial.print(setPoint),Serial.print(",");
delay(100);
}
Serial.print("]");
setPoint = setPoint / 100;
Serial.print("\nSetpoint:"), Serial.println(setPoint);
Serial.print("Core: "), Serial.println(xPortGetCoreID());
timerAlarmEnable(timer_motores);
}
void loop()
{
mpu6050.update();
if (millis() - timer > 10)
if (millis() - timer > 4)
{
// ax = mpu6050.getAngleX();
mpu6050.update();
ay = mpu6050.getAngleY();
//az = mpu6050.getAngleZ();
/* Serial.print(ax), Serial.print(",");
Serial.print(ay), Serial.print(",");
Serial.println(az);*/
if (ay > 4)
if (ay > 30 || ay < -30)
{
dir = -1;
}
else if (ay < -4)
{
dir = 1;
throttle_left_motor = 0;
throttle_right_motor = 0;
}
else
{
dir = 0;
control_on_off(ay);
}
throttle_left_motor = 6 * dir;
throttle_right_motor = 6 * dir;
//Serial.println(ay);*/
timer = millis();
}
}
void loop1(void *pvParameters)
{
for (;;)
if (tick_motores > 0)
{
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_1, 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_1, FWD); //Set output 3 high for a forward direction of the stepper motor
}
else if (throttle_counter_left_motor == 1)
digitalWrite(STEP_1, HIGH); //Set output 2 high to create a pulse for the stepper controller
else if (throttle_counter_left_motor == 2)
digitalWrite(STEP_1, 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_2, 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_2, FWD);
; //Set output 5 high for a forward direction of the stepper motor
}
else if (throttle_counter_right_motor == 1)
digitalWrite(STEP_2, HIGH); //Set output 4 high to create a pulse for the stepper controller
else if (throttle_counter_right_motor == 2)
digitalWrite(STEP_2, LOW);
delay(1);
motores_calc();
}
}
/*
=============================
Funciones Motores paso a paso
==============================
*/
//Interrupcion Tick Motores paso a paso
void IRAM_ATTR onTimer()
{
portENTER_CRITICAL_ISR(&timerMux);
tick_motores++;
portEXIT_CRITICAL_ISR(&timerMux);
}
void motores_calc()
{
portENTER_CRITICAL(&timerMux);
tick_motores--;
portEXIT_CRITICAL(&timerMux);
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_1, 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_1, FWD); //Set output 3 high for a forward direction of the stepper motor
}
else if (throttle_counter_left_motor == 1)
digitalWrite(STEP_1, HIGH); //Set output 2 high to create a pulse for the stepper controller
else if (throttle_counter_left_motor == 2)
digitalWrite(STEP_1, 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_2, 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_2, FWD);
; //Set output 5 high for a forward direction of the stepper motor
}
else if (throttle_counter_right_motor == 1)
digitalWrite(STEP_2, HIGH); //Set output 4 high to create a pulse for the stepper controller
else if (throttle_counter_right_motor == 2)
digitalWrite(STEP_2, LOW);
}
void control_on_off(float angulo)
{
float error = angulo - setPoint;
if (error > 4)
{
dir = -1;
}
else if (error < -4)
{
dir = 1;
}
else
{
dir = 0;
}
throttle_left_motor = max_speed * dir;
throttle_right_motor = max_speed * dir;
}
/*void loop1(void *pvParameters){
for(;;){
Serial.print("Core: "),Serial.println(xPortGetCoreID());
}
}*/