Controlador Joystick & Arreglos

rtos/Proyecto_Plotter/Proyecto_Plotter.ino

@@ -1,14 +1,15 @@
 #include "arm_controller.h"
 #include "web_server.h"
+#include "joystick_controller.h"
 #include "scheduler.h"
 
 void setup() 
 {
   Serial.begin(115200);
   ARM_init();
   //WEB_SERVER_init();
-  //SCHEDULER_init();
-
+  SCHEDULER_init();
+  JOYSTICK_init();
   Serial.println("Setup Complete");
 }
 
@@ -18,6 +19,5 @@ void loop()
   //SERVO_test(); //Funcion Bloqueante para testear el funcionamiento de los tres servos
   //SERVO_calculate_range();
 
-  //SCHEDULER_dispatch_tasks();
-
+  SCHEDULER_dispatch_tasks();
 }

rtos/Proyecto_Plotter/arm_controller.cpp

@@ -5,17 +5,41 @@ static int current_pos_y = 0;
 static int target_pos_x = 0;
 static int target_pos_y = 0;
 
+static bool initialized_flag = false;
 static bool currently_moving = false;
 
+static bool check_valid_coords(int coord_x, int coord_y); //Chequea que las coordenadas se encuentren en el rango posible de graficar
+
 void ARM_init()
 {
   SERVO_init();
-  SERVO_moveto(STARTING_X, STARTING_Y);
+  if (check_valid_coords(STARTING_X, STARTING_Y))
+  {
+    SERVO_moveto(STARTING_X + MIN_X, STARTING_Y + MIN_Y);
+    current_pos_x = STARTING_X;
+    current_pos_y = STARTING_Y;
+    target_pos_x = STARTING_X;
+    target_pos_y = STARTING_Y;
+  }
+  else
+  {
+    SERVO_moveto(MIN_X, MIN_Y);
+    current_pos_x = MIN_X;
+    current_pos_y = MIN_Y;
+    target_pos_x = MIN_X;
+    target_pos_y = MIN_Y;
+  }
+  delay(1000); //Asegura que el brazo este en la posicion inicial antes de continuar
+  initialized_flag = true; 
+}
 
-  current_pos_x = STARTING_X;
-  current_pos_y = STARTING_Y;
-  target_pos_x = STARTING_X;
-  target_pos_y = STARTING_Y;
+//Chequea que las coordenadas se encuentren en el rango posible de graficar
+static bool check_valid_coords(int coord_x, int coord_y)
+{
+  if ((coord_x >= 0 && coord_x <= RANGE_X) && (coord_y >= 0) && (coord_y <= RANGE_Y))
+    return true;
+  else 
+    return false;
 }
 
 // Mueve el brazo en linea recta hacia las coordenadas recibidas (Devuelve 'false' si aun no puede procesar la instruccion)
@@ -24,10 +48,12 @@ bool ARM_line_to(int target_x, int target_y)
   //Solo acepta la siguiente linea si no se esta moviendo
   if (!currently_moving)
   {
-    target_pos_x = target_x;
-    target_pos_y = target_y;
-
-    currently_moving = true;
+    if (check_valid_coords(target_x, target_y))
+    {
+      target_pos_x = target_x;
+      target_pos_y = target_y;
+      currently_moving = true;
+    }
     return true;
   }
   else
@@ -40,43 +66,51 @@ void ARM_shift_by(int shift_x, int shift_y)
 {
   if (!currently_moving)
   {
-    target_pos_x = current_pos_x + shift_x;
-    target_pos_y = current_pos_y + shift_y;
+    if (check_valid_coords(current_pos_x + shift_x, current_pos_y + shift_y))
+    {
+      target_pos_x = current_pos_x + shift_x;
+      target_pos_y = current_pos_y + shift_y;
+
+      currently_moving = true;
+    }
 
-    currently_moving = true;
   } 
 }
 
 //Actualiza la posicion del brazo si su posicion actual no es la deseada, y espera a que se muevan los servos.
 void ARM_update()
 {
   static int wait_timer = 0;
-  int shift_x = 0;
-  int shift_y = 0;
+  //int shift_x = 0;
+  //int shift_y = 0;
 
-  if (wait_timer == 0)
+  if (initialized_flag)
   {
-    if ((current_pos_x != target_pos_x) || (current_pos_y != target_pos_y))
+    if (wait_timer <= 0)
     {
-      //De momento se mueve directamente al destino final
-      //Seria posible dividir el camino en multiples lineas intermedias para mejorar la presicion del dibujo
-      SERVO_moveto(target_pos_x, target_pos_y);
-      current_pos_x = target_pos_x;
-      current_pos_y = target_pos_y;
+      if ((current_pos_x != target_pos_x) || (current_pos_y != target_pos_y))
+      {
+        //De momento se mueve directamente al destino final
+        //Seria posible dividir el camino en multiples lineas intermedias para mejorar la presicion del dibujo
+        SERVO_moveto(target_pos_x + MIN_X, target_pos_y + MIN_Y);
+        current_pos_x = target_pos_x;
+        current_pos_y = target_pos_y;
 
-      //Setea el tiempo de espera hasta que se terminen de mover los servos
-      //La cantidad de ciclos que espera podria ser proporcional a la distancia que debe moverse
-      wait_timer = WAIT_CYCLES;     
+        //Setea el tiempo de espera hasta que se terminen de mover los servos
+        //La cantidad de ciclos que espera podria ser proporcional a la distancia que debe moverse
+        currently_moving = true;
+        wait_timer = WAIT_CYCLES;     
+      }
+      else
+      {
+        //Deja de moverse cuando llega a su destino
+        currently_moving = false;
+      }
     }
     else
     {
-      //Deja de moverse cuando llega a su destino
-      currently_moving = false;
+      //Si esta esperando, decrementa el tiempo de espera
+      wait_timer--; 
     }
   }
-  else
-  {
-    //Si esta esperando, decrementa el tiempo de espera
-    wait_timer--; 
-  }
 }

rtos/Proyecto_Plotter/arm_controller.h

@@ -5,12 +5,22 @@
 
 #include "servo_controller.h"
 
+//Valores maximos y minimos de coordenadas que puede alcanzar el lapiz
+//Usados para abstraer los limites del area de dibujo
+#define MIN_X 450
+#define MAX_X 1350
+#define MIN_Y -750
+#define MAX_Y 750
+#define RANGE_Y (MAX_Y-MIN_Y)
+#define RANGE_X (MAX_X-MIN_X)
+
 // Coordenadas iniciales del brazo
-#define STARTING_X 90
-#define STARTING_Y 90
+#define STARTING_X RANGE_X-MIN_X
+#define STARTING_Y RANGE_Y
+
 
 // Ciclos del scheduler que espera el brazo a que se terminen de mover los servos
-#define WAIT_CYCLES 15
+#define WAIT_CYCLES 2
 
 void ARM_init();
 void ARM_update(); //Actualiza la posicion del brazo si su posicion actual no es la deseada, y espera a que se muevan los servos.

rtos/Proyecto_Plotter/joystick_controller.cpp

@@ -0,0 +1,74 @@
+
+#include "joystick_controller.h"
+
+//Lecturas de X,Y cuando el joystick esta inactivo, y rango de valores ignorados
+#define X_STANDBY 800
+#define Y_STANDBY 3200
+#define DEADZONE 300
+
+static bool initialized_flag = false;
+
+static int x_reading = X_STANDBY;
+static int y_reading = Y_STANDBY;
+static int stick_button_state = 0;
+
+static bool shift = false;
+static int shift_x = 0;
+static int shift_y = 0;
+
+void JOYSTICK_init()
+{
+  pinMode(SWPin, INPUT_PULLUP);
+  initialized_flag = true;
+}
+
+void JOYSTICK_update()
+{
+  if (initialized_flag)
+    {
+      shift = false;
+      shift_x = 0;
+      shift_y = 0;
+
+      // Lee los valores del joystick
+      x_reading = analogRead(VRxPin);
+      y_reading = analogRead(VRyPin);
+      stick_button_state = digitalRead(SWPin);  // Lee el boton (0 si se presiona, 1 si no)
+
+      if (x_reading > X_STANDBY + 3*DEADZONE)
+      {
+        //Serial.println("Moviendo X+");
+        shift_x = 1;
+        shift = true;
+      }
+
+      if (x_reading < X_STANDBY - 2*DEADZONE)
+      {
+        //Serial.println("Moviendo X-");
+        shift_x = -1;
+        shift = true;
+      }
+
+      if (y_reading > Y_STANDBY + 2*DEADZONE)
+      {
+        //Serial.println("Moviendo Y-");
+        shift_y = -1;
+        shift = true;
+      }
+
+      if (y_reading < Y_STANDBY - 3*DEADZONE)
+      {
+        //Serial.println("Moviendo Y+");
+        shift_y = 1;
+        shift = true;
+      }
+
+      if (shift) ARM_shift_by(shift_x, shift_y);
+
+      if (!stick_button_state)
+      {
+        //Serial.println("Stick Presionado");
+        ARM_line_to(STARTING_X, STARTING_Y);
+      }
+  }
+}

rtos/Proyecto_Plotter/joystick_controller.h

@@ -0,0 +1,16 @@
+
+#ifndef _JOYSTICK_CONTROLLER_H_
+#define _JOYSTICK_CONTROLLER_H_
+
+#include <Arduino.h>
+#include "arm_controller.h"
+
+//Pines a los que se conectan las entradas de X, Y, y el boton del Stick respectivamente
+const int VRxPin = 2;  
+const int VRyPin = 4;
+const int SWPin = 26; 
+
+void JOYSTICK_init();
+void JOYSTICK_update();
+
+#endif

rtos/Proyecto_Plotter/scheduler.cpp

@@ -6,17 +6,20 @@ Ticker msTicker;
 volatile bool flag_LED = false;
 volatile bool flag_arm = false;
 volatile bool flag_wifi_server = false;
+volatile bool flag_joystick = false;
 
 // Funcion del TICK
 void onTick() 
 {
   static uint8_t counter = 0;
   counter++;
 
+  flag_arm = true; //cada 10ms
+
   if (counter % 10 == 0) //cada 100ms
   {
-    flag_arm = true;
     flag_wifi_server = true;
+    flag_joystick = true;
   }
 
   if (counter >= 100) //cada 1sec
@@ -45,6 +48,11 @@ void SCHEDULER_dispatch_tasks()
     WEB_SERVER_update();
   }
 
+  if (flag_joystick)
+  {
+    JOYSTICK_update();
+  }
+
   if (flag_LED)    
   {
     //Usado para testear que el timer funciona correctamente

rtos/Proyecto_Plotter/scheduler.h

@@ -4,6 +4,7 @@
 #include <Arduino.h>
 #include <Ticker.h>
 #include "arm_controller.h"
+#include "joystick_controller.h"
 #include "web_server.h"
 
 void SCHEDULER_init();

rtos/Proyecto_Plotter/servo_controller.h

@@ -10,8 +10,8 @@
 #define SERVO_VERTICAL_PIN 23
 
 //Longitud de cada brazo
-#define ARM_LENGTH_A 90 //Brazo conectado al servo alpha
-#define ARM_LENGTH_B 90 //Brazo conectado al servo beta
+#define ARM_LENGTH_A 900 //Brazo conectado al servo alpha
+#define ARM_LENGTH_B 900 //Brazo conectado al servo beta
 
 void SERVO_init(); 
 void SERVO_moveto(int x_coord, int y_coord); //Calcula los angulos alfa y beta para posicionar el extremo en (X,Y)